zjk/AUV_150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mods by mikerb Sat Aug 31 09:35:01 EDT 2024

Browse Source
This commit is contained in:
mikerb
2024-08-31 09:35:01 -04:00
commit cf4cf9217b
27 changed files with 1725 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,25 @@
##############################################################################
# FILE: moos-ivp-extend/src/CMakeLists.txt
# DATE: 2010/09/07
# 2020/05/09 minor mods
# DESCRIPTION: CMakeLists.txt file for the moos-ivp-extend source directory
##############################################################################
#============================================================================
# Add the libraries in the current directory to the include path
#============================================================================
FILE(GLOB LOCAL_LIBRARY_DIRS ./lib_*)
INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS})
#============================================================================
# List the subdirectories to build...
#============================================================================
ADD_SUBDIRECTORY(lib_behaviors-test)
ADD_SUBDIRECTORY(pExampleApp)
ADD_SUBDIRECTORY(pXRelayTest)
##############################################################################
# END of CMakeLists.txt
##############################################################################

154
src/lib_behaviors-test/AOF_SimpleWaypoint.cpp Normal file
View File

@@ -0,0 +1,154 @@
/*****************************************************************/
/* NAME: Michael Benjamin and John Leonard */
/* ORGN: NAVSEA Newport RI and MIT Cambridge MA */
/* FILE: AOF_SimpleWaypoint.cpp */
/* DATE: Feb 22th 2009 */
/*****************************************************************/
#ifdef _WIN32
#pragma warning(disable : 4786)
#pragma warning(disable : 4503)
#endif
#include <math.h>
#include "AOF_SimpleWaypoint.h"
#include "AngleUtils.h"
#include "GeomUtils.h"
using namespace std;
//----------------------------------------------------------
// Procedure: Constructor
AOF_SimpleWaypoint::AOF_SimpleWaypoint(IvPDomain g_domain) : AOF(g_domain)
{
// Unitialized cache values for later use in evalBox calls
m_min_speed = 0;
m_max_speed = 0;
m_angle_to_wpt = 0;
// Initialization parameters
m_osx = 0; // ownship x-position
m_osy = 0; // ownship y-position
m_ptx = 0; // waypoint x-position
m_pty = 0; // waypoint y-position
m_desired_spd = 0;
// Initialization parameter flags
m_osy_set = false;
m_osx_set = false;
m_pty_set = false;
m_ptx_set = false;
m_desired_spd_set = false;
}
//----------------------------------------------------------------
// Procedure: setParam
bool AOF_SimpleWaypoint::setParam(const string& param, double param_val)
{
if(param == "osy") {
m_osy = param_val;
m_osy_set = true;
return(true);
}
else if(param == "osx") {
m_osx = param_val;
m_osx_set = true;
return(true);
}
else if(param == "pty") {
m_pty = param_val;
m_pty_set = true;
return(true);
}
else if(param == "ptx") {
m_ptx = param_val;
m_ptx_set = true;
return(true);
}
else if(param == "desired_speed") {
m_desired_spd = param_val;
m_desired_spd_set = true;
return(true);
}
else
return(false);
}
//----------------------------------------------------------------
// Procedure: initialize
bool AOF_SimpleWaypoint::initialize()
{
// Check for failure conditions
if(!m_osy_set || !m_osx_set || !m_pty_set || !m_ptx_set || !m_desired_spd_set)
return(false);
if(!m_domain.hasDomain("speed") || !m_domain.hasDomain("course"))
return(false);
// Initialize local variables to cache intermediate calculations
m_angle_to_wpt = relAng(m_osx, m_osy, m_ptx, m_pty);;
m_min_speed = m_domain.getVarLow("speed");
m_max_speed = m_domain.getVarHigh("speed");
return(true);
}
//----------------------------------------------------------------
// Procedure: evalPoint
// Purpose: Evaluate a candidate point in the decision space
double AOF_SimpleWaypoint::evalPoint(const vector<double>& point) const
{
// Determine the course and speed being evaluated
double eval_crs = extract("course", point);
double eval_spd = extract("speed", point);
// Calculate the first score, score_roc, based on rate of closure
double angle_diff = angle360(eval_crs - m_angle_to_wpt);
double rad_diff = degToRadians(angle_diff);
double rate_of_closure = cos(rad_diff) * eval_spd;
double roc_range = 2 * m_max_speed;
double roc_diff = (m_desired_spd - rate_of_closure);
if(roc_diff < 0)
roc_diff *= -0.5; // flip the sign, cut the penalty for being over
if(roc_diff > roc_range)
roc_diff = roc_range;
double pct = (roc_diff / roc_range);
double score_roc = (1.0 - pct) * 100;
// Calculate the second score, score_rod, based on rate of detour
double angle_180 = angle180(angle_diff);
if(angle_180 < 0)
angle_180 *= -1;
if(eval_spd < 0)
eval_spd = 0;
double rate_of_detour = (angle_180 * eval_spd);
double rod_range = (m_max_speed * 180);
double rod_pct = (rate_of_detour / rod_range);
double score_rod = (1.0 - rod_pct) * 100;
// Calculate the third score, score_spd, based on ideal speed.
double spd_range = m_max_speed - m_desired_spd;
if((m_desired_spd - m_min_speed) > spd_range)
spd_range = m_desired_spd - m_min_speed;
double spd_diff = m_desired_spd - eval_spd;
if(spd_diff < 0)
spd_diff *= -1;
if(spd_diff > spd_range)
spd_diff = spd_range;
double spd_pct = (spd_diff / spd_range);
double score_spd = (1.0 - spd_pct) * 100;
// CALCULATE THE COMBINED SCORE
double combined_score = 0;
combined_score += (0.75 * score_roc);
combined_score += (0.2 * score_rod);
combined_score += (0.05 * score_spd);
return(combined_score);
}

