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

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 <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);
+}
+