Waypoint approach is now more accurate

apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp

@@ -1587,7 +1587,7 @@ void reactive_navigator_demoframe::Onplot3DMouseClick(wxMouseEvent& event)
           heading *= M_PI / 180;
         }
         m_waypoints_clicked.waypoints.emplace_back(
-            m_curCursorPos.x, m_curCursorPos.y, 0.2 /* allowed dist */, allow_skip_wps, heading);
+            m_curCursorPos.x, m_curCursorPos.y, 0.35 /* allowed dist */, allow_skip_wps, heading);
       }
       if (event.ButtonIsDown(wxMOUSE_BTN_RIGHT))
       {

doc/source/doxygen-docs/changelog.md

@@ -1,7 +1,9 @@
 \page changelog Change Log
 
 # Version 2.14.3: UNRELEASED
-(none yet)
+- Changes in libraries:
+  - \ref mrpt_nav_grp:
+    - mrpt::nav::CWaypointsNavigator: New parameter "minimum_target_approach_per_step" and feature to keep approaching waypoints until no significant improvement is done.
 
 # Version 2.14.2: Released Oct 5th, 2024
 - Changes in libraries:

libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h

@@ -119,22 +119,31 @@ class CWaypointsNavigator : public mrpt::nav::CAbstractNavigator
 
   struct TWaypointsNavigatorParams : public mrpt::config::CLoadableOptions
   {
+    TWaypointsNavigatorParams() = default;
+
     /** In meters. <0: unlimited */
-    double max_distance_to_allow_skip_waypoint{-1.0};
+    double max_distance_to_allow_skip_waypoint = -1.0;
+
     /** How many times shall a future waypoint be seen as reachable to skip
      * to it (Default: 1) */
-    int min_timesteps_confirm_skip_waypoints{1};
+    int min_timesteps_confirm_skip_waypoints = 1;
+
     /** [rad] Angular error tolerance for waypoints with an assigned heading
      * (Default: 5 deg) */
-    double waypoint_angle_tolerance;
+    double waypoint_angle_tolerance = mrpt::DEG2RAD(5.0);
+
     /** >=0 number of waypoints to forward to the underlying navigation
      * engine, to ease obstacles avoidance when a waypoint is blocked
      * (Default=0 : none). */
-    int multitarget_look_ahead{0};
+    int multitarget_look_ahead = 0;
+
+    /** While within the waypoint allowed_distance radius, this is the minimum distance [m]
+     *  to be reduced for each time step. Once it is not, the waypoint will be marked as reached.
+     */
+    double minimum_target_approach_per_step = 0.02;
 
     void loadFromConfigFile(const mrpt::config::CConfigFileBase& c, const std::string& s) override;
     void saveToConfigFile(mrpt::config::CConfigFileBase& c, const std::string& s) const override;
-    TWaypointsNavigatorParams();
   };
 
   TWaypointsNavigatorParams params_waypoints_navigator;

libs/nav/include/mrpt/nav/reactive/TWaypoint.h

@@ -189,6 +189,9 @@ struct TWaypointStatusSequence
 
   mrpt::math::TSegment2D robot_move_seg;
 
+  /** Used to detect whether we peaked in closeness to a waypoint */
+  double prevDist2target = .0;
+
   /** Whether the last timestep was "is_aligning" in a waypoint with heading
    */
   bool was_aligning = false;

libs/nav/src/reactive/CWaypointsNavigator.cpp

@@ -206,13 +206,22 @@ void CWaypointsNavigator::internal_select_next_waypoint_default_policy(
   auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
   const double dist2target = wps.robot_move_seg.distance(wp.target);
 
+  const double prev_dist2target = wps.prevDist2target;
+  wps.prevDist2target = dist2target;
+
   if (dist2target > wp.allowed_distance && !wps.was_aligning /* we were already aligning at a WP */)
     return;  // no need to check, we are not close enough.
 
-  if (!wps.was_aligning)
+  if (!wps.was_aligning && wps.prevDist2target > 0)
   {
     // We are approaching a WP, within |wp.allowed_distance| radius.
-    // XXX
+    // As long as we are getting closer and closer, let it keep going:
+    if (dist2target <
+        prev_dist2target - params_waypoints_navigator.minimum_target_approach_per_step)
+    {  // ok, we are getting closer, do nothing:
+      return;
+    }
+    // Continue and accept the WP as reached (or perform the alignment there)
   }
 
   bool consider_wp_reached = false;
@@ -506,6 +515,7 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFi
   MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
   MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
   MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
+  MRPT_LOAD_CONFIG_VAR(minimum_target_approach_per_step, double, c, s);
 }
 
 void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile(
@@ -527,12 +537,7 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile
       ">=0 number of waypoints to forward to the underlying navigation "
       "engine, to ease obstacles avoidance when a waypoint is blocked "
       "(Default=0 : none)");
-}
-
-CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams() :
-    waypoint_angle_tolerance(mrpt::DEG2RAD(5.0))
-
-{
+  MRPT_SAVE_CONFIG_VAR(minimum_target_approach_per_step, c, s);
 }
 
 /** \callergraph */

share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini

@@ -22,8 +22,8 @@ enable_time_profiler                         = false
 max_distance_to_allow_skip_waypoint               = -1.000000            // Max distance to `foresee` waypoints [meters]. (<0: unlimited)
 min_timesteps_confirm_skip_waypoints              = 1                    // Min timesteps a `future` waypoint must be seen as reachable to become the active one.
 waypoint_angle_tolerance                          = 5.0                  // Angular error tolerance for waypoints with an assigned heading [deg]
-multitarget_look_ahead   = 0  // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none).
-
+multitarget_look_ahead                            = 0                    // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none).
+minimum_target_approach_per_step                  = 0.02                 // While within the waypoint allowed_distance radius, this is the minimum distance [m] to be reduced for each time step. Once it is not, the waypoint will be marked as reached.
 
 [CAbstractPTGBasedReactive]
 robotMax_V_mps                                    = ${ROBOT_MAX_V}                // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set]