46
src/lib_behaviors-test/AOF_SimpleWaypoint.h Normal file
View File

@@ -0,0 +1,46 @@
/*****************************************************************/
/* NAME: Michael Benjamin and John Leonard */
/* ORGN: NAVSEA Newport RI and MIT Cambridge MA */
/* FILE: AOF_SimpleWaypoint.h */
/* DATE: Feb 22th 2009 */
/*****************************************************************/
#ifndef AOF_SIMPLE_WAYPOINT_HEADER
#define AOF_SIMPLE_WAYPOINT_HEADER
#include "AOF.h"
#include "IvPDomain.h"
class AOF_SimpleWaypoint: public AOF {
public:
AOF_SimpleWaypoint(IvPDomain);
~AOF_SimpleWaypoint() {};
public: // virtuals defined
double evalPoint(const std::vector<double>&) const;
bool setParam(const std::string&, double);
bool initialize();
protected:
// Initialization parameters
double m_osx; // Ownship x position at time Tm.
double m_osy; // Ownship y position at time Tm.
double m_ptx; // x component of next the waypoint.
double m_pty; // y component of next the waypoint.
double m_desired_spd;
// Initialization parameter set flags
bool m_osx_set;
bool m_osy_set;
bool m_ptx_set;
bool m_pty_set;
bool m_desired_spd_set;
// Cached values for more efficient evalBox calls
double m_angle_to_wpt;
double m_min_speed;
double m_max_speed;
};
#endif

224
src/lib_behaviors-test/BHV_SimpleWaypoint.cpp Normal file
View File

