progress with nav interface

libselfdriving/include/selfdriving/algos/WaypointSequencer.h

@@ -6,11 +6,16 @@
 
 #pragma once
 
-#include <mrpt/nav/reactive/CAbstractNavigator.h>
+#include <mrpt/poses/CPose2DInterpolator.h>
+#include <mrpt/system/COutputLogger.h>
+#include <mrpt/system/CTimeLogger.h>
 #include <selfdriving/data/Waypoints.h>
 #include <selfdriving/interfaces/ObstacleSource.h>
 #include <selfdriving/interfaces/VehicleMotionInterface.h>
 
+#include <functional>
+#include <list>
+
 namespace selfdriving
 {
 /** The different states for the navigation system. */
@@ -66,12 +71,16 @@ struct NavErrorReason
  *  }
  *  \enddot
  *
+ * All methods are thread-safe, in the sense that mutexes are internally used
+ * to ensure no undefined navigation state is possible if invoking an object of
+ * this class from more than one thread.
+ *
  */
-class WaypointSequencer
+class WaypointSequencer : public mrpt::system::COutputLogger
 {
    public:
     /** ctor */
-    WaypointSequencer() = default;
+    WaypointSequencer() : mrpt::system::COutputLogger("WaypointSequencer") {}
 
     /** dtor */
     virtual ~WaypointSequencer();
@@ -162,7 +171,7 @@ class WaypointSequencer
     virtual void resetNavError();
 
     /** Returns the current navigator state. */
-    inline NavState getCurrentState() const { return m_navigationState; }
+    inline NavState getCurrentState() const { return navigationState_; }
 
     /** In case of state=NAV_ERROR, this returns the reason for the error.
      * Error state is reseted every time a new navigation starts with
@@ -191,7 +200,7 @@ class WaypointSequencer
     void endWaypointsAccess() { m_nav_waypoints_cs.unlock(); }
 
     /** Publicly available time profiling object. Default: disabled */
-    mrpt::system::CTimeLogger m_navProfiler{
+    mrpt::system::CTimeLogger navProfiler_{
         true /*enabled*/, "WaypointSequencer"};
 
     /** @}*/
@@ -208,10 +217,12 @@ class WaypointSequencer
 
    protected:
     /** Current and last internal state of navigator: */
-    NavState       m_navigationState     = NavState::IDLE;
-    NavState       m_lastNavigationState = NavState::IDLE;
+    NavState       navigationState_     = NavState::IDLE;
+    NavState       lastNavigationState_ = NavState::IDLE;
     NavErrorReason m_navErrorReason;
 
+    bool initialized_ = false;
+
     /** mutex for all navigation methods */
     std::recursive_mutex m_nav_cs;
 
@@ -245,7 +256,7 @@ class WaypointSequencer
     /** Events generated during navigationStep(), enqueued to be called at the
      * end of the method execution to avoid user code to change the navigator
      * state. */
-    std::list<std::function<void(void)>> m_pending_events;
+    std::list<std::function<void(void)>> pendingEvents_;
 
     void dispatchPendingNavEvents();
 
@@ -262,8 +273,7 @@ class WaypointSequencer
      * `call_virtual_nav_method` can be set to false to avoid calling the
      * virtual method performNavigationStep()
      */
-    virtual void performNavigationStepNavigating(
-        bool call_virtual_nav_method = true);
+    virtual void performNavigationStepNavigating();
 
     /** Will be false until the navigation end is sent, and it is reset with
      * each new command */
@@ -284,6 +294,8 @@ class WaypointSequencer
     //    virtual bool impl_waypoint_is_reachable(
     //        const mrpt::math::TPoint2D& wp_local_wrt_robot) const = 0;
 
+    void internal_on_start_new_navigation();
+
 #if 0
     bool checkHasReachedTarget(const double targetDist) const override;
 

libselfdriving/include/selfdriving/data/Waypoints.h

@@ -196,13 +196,13 @@ struct WaypointStatusSequence
     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};
+     * It has no value if navigation has not started yet */
+    std::optional<size_t> waypoint_index_current_goal;
 
     /** Robot pose at last time step (has INVALID_NUM fields upon
      * initialization) */

libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h

@@ -7,14 +7,21 @@
 #pragma once
 
 #include <mrpt/kinematics/CVehicleVelCmd.h>
-#include <selfdriving/EnqueuedMotionCmd.h>
-#include <selfdriving/VehicleLocalizationState.h>
-#include <selfdriving/VehicleOdometryState.h>
+#include <mrpt/system/COutputLogger.h>
+#include <selfdriving/data/EnqueuedMotionCmd.h>
+#include <selfdriving/data/VehicleLocalizationState.h>
+#include <selfdriving/data/VehicleOdometryState.h>
 
 namespace selfdriving
 {
 using mrpt::kinematics::CVehicleVelCmd;
 
+enum class STOP_TYPE : uint8_t
+{
+    REGULAR = 0,
+    EMERGENCY
+};
+
 /** The virtual interface between a path follower / plan executor and a real
  * mobile platform (vehicle, robot), regarding controlling its motion.
  *
@@ -41,12 +48,15 @@ using mrpt::kinematics::CVehicleVelCmd;
  *
  *
  */
-class VehicleMotionInterface
+class VehicleMotionInterface : public mrpt::system::COutputLogger
 {
    public:
     using Ptr = std::shared_ptr<VehicleMotionInterface>;
 
-    VehicleMotionInterface()          = default;
+    VehicleMotionInterface()
+        : mrpt::system::COutputLogger("VehicleMotionInterface")
+    {
+    }
     virtual ~VehicleMotionInterface() = default;
 
     /** Provides access to the vehicle localization data.
@@ -114,6 +124,36 @@ class VehicleMotionInterface
     virtual bool motion_execute(
         const std::optional<CVehicleVelCmd::Ptr>& immediate,
         const std::optional<EnqueuedMotionCmd>&   next) = 0;
+
+    /** Stops the vehicle. Different levels of abruptness in the stop can be
+     * considered given the emergency condition or not of the command.
+     */
+    virtual void stop(const STOP_TYPE stopType) = 0;
+
+    virtual void stop_watchdog()
+    {
+        MRPT_LOG_INFO("Default stop_watchdog() called.");
+    }
+    virtual void start_watchdog([
+        [maybe_unused]] const size_t periodMilliseconds)
+    {
+        MRPT_LOG_INFO("Default start_watchdog() called.");
+    }
+
+    /** @name Event callbacks
+     *  @{ */
+
+    virtual void on_nav_end_due_to_error()
+    {
+        MRPT_LOG_WARN("Default on_nav_end_due_to_error() called.");
+    }
+
+    virtual void on_nav_start()
+    {
+        MRPT_LOG_WARN("Default on_nav_start() event handler called.");
+    }
+
+    /** @} */
 };
 
 }  // namespace selfdriving