diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f77fb98 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,102 @@ +#=========================================================================== +# FILE: moos-ivp-extend/CMakeLists.txt +# DATE: 2012/07/24 +# INFO: Top-level CMakeLists.txt file for the moos-ivp-extend project +# NAME: Maintained by Mike Benjamin - Original setup by Christian Convey +# Chris Gagner, and tips borrowed from Dave Billin +#=========================================================================== + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) +PROJECT( IVP_EXTEND ) + +#============================================================================= +# Set the output directories for the binary and library files +#============================================================================= + +GET_FILENAME_COMPONENT(IVP_EXTEND_BIN_DIR "${CMAKE_SOURCE_DIR}/bin" ABSOLUTE ) +GET_FILENAME_COMPONENT(IVP_EXTEND_LIB_DIR "${CMAKE_SOURCE_DIR}/lib" ABSOLUTE ) + +SET( LIBRARY_OUTPUT_PATH "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" ) +SET( ARCHIVE_OUTPUT_DIRECTORY "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" ) +SET( LIBRARY_OUTPUT_DIRECTORY "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" ) + +SET( EXECUTABLE_OUTPUT_PATH "${IVP_EXTEND_BIN_DIR}" CACHE PATH "" ) +SET( RUNTIME_OUTPUT_DIRECTORY "${IVP_EXTEND_BIN_DIR}" CACHE PATH "" ) + +#============================================================================= +# Find MOOS +#============================================================================= +find_package(MOOS 10.0) +INCLUDE_DIRECTORIES(${MOOS_INCLUDE_DIRS}) + +#============================================================================= +# Find the "moos-ivp" base directory +#============================================================================= + +# Search for the moos-ivp folder +find_path( MOOSIVP_SOURCE_TREE_BASE + NAMES build-ivp.sh build-moos.sh configure-ivp.sh + PATHS "../moos-ivp" "../../moos-ivp" "../../moos-ivp/trunk/" "../moos-ivp/trunk/" + DOC "Base directory of the MOOS-IvP source tree" + NO_DEFAULT_PATH +) + +if (NOT MOOSIVP_SOURCE_TREE_BASE) + message("Please set MOOSIVP_SOURCE_TREE_BASE to ") + message("the location of the \"moos-ivp\" folder ") + return() +endif() + +#============================================================================= +# Specify where to find IvP's headers and libraries... +#============================================================================= + +FILE(GLOB IVP_INCLUDE_DIRS ${MOOSIVP_SOURCE_TREE_BASE}/ivp/src/lib_* ) +INCLUDE_DIRECTORIES(${IVP_INCLUDE_DIRS}) + +FILE(GLOB IVP_LIBRARY_DIRS ${MOOSIVP_SOURCE_TREE_BASE}/lib ) +LINK_DIRECTORIES(${IVP_LIBRARY_DIRS}) + +#============================================================================= +# Specify Compiler Flags +#============================================================================= +IF( ${WIN32} ) + #--------------------------------------------- + # Windows Compiler Flags + #--------------------------------------------- + IF(MSVC) + # Flags for Microsoft Visual Studio + SET( WALL_ON OFF CACHE BOOL + "tell me about all compiler warnings (-Wall) ") + IF(WALL_ON) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") + ENDIF(WALL_ON) + ELSE(MSVC) + # Other Windows compilers go here + ENDIF(MSVC) + +ELSE( ${WIN32} ) + #--------------------------------------------- + # Linux and Apple Compiler Flags + #--------------------------------------------- + # Force -fPIC because gcc complains when we don't use it with x86_64 code. + # Note sure why: -fPIC should only be needed for shared objects, and + # AFAIK, CMake gets that right when building shared objects. -CJC + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -g") + IF(CMAKE_COMPILER_IS_GNUCXX) + # Flags for the GNU C++ Compiler + SET( WALL_ON OFF CACHE BOOL + "tell me about all compiler warnings (-Wall) ") + IF(WALL_ON) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall" -C++11) + ENDIF( WALL_ON) + ELSE(CMAKE_COMPILER_IS_GNUCXX) + + ENDIF(CMAKE_COMPILER_IS_GNUCXX) + +ENDIF( ${WIN32} ) + +#============================================================================= +# Add Subdirectories +#============================================================================= +ADD_SUBDIRECTORY( src ) diff --git a/README b/README new file mode 100644 index 0000000..1220cf3 --- /dev/null +++ b/README @@ -0,0 +1,84 @@ +############################################################################## +# FILE: moos-ivp-extend/README +# DATE: 2014/01/02 +# DESCRIPTION: Contains important information regarding the moos-ivp-extend +# repository. +############################################################################## + +#============================================================================= +# Introduction +#============================================================================= +The moos-ivp-extend repository contains examples for extending the MOOS-IvP +Autonomy system. This includes a MOOS application and an IvP behavior. + + +#============================================================================= +# Directory Structure +#============================================================================= +The directory structure for the moos-ivp-extend is decribed below: + +bin - Directory for generated executable files +build - Directory for build object files +build.sh - Script for building moos-ivp-extend +CMakeLists.txt - CMake configuration file for the project +data - Directory for storing data +lib - Directory for generated library files +missions - Directory for mission files +README - Contains helpful information - (this file). +scripts - Directory for script files +src - Directory for source code + + +#============================================================================= +# Build Instructions +#============================================================================= +#-------------------- +# Linux and Mac Users +#-------------------- + +To build on Linux and Apple platforms, execute the build script within this +directory: + + $ ./build.sh + +To build without using the supplied script, execute the following commands +within this directory: + + $ mkdir -p build + $ cd build + $ cmake ../ + $ make + $ cd .. + + +#-------------- +# Windows Users +#-------------- +To build on Windows platform, open CMake using your favorite shortcut. Then +set the source directory to be this directory and set the build directory +to the "build" directory inside this directory. + +The source directory is typically next to the question: + "Where is the source code?" + +The build directory is typically next to the question: + "Where to build the binaries?" + +Alternatively, CMake can be invoked via the command line. However, you must +specify your gernerator. Use "cmake --help" for a list of generators and +additional help. + +#============================================================================= +# Environment variables +#============================================================================= +The moos-ivp-extend binaries files should be added to your path to allow them +to be launched from pAntler. + +In order for generated IvP Behaviors to be recognized by the IvP Helm, you +should add the library directory to the "IVP_BEHAVIOR_DIRS" environment +variable. + +############################################################################## +# END of README +############################################################################## + diff --git a/bin/MyGenMOOSApp b/bin/MyGenMOOSApp new file mode 100755 index 0000000..f75b65b --- /dev/null +++ b/bin/MyGenMOOSApp @@ -0,0 +1,223 @@ +#!/bin/bash + +if [ -z "$1" ] ; then + echo "GenMOOSApp: usage: $0 [app-name] [prefix]" + exit 0 +fi + +if [ -z "$2" ] ; then + echo "GenMOOSApp: usage: $0 [app-name] [prefix]" + exit 0 +fi + +mkdir $2$1 +cd $2$1 + +cat >> CMakeLists.txt <> ${1}.h <> ${1}Main.cpp < +#include "${1}.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + // default parameters file + string sMissionFile = "${1}.moos"; + + //under what name shoud the application register with the MOOSDB? + string sMOOSName = "${2}${1}"; + + switch(argc) + { + case 3: + //command line says don't register with default name + sMOOSName = argv[2]; + case 2: + //command line says don't use default config file + sMissionFile = argv[1]; + } + + //make an application + ${1} ${1}App; + + //run it + ${1}App.Run(sMOOSName.c_str(), sMissionFile.c_str()); + + return(0); +} + +EOF + +cat >> $2${1}.moos <> ${1}.cpp < +#include +#include "MBUtils.h" +#include "${1}.h" + +using namespace std; + +//--------------------------------------------------------- +// Constructor + +${1}::${1}() +{ +} + +//--------------------------------------------------------- +// Destructor + +${1}::~${1}() +{ +} + +//--------------------------------------------------------- +// Procedure: OnNewMail + +bool ${1}::OnNewMail(MOOSMSG_LIST &NewMail) +{ + MOOSMSG_LIST::iterator p; + + for(p=NewMail.begin(); p!=NewMail.end(); p++) { + CMOOSMsg &msg = *p; + } + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnConnectToServer + +bool ${1}::OnConnectToServer() +{ + RegisterVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: Iterate() + +bool ${1}::Iterate() +{ + // happens AppTick times per second + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnStartUp() +// happens before connection is open + +bool ${1}::OnStartUp() +{ + list sParams; + if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + list::iterator p; + for(p=sParams.begin(); p!=sParams.end(); p++) { + string original_line = *p; + string line = *p; + string param = stripBlankEnds(toupper(biteString(line, '='))); + string value = stripBlankEnds(line); + + if(param == "FOO") { + //handled + } + else if(param == "BAR") { + //handled + } + } + } + + RegisterVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: RegisterVariables + +void ${1}::RegisterVariables() +{ + // Register("FOOBAR", 0); +} + +EOF + +echo "$2${1} generated" diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..e96a58a --- /dev/null +++ b/build.sh @@ -0,0 +1,46 @@ +#!/bin/bash + +INVOCATION_ABS_DIR=`pwd` +BUILD_TYPE="None" +CMD_LINE_ARGS="" + +#------------------------------------------------------------------- +# Part 1: Check for and handle command-line arguments +#------------------------------------------------------------------- +for ARGI; do + if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then + printf "%s [SWITCHES] \n" $0 + printf "Switches: \n" + printf " --help, -h \n" + printf " --debug, -d \n" + printf " --release, -r \n" + printf "Notes: \n" + printf " (1) All other command line args will be passed as args \n" + printf " to \"make\" when it is eventually invoked. \n" + printf " (2) For example -k will continue making when/if a failure \n" + printf " is encountered in building one of the subdirectories. \n" + printf " (3) For example -j2 will utilize a 2nd core in the build \n" + printf " if your machine has two cores. -j4 etc for quad core. \n" + exit 0; + elif [ "${ARGI}" = "--debug" -o "${ARGI}" = "-d" ] ; then + BUILD_TYPE="Debug" + elif [ "${ARGI}" = "--release" -o "${ARGI}" = "-r" ] ; then + BUILD_TYPE="Release" + else + CMD_LINE_ARGS=$CMD_LINE_ARGS" "$ARGI + fi +done + +#------------------------------------------------------------------- +# Part 2: Invoke the call to make in the build directory +#------------------------------------------------------------------- + +mkdir -p build +cd build + +cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} ../ + +make ${CMD_LINE_ARGS} +cd ${INVOCATION_ABS_DIR} + + diff --git a/clean.sh b/clean.sh new file mode 100755 index 0000000..7550cd3 --- /dev/null +++ b/clean.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +rm -rf build/* +rm -rf lib/* +rm -rf bin/p* + +find . -name '.DS_Store' -print -exec rm -rfv {} \; +find . -name '*~' -print -exec rm -rfv {} \; +find . -name '#*' -print -exec rm -rfv {} \; +find . -name '*.moos++' -print -exec rm -rfv {} \; + +#find . -name 'MOOSLog*' -print -exec rm -rfv {} \; + diff --git a/missions/alder/README b/missions/alder/README new file mode 100644 index 0000000..3a49d99 --- /dev/null +++ b/missions/alder/README @@ -0,0 +1,11 @@ +To Run this mission, make sure that the following two bin +directories are in your path: + +moos-ivp/ivp/bin/ +moos-ivp-extend/bin/ + +Then launch the mission by: + +pAntler alder.moos + + diff --git a/missions/alder/alder.bhv b/missions/alder/alder.bhv new file mode 100644 index 0000000..9416a17 --- /dev/null +++ b/missions/alder/alder.bhv @@ -0,0 +1,42 @@ +//-------- FILE: alder.bhv ------------- + +initialize DEPLOY = false +initialize RETURN = false + +//---------------------------------------------- +Behavior = BHV_SimpleWaypoint +{ + name = waypt_to_point + pwt = 100 + condition = RETURN = false + condition = DEPLOY = true + condition = (ODOMETRY_DIST < 50) + endflag = RETURN = true + + speed = 2.0 // meters per second + radius = 8.0 + ptx = 100 + pty = -50 +} + +//---------------------------------------------- +Behavior = BHV_SimpleWaypoint +{ + name = waypt_return + pwt = 100 + condition = (RETURN = true) or (ODOMETRY_DIST >= 50) + condition = (DEPLOY = true) + + speed = 2.0 + radius = 8.0 + ptx = 0 + pty = 0 +} + +//---------------------------------------------- +Behavior = BHV_HSLine +{ + name = hsline + time_on_leg = 20 +} + diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos new file mode 100644 index 0000000..c621a71 --- /dev/null +++ b/missions/alder/alder.moos @@ -0,0 +1,175 @@ +// Level 2 Configuration file: M. Benjamin + +ServerHost = localhost +ServerPort = 9000 +Simulator = true + +Community = alder + +MOOSTimeWarp = 1 + +LatOrigin = 43.825300 +LongOrigin = -70.330400 + +//------------------------------------------ +// Antler configuration block +ProcessConfig = ANTLER +{ + MSBetweenLaunches = 200 + + Run = MOOSDB @ NewConsole = false + Run = uSimMarine @ NewConsole = false + Run = pNodeReporter @ NewConsole = false + Run = pMarinePID @ NewConsole = false + Run = pMarineViewer @ NewConsole = false + Run = uProcessWatch @ NewConsole = false + Run = pHelmIvP @ NewConsole = false + Run = pOdometry @ NewConsole = false + RUn = uXMS @ NewConsole = false +} + +//------------------------------------------ +// uSimMarine config block + +ProcessConfig = uSimMarine +{ + AppTick = 10 + CommsTick = 10 + + START_X = 0 + START_Y = 0 + START_SPEED = 0 + START_HEADING = 180 + + PREFIX = NAV +} + + +//------------------------------------------ +// uProcessWatch config block + +ProcessConfig = uProcessWatch +{ + AppTick = 4 + CommsTick = 4 + + term_report_interval = 0.8 + + summary_wait = 5 + + nowatch = uXMS* + nowatch = uPokeDB* + nowatch = uTermCommand* + watch_all = true +} + + +//------------------------------------------ +// pHelmIvP config block + +ProcessConfig = pHelmIvP +{ + AppTick = 4 + CommsTick = 4 + + Behaviors = alder.bhv + Verbose = quiet + Domain = course:0:359:360 + Domain = speed:0:4:21 + + IVP_BEHAVIOR_DIR = ../../lib + + ok_skew = any + active_start = false +} + +//------------------------------------------ +// pMarinePID config block + +ProcessConfig = pMarinePID +{ + AppTick = 20 + CommsTick = 20 + + VERBOSE = true + DEPTH_CONTROL = false + + // Yaw PID controller + YAW_PID_KP = 0.5 + YAW_PID_KD = 0.0 + YAW_PID_KI = 0.0 + YAW_PID_INTEGRAL_LIMIT = 0.07 + + // Speed PID controller + SPEED_PID_KP = 1.0 + SPEED_PID_KD = 0.0 + SPEED_PID_KI = 0.0 + SPEED_PID_INTEGRAL_LIMIT = 0.07 + + //MAXIMUMS + MAXRUDDER = 100 + MAXTHRUST = 100 + + // A non-zero SPEED_FACTOR overrides use of SPEED_PID + // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR + SPEED_FACTOR = 20 +} + +//------------------------------------------ +// pMarineViewer config block + +ProcessConfig = pMarineViewer +{ + AppTick = 4 + CommsTick = 4 + + TIFF_FILE = forrest19.tif + set_pan_x = -90 + set_pan_y = -280 + zoom = 0.65 + vehicle_shape_scale = 1.5 + hash_delta = 50 + hash_shade = 0.4 + hash_viewable = true + + scope = ODOMETRY_DIST + + // Appcast configuration + appcast_height = 75 + appcast_width = 30 + appcast_viewable = true + appcast_color_scheme = indigo + nodes_font_size = medium + procs_font_size = medium + appcast_font_size = small + + BUTTON_ONE = DEPLOY # DEPLOY=true + BUTTON_ONE = MOOS_MANUAL_OVERIDE=false # RETURN=false + BUTTON_TWO = RETURN # RETURN=true +} + +//------------------------------------------ +// pNodeReporter config block + +ProcessConfig = pNodeReporter +{ + AppTick = 2 + CommsTick = 2 + VESSEL_TYPE = KAYAK +} + +//----------------- +// pOdometry config block +{ + AppTick = 4 + CommsTick = 4 + +} + +//----------------- +// uXMS config block +{ + AppTick = 4 + CommsTick = 4 + //VAR = PEARS, +} diff --git a/missions/xrelay/xrelay.moos b/missions/xrelay/xrelay.moos new file mode 100644 index 0000000..dd82ab7 --- /dev/null +++ b/missions/xrelay/xrelay.moos @@ -0,0 +1,64 @@ +// MOOS file + +ServerHost = localhost +ServerPort = 9000 + +//------------------------------------------ +// Antler configuration block +ProcessConfig = ANTLER +{ + MSBetweenLaunches = 200 + + Run = MOOSDB @ NewConsole = false + Run = pXRelayTest @ NewConsole = true ~ pXRelay_APPLES + Run = pXRelayTest @ NewConsole = true ~ pXRelay_PEARS + Run = uXMS @ NewConsole = true + Run - uTimerScript @ NewConsole = false +} + + +//------------------------------------------ +// First pXRelay configuration block + +ProcessConfig = pXRelay_APPLES +{ + AppTick = 4 + CommsTick = 4 + + OUTGOING_VAR = APPLES + INCOMING_VAR = PEARS +} + +//------------------------------------------ +// Second pXRelay configuration block + +ProcessConfig = pXRelay_PEARS +{ + AppTick = 4 + CommsTick = 4 + + OUTGOING_VAR = PEARS + INCOMING_VAR = APPLES +} + +//------------------------------------------ +// uXMS configuration block + +ProcessConfig = uXMS +{ + AppTick = 4 + CommsTick = 4 + + VAR = PEARS, PEARS_ITER_HZ, PEARS_POST_HZ + VAR = APPLES, APPLES_ITER_HZ, APPLES_POST_HZ +} + +ProcessConfig = uTimerStart +{ + AppTick = 4 + Commstick = 4 + + event = var=PEARS, val=1, time=5 + +} + diff --git a/scripts/README b/scripts/README new file mode 100644 index 0000000..a4a75ee --- /dev/null +++ b/scripts/README @@ -0,0 +1,5 @@ +This directory may contain developer utility scripts. + +The MyGenMOOSApp script is similar to that found in the moos-ivp/trunk/scripts/ +directory. Users are encouraged to tailor this one to their own needs. + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..f1cba06 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,27 @@ +############################################################################## +# FILE: moos-ivp-extend/src/CMakeLists.txt +# DATE: 2010/09/07 +# 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(pXRelayTest) +ADD_SUBDIRECTORY(pExampleApp) +#ADD_SUBDIRECTORY(pPrimeFactor) +#ADD_SUBDIRECTORY(pFoobar) +ADD_SUBDIRECTORY(pOdometry) + +############################################################################## +# END of CMakeLists.txt +############################################################################## + diff --git a/src/lib_behaviors-test/AOF_SimpleWaypoint.cpp b/src/lib_behaviors-test/AOF_SimpleWaypoint.cpp new file mode 100644 index 0000000..ebc8a1a --- /dev/null +++ b/src/lib_behaviors-test/AOF_SimpleWaypoint.cpp @@ -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 +#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& 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); +} + diff --git a/src/lib_behaviors-test/AOF_SimpleWaypoint.h b/src/lib_behaviors-test/AOF_SimpleWaypoint.h new file mode 100644 index 0000000..bae71d8 --- /dev/null +++ b/src/lib_behaviors-test/AOF_SimpleWaypoint.h @@ -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&) 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 + diff --git a/src/lib_behaviors-test/BHV_SimpleWaypoint.cpp b/src/lib_behaviors-test/BHV_SimpleWaypoint.cpp new file mode 100644 index 0000000..b092ad2 --- /dev/null +++ b/src/lib_behaviors-test/BHV_SimpleWaypoint.cpp @@ -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 +#include +#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); +} diff --git a/src/lib_behaviors-test/BHV_SimpleWaypoint.h b/src/lib_behaviors-test/BHV_SimpleWaypoint.h new file mode 100644 index 0000000..7c5f319 --- /dev/null +++ b/src/lib_behaviors-test/BHV_SimpleWaypoint.h @@ -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 +#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 + + + + + + + + + diff --git a/src/lib_behaviors-test/CMakeLists.txt b/src/lib_behaviors-test/CMakeLists.txt new file mode 100644 index 0000000..946a742 --- /dev/null +++ b/src/lib_behaviors-test/CMakeLists.txt @@ -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} ) diff --git a/src/lib_behaviors-test/README b/src/lib_behaviors-test/README new file mode 100644 index 0000000..4494d4f --- /dev/null +++ b/src/lib_behaviors-test/README @@ -0,0 +1,8 @@ + +To use the dynamic loading of behaviors, you need to set the following +environment variable (in your .cshrc file for tcsh users, or the equivalent +for bash users): + +setenv IVP_BEHAVIOR_DIRS '/home/bob/moos-ivp-extend/lib' + + diff --git a/src/pExampleApp/CMakeLists.txt b/src/pExampleApp/CMakeLists.txt new file mode 100644 index 0000000..a78f470 --- /dev/null +++ b/src/pExampleApp/CMakeLists.txt @@ -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} ) diff --git a/src/pExampleApp/ExampleApp.h b/src/pExampleApp/ExampleApp.h new file mode 100644 index 0000000..c58a006 --- /dev/null +++ b/src/pExampleApp/ExampleApp.h @@ -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 X(100) ; + Notify("X" ,X) ; + return true ; + } +}; + diff --git a/src/pExampleApp/main.cpp b/src/pExampleApp/main.cpp new file mode 100644 index 0000000..e3339d7 --- /dev/null +++ b/src/pExampleApp/main.cpp @@ -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); +} + diff --git a/src/pOdometry/CMakeLists.txt b/src/pOdometry/CMakeLists.txt new file mode 100644 index 0000000..f31f9b9 --- /dev/null +++ b/src/pOdometry/CMakeLists.txt @@ -0,0 +1,19 @@ +#-------------------------------------------------------- +# The CMakeLists.txt for: pOdometry +# Author(s): Clement Li +#-------------------------------------------------------- + +SET(SRC + Odometry.cpp + Odometry_Info.cpp + main.cpp +) + +ADD_EXECUTABLE(pOdometry ${SRC}) + +TARGET_LINK_LIBRARIES(pOdometry + ${MOOS_LIBRARIES} + mbutil + m + pthread) + diff --git a/src/pOdometry/Odometry.cpp b/src/pOdometry/Odometry.cpp new file mode 100644 index 0000000..74b9e08 --- /dev/null +++ b/src/pOdometry/Odometry.cpp @@ -0,0 +1,178 @@ +/************************************************************/ +/* NAME: Clement Li */ +/* ORGN: MIT */ +/* FILE: Odometry.cpp */ +/* DATE: 2-14-19 +/* */ +/* Calculates total distance travelled (ODOMETRY_DIST) */ +/* Registers for: NAV_X, NAV_Y */ +/* Publishes: ODOMETRY_DIST */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "Odometry.h" + +using namespace std; + +//--------------------------------------------------------- +// Constructor + +Odometry::Odometry() +{ + m_first_reading = true; + m_current_x = 0; + m_current_y = 0; + m_previous_x = 0; + m_previous_y = 0; + m_total_distance = 0; +} + +//--------------------------------------------------------- +// Destructor + +Odometry::~Odometry() +{ +} + +//--------------------------------------------------------- +// Procedure: OnNewMail + +bool Odometry::OnNewMail(MOOSMSG_LIST &NewMail) +{ + AppCastingMOOSApp::OnNewMail(NewMail); + + MOOSMSG_LIST::iterator p; + + for(p=NewMail.begin(); p!=NewMail.end(); p++) { + CMOOSMsg &msg = *p; + +#if 0 // Keep these around just for template + string key = msg.GetKey(); + string comm = msg.GetCommunity(); + double dval = msg.GetDouble(); + string sval = msg.GetString(); + string msrc = msg.GetSource(); + double mtime = msg.GetTime(); + bool mdbl = msg.IsDouble(); + bool mstr = msg.IsString(); +#endif + + string key = msg.GetKey(); + + //update NAV_X + if (key == "NAV_X") { + m_current_x = msg.GetDouble(); + + } + + //update NAV_Y + if (key == "NAV_Y") { + m_current_y = msg.GetDouble(); + } + + } + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnConnectToServer + +bool Odometry::OnConnectToServer() +{ + // register for variables here + // possibly look at the mission file? + // m_MissionReader.GetConfigurationParam("Name", ); + // m_Comms.Register("VARNAME", 0); + + RegisterVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: Iterate() +// happens AppTick times per second + +bool Odometry::Iterate() +{ + AppCastingMOOSApp::Iterate(); + + double subdist; //distance travelled since last calculation + + //if first measurement, set odometer to zero, calibrate location + if (m_first_reading) { + m_total_distance = 0; + m_first_reading = false; + + } + else { + + //calculate distance travelled since last point (calculated) + subdist = sqrt((m_current_x - m_previous_x)*(m_current_x - m_previous_x) + + (m_current_y - m_previous_y)*(m_current_y - m_previous_y)); + m_total_distance = m_total_distance + subdist; + } + + //update old position to current position + m_previous_x = m_current_x; + m_previous_y = m_current_y; + + //post total distance to ODOMETRY_DIST + Notify("ODOMETRY_DIST",m_total_distance); + + AppCastingMOOSApp::PostReport(); + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnStartUp() +// happens before connection is open + +bool Odometry::OnStartUp() +{ + AppCastingMOOSApp::OnStartUp(); + list sParams; + m_MissionReader.EnableVerbatimQuoting(false); + if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + list::iterator p; + for(p=sParams.begin(); p!=sParams.end(); p++) { + string line = *p; + string param = tolower(biteStringX(line, '=')); + string value = line; + + if(param == "foo") { + //handled + } + else if(param == "bar") { + //handled + } + } + } + + RegisterVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: RegisterVariables + +void Odometry::RegisterVariables() +{ + + AppCastingMOOSApp::RegisterVariables(); + // Register("FOOBAR", 0); + Register("NAV_X",0); + Register("NAV_Y",0); +} + +//------------- +// Build Report for AppCasting +bool Odometry::buildReport() +{ + //report total distance + m_msgs << "Total distance travelled: " << m_total_distance << endl; + return(true); +} + diff --git a/src/pOdometry/Odometry.h b/src/pOdometry/Odometry.h new file mode 100644 index 0000000..8df61f4 --- /dev/null +++ b/src/pOdometry/Odometry.h @@ -0,0 +1,42 @@ +/************************************************************/ +/* NAME: Clement Li */ +/* ORGN: MIT */ +/* FILE: Odometry.h */ +/* DATE: */ +/************************************************************/ + +#ifndef Odometry_HEADER +#define Odometry_HEADER + +#include "MOOS/libMOOS/MOOSLib.h" +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" + +class Odometry : public AppCastingMOOSApp +{ + public: + Odometry(); + ~Odometry(); + + protected: // Standard MOOSApp functions to overload + bool OnNewMail(MOOSMSG_LIST &NewMail); + bool Iterate(); + bool OnConnectToServer(); + bool OnStartUp(); + bool buildReport(); + + protected: + void RegisterVariables(); + + + private: // Configuration variables + + private: // State variables + bool m_first_reading; + double m_current_x; + double m_current_y; + double m_previous_x; + double m_previous_y; + double m_total_distance; +}; + +#endif diff --git a/src/pOdometry/Odometry_Info.cpp b/src/pOdometry/Odometry_Info.cpp new file mode 100644 index 0000000..729a9fd --- /dev/null +++ b/src/pOdometry/Odometry_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: Clement Li */ +/* ORGN: MIT Cambridge MA */ +/* FILE: Odometry_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#include "Odometry_Info.h" +#include "ColorParse.h" +#include "ReleaseInfo.h" + +using namespace std; + +//---------------------------------------------------------------- +// Procedure: showSynopsis + +void showSynopsis() +{ + blk("SYNOPSIS: "); + blk("------------------------------------ "); + blk(" The pOdometry application is used for "); + blk(" "); + blk(" "); + blk(" "); + blk(" "); +} + +//---------------------------------------------------------------- +// Procedure: showHelpAndExit + +void showHelpAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("Usage: pOdometry file.moos [OPTIONS] "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("Options: "); + mag(" --alias","= "); + blk(" Launch pOdometry with the given process name "); + blk(" rather than pOdometry. "); + mag(" --example, -e "); + blk(" Display example MOOS configuration block. "); + mag(" --help, -h "); + blk(" Display this help message. "); + mag(" --interface, -i "); + blk(" Display MOOS publications and subscriptions. "); + mag(" --version,-v "); + blk(" Display the release version of pOdometry. "); + 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("pOdometry Example MOOS Configuration "); + blu("=============================================================== "); + blk(" "); + blk("ProcessConfig = pOdometry "); + blk("{ "); + blk(" AppTick = 4 "); + blk(" CommsTick = 4 "); + blk(" "); + blk("} "); + blk(" "); + exit(0); +} + + +//---------------------------------------------------------------- +// Procedure: showInterfaceAndExit + +void showInterfaceAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("pOdometry INTERFACE "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("SUBSCRIPTIONS: "); + blk("------------------------------------ "); + blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, "); + blk(" string_val=BAR "); + blk(" "); + blk("PUBLICATIONS: "); + blk("------------------------------------ "); + blk(" Publications are determined by the node message content. "); + blk(" "); + exit(0); +} + +//---------------------------------------------------------------- +// Procedure: showReleaseInfoAndExit + +void showReleaseInfoAndExit() +{ + showReleaseInfo("pOdometry", "gpl"); + exit(0); +} + diff --git a/src/pOdometry/Odometry_Info.h b/src/pOdometry/Odometry_Info.h new file mode 100644 index 0000000..8cd9d8d --- /dev/null +++ b/src/pOdometry/Odometry_Info.h @@ -0,0 +1,18 @@ +/****************************************************************/ +/* NAME: Clement Li */ +/* ORGN: MIT Cambridge MA */ +/* FILE: Odometry_Info.h */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#ifndef Odometry_INFO_HEADER +#define Odometry_INFO_HEADER + +void showSynopsis(); +void showHelpAndExit(); +void showExampleConfigAndExit(); +void showInterfaceAndExit(); +void showReleaseInfoAndExit(); + +#endif + diff --git a/src/pOdometry/main.cpp b/src/pOdometry/main.cpp new file mode 100644 index 0000000..db78e14 --- /dev/null +++ b/src/pOdometry/main.cpp @@ -0,0 +1,52 @@ +/************************************************************/ +/* NAME: Clement Li */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ColorParse.h" +#include "Odometry.h" +#include "Odometry_Info.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + string mission_file; + string run_command = argv[0]; + + for(int i=1; i +#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=m_tally_recd+1; + } + 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 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); +} + diff --git a/src/pXRelayTest/Relayer.h b/src/pXRelayTest/Relayer.h new file mode 100644 index 0000000..b6d629c --- /dev/null +++ b/src/pXRelayTest/Relayer.h @@ -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 diff --git a/src/pXRelayTest/Relayer_Info.cpp b/src/pXRelayTest/Relayer_Info.cpp new file mode 100644 index 0000000..7fd8ccf --- /dev/null +++ b/src/pXRelayTest/Relayer_Info.cpp @@ -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 +#include +#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","= "); + 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","= "); + blk(" Use as the Relay incoming variable "); + mag(" --interface, -i "); + blk(" Display MOOS publications and subscriptions. "); + mag(" --out","= "); + blk(" Use 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); +} + diff --git a/src/pXRelayTest/Relayer_Info.h b/src/pXRelayTest/Relayer_Info.h new file mode 100644 index 0000000..064d222 --- /dev/null +++ b/src/pXRelayTest/Relayer_Info.h @@ -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 diff --git a/src/pXRelayTest/main.cpp b/src/pXRelayTest/main.cpp new file mode 100644 index 0000000..153b095 --- /dev/null +++ b/src/pXRelayTest/main.cpp @@ -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 +#include +#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