@@ -0,0 +1,224 @@
/*****************************************************************/
/* NAME: M.Benjamin, H.Schmidt, J. Leonard */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: BHV_SimpleWaypoint.cpp */
/* DATE: July 1st 2008 (For purposes of simple illustration) */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#include <cstdlib>
#include <math.h>
#include "BHV_SimpleWaypoint.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include "BuildUtils.h"
#include "ZAIC_PEAK.h"
#include "OF_Coupler.h"
#include "OF_Reflector.h"
#include "AOF_SimpleWaypoint.h"
using namespace std;
//-----------------------------------------------------------
// Procedure: Constructor
BHV_SimpleWaypoint::BHV_SimpleWaypoint(IvPDomain gdomain) :
IvPBehavior(gdomain)
{
IvPBehavior::setParam("name", "simple_waypoint");
m_domain = subDomain(m_domain, "course,speed");
// All distances are in meters, all speed in meters per second
// Default values for configuration parameters
m_desired_speed = 0;
m_arrival_radius = 10;
m_ipf_type = "zaic";
// Default values for behavior state variables
m_osx = 0;
m_osy = 0;
addInfoVars("NAV_X, NAV_Y");
}
//---------------------------------------------------------------
// Procedure: setParam - handle behavior configuration parameters
bool BHV_SimpleWaypoint::setParam(string param, string val)
{
// Convert the parameter to lower case for more general matching
param = tolower(param);
double double_val = atof(val.c_str());
if((param == "ptx") && (isNumber(val))) {
m_nextpt.set_vx(double_val);
return(true);
}
else if((param == "pty") && (isNumber(val))) {
m_nextpt.set_vy(double_val);
return(true);
}
else if((param == "speed") && (double_val > 0) && (isNumber(val))) {
m_desired_speed = double_val;
return(true);
}
else if((param == "radius") && (double_val > 0) && (isNumber(val))) {
m_arrival_radius = double_val;
return(true);
}
else if(param == "ipf_type") {
val = tolower(val);
if((val == "zaic") || (val == "reflector")) {
m_ipf_type = val;
return(true);
}
}
return(false);
}
//-----------------------------------------------------------
// Procedure: onIdleState
void BHV_SimpleWaypoint::onIdleState()
{
postViewPoint(false);
}
//-----------------------------------------------------------
// Procedure: postViewPoint
void BHV_SimpleWaypoint::postViewPoint(bool viewable)
{
m_nextpt.set_label(m_us_name + "'s next waypoint");
string point_spec;
if(viewable)
point_spec = m_nextpt.get_spec("active=true");
else
point_spec = m_nextpt.get_spec("active=false");
postMessage("VIEW_POINT", point_spec);
}
//-----------------------------------------------------------
// Procedure: onRunState
IvPFunction *BHV_SimpleWaypoint::onRunState()
{
// Part 1: Get vehicle position from InfoBuffer and post a
// warning if problem is encountered
bool ok1, ok2;
m_osx = getBufferDoubleVal("NAV_X", ok1);
m_osy = getBufferDoubleVal("NAV_Y", ok2);
if(!ok1 || !ok2) {
postWMessage("No ownship X/Y info in info_buffer.");
return(0);
}
// Part 2: Determine if the vehicle has reached the destination
// point and if so, declare completion.
#ifdef WIN32
double dist = _hypot((m_nextpt.x()-m_osx), (m_nextpt.y()-m_osy));
#else
double dist = hypot((m_nextpt.x()-m_osx), (m_nextpt.y()-m_osy));
#endif
if(dist <= m_arrival_radius) {
setComplete();
postViewPoint(false);
return(0);
}
// Part 3: Post the waypoint as a string for consumption by
// a viewer application.
postViewPoint(true);
// Part 4: Build the IvP function with either the ZAIC tool
// or the Reflector tool.
IvPFunction *ipf = 0;
if(m_ipf_type == "zaic")
ipf = buildFunctionWithZAIC();
else
ipf = buildFunctionWithReflector();
if(ipf == 0)
postWMessage("Problem Creating the IvP Function");
if(ipf)
ipf->setPWT(m_priority_wt);
return(ipf);
}
//-----------------------------------------------------------
// Procedure: buildFunctionWithZAIC
IvPFunction *BHV_SimpleWaypoint::buildFunctionWithZAIC()
{
ZAIC_PEAK spd_zaic(m_domain, "speed");
spd_zaic.setSummit(m_desired_speed);
spd_zaic.setPeakWidth(0.5);
spd_zaic.setBaseWidth(1.0);
spd_zaic.setSummitDelta(0.8);
if(spd_zaic.stateOK() == false) {
string warnings = "Speed ZAIC problems " + spd_zaic.getWarnings();
postWMessage(warnings);
return(0);
}
double rel_ang_to_wpt = relAng(m_osx, m_osy, m_nextpt.x(), m_nextpt.y());
ZAIC_PEAK crs_zaic(m_domain, "course");
crs_zaic.setSummit(rel_ang_to_wpt);
crs_zaic.setPeakWidth(0);
crs_zaic.setBaseWidth(180.0);
crs_zaic.setSummitDelta(0);
crs_zaic.setValueWrap(true);
if(crs_zaic.stateOK() == false) {
string warnings = "Course ZAIC problems " + crs_zaic.getWarnings();
postWMessage(warnings);
return(0);
}
IvPFunction *spd_ipf = spd_zaic.extractIvPFunction();
IvPFunction *crs_ipf = crs_zaic.extractIvPFunction();
OF_Coupler coupler;
IvPFunction *ivp_function = coupler.couple(crs_ipf, spd_ipf, 50, 50);
return(ivp_function);
}
//-----------------------------------------------------------
// Procedure: buildFunctionWithReflector
IvPFunction *BHV_SimpleWaypoint::buildFunctionWithReflector()
{
IvPFunction *ivp_function;
bool ok = true;
AOF_SimpleWaypoint aof_wpt(m_domain);
ok = ok && aof_wpt.setParam("desired_speed", m_desired_speed);
ok = ok && aof_wpt.setParam("osx", m_osx);
ok = ok && aof_wpt.setParam("osy", m_osy);
ok = ok && aof_wpt.setParam("ptx", m_nextpt.x());
ok = ok && aof_wpt.setParam("pty", m_nextpt.y());
ok = ok && aof_wpt.initialize();
if(ok) {
OF_Reflector reflector(&aof_wpt);
reflector.create(600, 500);
ivp_function = reflector.extractIvPFunction();
}
return(ivp_function);
}

