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

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'