Refactor and clean up waypoint sequencing logic code

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

@@ -164,6 +164,13 @@ class CWaypointsNavigator : public mrpt::nav::CAbstractNavigator
   /** The waypoints-specific part of navigationStep() */
   virtual void waypoints_navigationStep();
 
+  /// Sub-algorithms within waypoints_navigationStep()
+  void internal_select_next_waypoint();
+  void internal_select_next_waypoint_default_policy(
+      std::list<std::function<void(void)>>& new_events);
+  void internal_select_next_waypoint_skip_policy(std::list<std::function<void(void)>>& new_events);
+  void internal_send_new_nav_cmd(const int prev_wp_index);
+
   bool waypoints_isAligning() const { return m_is_aligning; }
   /** Whether the last timestep was "is_aligning" in a waypoint with heading
    */

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

@@ -12,6 +12,7 @@
 #include <mrpt/img/TColor.h>
 #include <mrpt/math/TPoint2D.h>
 #include <mrpt/math/TPose2D.h>
+#include <mrpt/math/TSegment2D.h>
 #include <mrpt/opengl/opengl_frwds.h>
 #include <mrpt/system/datetime.h>
 
@@ -173,19 +174,21 @@ struct TWaypointStatusSequence
   mrpt::system::TTimeStamp timestamp_nav_started = INVALID_TIMESTAMP;
 
   /** Whether the final waypoint has been reached successfuly. */
-  bool final_goal_reached{false};
+  bool final_goal_reached = false;
 
   /** Index in `waypoints` of the waypoint the navigator is currently trying
    * to reach.
    * This will point to the last waypoint after navigation ends successfully.
    * Its value is `-1` if navigation has not started yet */
-  int waypoint_index_current_goal{-1};
+  int waypoint_index_current_goal = -1;
 
   /** Robot pose at last time step (has INVALID_NUM fields upon
    * initialization) */
   mrpt::math::TPose2D last_robot_pose{
       TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM};
 
+  mrpt::math::TSegment2D robot_move_seg;
+
   /** Ctor with default values */
   /** Gets navigation params as a human-readable format */
   std::string getAsText() const;