75
src/lib_behaviors-test/BHV_SimpleWaypoint.h Normal file
View File

@@ -0,0 +1,75 @@
/*****************************************************************/
/* NAME: M.Benjamin, H.Schmidt, J. Leonard */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: BHV_SimpleWaypoint.ch */
/* DATE: July 1st 2008 (For purposes of simple illustration) */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#ifndef BHV_SIMPLE_WAYPOINT_HEADER
#define BHV_SIMPLE_WAYPOINT_HEADER
#include <string>
#include "IvPBehavior.h"
#include "XYPoint.h"
class BHV_SimpleWaypoint : public IvPBehavior {
public:
BHV_SimpleWaypoint(IvPDomain);
~BHV_SimpleWaypoint() {};
bool setParam(std::string, std::string);
void onIdleState();
IvPFunction* onRunState();
protected:
void postViewPoint(bool viewable=true);
IvPFunction* buildFunctionWithZAIC();
IvPFunction* buildFunctionWithReflector();
protected: // Configuration parameters
double m_arrival_radius;
double m_desired_speed;
XYPoint m_nextpt;
std::string m_ipf_type;
protected: // State variables
double m_osx;
double m_osy;
};
#ifdef WIN32
// Windows needs to explicitly specify functions to export from a dll
#define IVP_EXPORT_FUNCTION __declspec(dllexport)
#else
#define IVP_EXPORT_FUNCTION
#endif
extern "C" {
IVP_EXPORT_FUNCTION IvPBehavior * createBehavior(std::string name, IvPDomain domain)
{return new BHV_SimpleWaypoint(domain);}
}
#endif

47
src/lib_behaviors-test/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
#--------------------------------------------------------
# The CMakeLists.txt for: lib_behaviors-test
# Author(s):
#--------------------------------------------------------
# Set System Specific Libraries
if (${WIN32})
# Windows Libraries
SET(SYSTEM_LIBS
)
else (${WIN32})
# Linux and Apple Libraries
SET(SYSTEM_LIBS
m )
endif (${WIN32})
MACRO(ADD_BHV BHV_NAME)
ADD_LIBRARY(${BHV_NAME} SHARED ${BHV_NAME}.cpp)
TARGET_LINK_LIBRARIES(${BHV_NAME}
helmivp
behaviors
ivpbuild
logic
ivpcore
bhvutil
mbutil
geometry
${SYSTEM_LIBS} )
ENDMACRO(ADD_BHV)
#--------------------------------------------------------
# BHV_SimpleWaypoint
#--------------------------------------------------------
ADD_LIBRARY(BHV_SimpleWaypoint SHARED
BHV_SimpleWaypoint.cpp AOF_SimpleWaypoint.cpp)
TARGET_LINK_LIBRARIES(BHV_SimpleWaypoint
helmivp
behaviors
ivpbuild
logic
ivpcore
bhvutil
mbutil
geometry
${SYSTEM_LIBS} )

12
src/lib_behaviors-test/README Normal file
View File

@@ -0,0 +1,12 @@
To use the dynamic loading of behaviors, you need to set the following
environment variable (in your .bashrc file for bash users, or in your
.cshrc file for tcsh users)
For bash users:
export IVP_BEHAVIOR_DIRS=/home/bob/moos-ivp-extend/lib
For tcsh users:
setenv IVP_BEHAVIOR_DIRS '/home/bob/moos-ivp-extend/lib'

27
src/pExampleApp/CMakeLists.txt Normal file
View File

@@ -0,0 +1,27 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pExampleApp
# Author(s): Mike Benjamin
#--------------------------------------------------------
# Set System Specific Libraries
if (${WIN32})
# Windows Libraries
SET(SYSTEM_LIBS
wsock32 )
else (${WIN32})
# Linux and Apple Libraries
SET(SYSTEM_LIBS
m
pthread )
endif (${WIN32})
SET(SRC
main.cpp
)
ADD_EXECUTABLE(pExampleApp ${SRC})
TARGET_LINK_LIBRARIES(pExampleApp
${MOOS_LIBRARIES}
${SYSTEM_LIBS} )

30
src/pExampleApp/ExampleApp.h Normal file
View File

@@ -0,0 +1,30 @@
// This is the CMOOSApp example from the MOOS website documentation
// Included here for convenience
//
// Feb 10th, 2013
#include "MOOS/libMOOS/App/MOOSApp.h"
class ExampleApp : public CMOOSApp {
bool OnNewMail (MOOSMSG_LIST & Mail)
{
// process it
MOOSMSG_LIST::iterator q;
for(q=Mail.begin(); q!=Mail.end(); q++) {
q->Trace();
}
return(true);
}
bool OnConnectToServer () {
return(Register("X", 0.0));
}
bool Iterate ( ) {
std :: vector<unsigned char> X(100) ;
Notify("X" ,X) ;
return true ;
}
};

25
src/pExampleApp/main.cpp Normal file
View File

@@ -0,0 +1,25 @@
// This is the CMOOSApp example from the MOOS website documentation
// Included here for convenience
//
// Feb 10th, 2013
#include "MOOS/libMOOS/App/MOOSApp.h"
#include "ExampleApp.h"
int main(int argc, char *argv[])
{
//here we do some command line parsing ...
MOOS::CommandLineParser P(argc, argv);
//mission file could be first free parameter
std::string mission_file = P.GetFreeParameter(0, "Mission.moos");
//app name can be the second free parameter
std::string app_name = P.GetFreeParameter(1, "ExampleApp");
ExampleApp App;
App.Run(app_name, mission_file, argc, argv);
return(0);
}

30
src/pXRelayTest/CMakeLists.txt Normal file
View File

@@ -0,0 +1,30 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pXRelayTest
# Author(s): Mike Benjamin
#--------------------------------------------------------
# Set System Specific Libraries
if (${WIN32})
# Windows Libraries
SET(SYSTEM_LIBS
wsock32 )
else (${WIN32})
# Linux and Apple Libraries
SET(SYSTEM_LIBS
m
pthread )
endif (${WIN32})
SET(SRC
Relayer.cpp
Relayer_Info.cpp
main.cpp
)
ADD_EXECUTABLE(pXRelayTest ${SRC})
TARGET_LINK_LIBRARIES(pXRelayTest
${MOOS_LIBRARIES}
mbutil
${SYSTEM_LIBS} )

146
src/pXRelayTest/Relayer.cpp Normal file
View File

@@ -0,0 +1,146 @@
/*****************************************************************/
/* NAME: Michael Benjamin */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: Relayer.cpp */
/* DATE: June 26th, 2008 */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#include <iterator>
#include "Relayer.h"
#include "MBUtils.h"
using namespace std;
//---------------------------------------------------------
// Constructor
Relayer::Relayer()
{
m_tally_recd = 0;
m_tally_sent = 0;
m_iterations = 0;
m_start_time_postings = 0;
m_start_time_iterations = 0;
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool Relayer::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
if(key == m_incoming_var)
m_tally_recd++;
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool Relayer::OnConnectToServer()
{
RegisterVariables();
return(true);
}
//------------------------------------------------------------
// Procedure: RegisterVariables
void Relayer::RegisterVariables()
{
if(m_incoming_var != "")
Register(m_incoming_var, 0);
}
//---------------------------------------------------------
// Procedure: Iterate()
bool Relayer::Iterate()
{
m_iterations++;
unsigned int i, amt = (m_tally_recd - m_tally_sent);
for(i=0; i<amt; i++) {
m_tally_sent++;
Notify(m_outgoing_var, m_tally_sent);
}
// If this is the first iteration just note the start time, otherwise
// note the currently calculated frequency and post it to the DB.
if(m_start_time_iterations == 0)
m_start_time_iterations = MOOSTime();
else {
double delta_time = (MOOSTime() - m_start_time_iterations) + 0.01;
double frequency = (double)(m_iterations) / delta_time;
Notify(m_outgoing_var+"_ITER_HZ", frequency);
}
// If this is the first time a received msg has been noted, just
// note the start time, otherwise calculate and post the frequency.
if(amt > 0) {
if(m_start_time_postings == 0)
m_start_time_postings = MOOSTime();
else {
double delta_time = (MOOSTime() - m_start_time_postings) + 0.01;
double frequency = (double)(m_tally_sent) / delta_time;
Notify(m_outgoing_var+"_POST_HZ", frequency);
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnStartUp()
// Note: happens before connection is open
bool Relayer::OnStartUp()
{
STRING_LIST sParams;
m_MissionReader.GetConfiguration(GetAppName(), sParams);
STRING_LIST::iterator p;
for(p = sParams.begin();p!=sParams.end();p++) {
string line = *p;
string param = tolower(biteStringX(line, '='));
string value = line;
if(param == "incoming_var")
m_incoming_var = value;
else if(param == "outgoing_var")
m_outgoing_var = value;
}
RegisterVariables();
return(true);
}

55
src/pXRelayTest/Relayer.h Normal file
View File

@@ -0,0 +1,55 @@
/****************************************************************/
/* NAME: Michael Benjamin */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: Relayer.h */
/* DATE: Jun 26th 2008 */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/****************************************************************/
#ifndef P_RELAY_VAR_HEADER
#define P_RELAY_VAR_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
class Relayer : public CMOOSApp
{
public:
Relayer();
virtual ~Relayer() {};
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
void setIncomingVar(std::string s) {m_incoming_var=s;};
void setOutgoingVar(std::string s) {m_outgoing_var=s;};
protected:
unsigned long int m_tally_recd;
unsigned long int m_tally_sent;
unsigned long int m_iterations;
std::string m_incoming_var;
std::string m_outgoing_var;
double m_start_time_postings;
double m_start_time_iterations;
};
#endif

125
src/pXRelayTest/Relayer_Info.cpp Normal file
View File

@@ -0,0 +1,125 @@
/*****************************************************************/
/* NAME: Michael Benjamin */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: Relayer_Info.cpp */
/* DATE: Jan 12th, 2012 */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#include <cstdlib>
#include <iostream>
#include "Relayer_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The purpose of the pXRelay application is to provide a simple ");
blk(" example of the MOOS publish-subscribe architecture. It is ");
blk(" typically run in conjunction with another instance of the same");
blk(" process to send mail back and forth to each other. ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pXRelayTest file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pXRelayTest with the given process name rather ");
blk(" than pXRelayTest. ");
mag(" --example, -e ");
blk(" Display example MOOS configuration block. ");
mag(" --help, -h ");
blk(" Display this help message. ");
mag(" --in","=<varname> ");
blk(" Use <varname> as the Relay incoming variable ");
mag(" --interface, -i ");
blk(" Display MOOS publications and subscriptions. ");
mag(" --out","=<varname> ");
blk(" Use <varname> as the Relay outgoing variable ");
blk(" ");
blk("Note: If argv[2] does not otherwise match a known option, ");
blk(" then it will be interpreted as a run alias. This is ");
blk(" to support pAntler launching conventions. ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showExampleConfigAndExit
void showExampleConfigAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pXRelay Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pXRelay ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk(" OUTGOING_VAR = APPLES ");
blk(" INCOMING_VAR = PEARS ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pXRelay INTERFACE ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("SUBSCRIPTIONS: ");
blk("------------------------------------ ");
blk(" Whatever variable is specified by the INCOMING_VAR ");
blk(" configuration parameter. ");
blk(" ");
blk("PUBLICATIONS: ");
blk("------------------------------------ ");
blk(" Whatever variable is specified by the OUTGOING_VAR ");
blk(" configuration parameter. ");
blk(" ");
exit(0);
}

31
src/pXRelayTest/Relayer_Info.h Normal file
View File

@@ -0,0 +1,31 @@
/*****************************************************************/
/* NAME: Michael Benjamin, Henrik Schmidt, and John Leonard */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: Relayer_Info.h */
/* DATE: Jan 12th 2012 */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#ifndef P_XRELAYER_TEST_INFO_HEADER
#define P_XRELAYER_TEST_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
#endif

71
src/pXRelayTest/main.cpp Normal file
View File

@@ -0,0 +1,71 @@
/*****************************************************************/
/* NAME: Michael Benjamin */
/* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */
/* FILE: main.cpp */
/* DATE: Jun 26th 2008 */
/* */
/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version */
/* 2 of the License, or (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be */
/* useful, but WITHOUT ANY WARRANTY; without even the implied */
/* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */
/* PURPOSE. See the GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public */
/* License along with this program; if not, write to the Free */
/* Software Foundation, Inc., 59 Temple Place - Suite 330, */
/* Boston, MA 02111-1307, USA. */
/*****************************************************************/
#include <string>
#include <list>
#include "Relayer.h"
#include "Relayer_Info.h"
#include "MBUtils.h"
using namespace std;
int main(int argc, char *argv[])
{
string mission_file;
string run_command = argv[0];
string incoming_var;
string outgoing_var;
for(int i=1; i<argc; i++) {
string argi = argv[i];
if((argi=="-e") || (argi=="--example") || (argi=="-example"))
showExampleConfigAndExit();
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
showHelpAndExit();
else if((argi == "-i") || (argi == "--interface"))
showInterfaceAndExit();
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
mission_file = argv[i];
else if(strBegins(argi, "--alias="))
run_command = argi.substr(8);
else if(strBegins(argi, "--in="))
incoming_var = argi.substr(5);
else if(strBegins(argi, "--out="))
outgoing_var = argi.substr(6);
else if(i==2)
run_command = argi;
}
if(mission_file == "")
showHelpAndExit();
Relayer relayer;
if(incoming_var != "")
relayer.setIncomingVar(incoming_var);
if(outgoing_var != "")
relayer.setOutgoingVar(outgoing_var);
relayer.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

14
src/pXRelayTest/pXRelayTest.moos Normal file
View File

@@ -0,0 +1,14 @@
// MOOS file
ServerHost = localhost
ServerPort = 9000
ProcessConfig = pXRelayTest
{
AppTick = 4
CommsTick = 4
OUTGOING_VAR = APPLES
INCOMING_VAR = PEARS
}