Progress implementing events (cc #10)

Author: jlblancoc

libselfdriving/include/selfdriving/algos/NavEngine.h

@@ -127,8 +127,6 @@ class NavEngine : public mrpt::system::COutputLogger
          * user in the navigation request. */
         double dist_to_target_for_sending_event{0};
 
-        /** navigator timeout (seconds) [Default=30 sec] */
-        double alarm_seems_not_approaching_target_timeout{30};
 
         /** (Default value=0.6) When closer than this distance, check if the
          * target is blocked to abort navigation with an error. */
@@ -149,6 +147,11 @@ class NavEngine : public mrpt::system::COutputLogger
         double maxDistanceForTargetApproach        = 1.5;  // [m]
         double maxRelativeHeadingForTargetApproach = 180.0_deg;  // [rad]
 
+        /** Navigation timeout (seconds) [Default=30 sec]
+         *  See description of VehicleMotionInterface::on_path_seems_blocked()
+         */
+        double timeoutNotGettingCloserGoal = 30;
+
         bool generateNavLogFiles = false;
 
         /** Actual files will be
@@ -447,6 +450,8 @@ class NavEngine : public mrpt::system::COutputLogger
                 activePlanPath.clear();
                 activePlanPathEdges.clear();
                 pathPlannerTargetWpIdx.reset();
+                lastDistanceToGoalTimestamp.reset();
+                lastDistanceToGoal.reset();
             }
         }
 
@@ -487,6 +492,11 @@ class NavEngine : public mrpt::system::COutputLogger
             navlogDebugMessages.clear();
         }
 
+        /** Values used to check against
+         * Configuration::timeoutNotGettingCloserGoal
+         */
+        std::optional<double> lastDistanceToGoalTimestamp, lastDistanceToGoal;
+
         /** @} */
 
         /** For sending an alarm (error event) when it seems that we are not

libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h

@@ -164,8 +164,8 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
     {
         MRPT_LOG_INFO("Default stop_watchdog() called.");
     }
-    virtual void start_watchdog([
-        [maybe_unused]] const size_t periodMilliseconds)
+    virtual void start_watchdog(
+        [[maybe_unused]] const size_t periodMilliseconds)
     {
         MRPT_LOG_INFO("Default start_watchdog() called.");
     }
@@ -184,13 +184,15 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
     {
         MRPT_LOG_WARN("Default on_nav_end_due_to_error() called.");
     }
+
     /**
      * @brief Callback upon starting a new waypointsequence navigation
      */
     virtual void on_nav_start()
     {
         MRPT_LOG_WARN("Default on_nav_start() event handler called.");
     }
+
     /**
      * @brief Callback if navigation ended by an accepted trigger or reached the
      * last specified waypoint
@@ -199,16 +201,25 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
     {
         MRPT_LOG_WARN("Default on_nav_end() event handler called.");
     }
+
     /**
-     * @brief Callback when NavEngine encounters a blocked way
-     * Could lead to path replan
+     * @brief Callback for when NavEngine cannot make progress to get
+     * increasingly closer to the final target during a certain period of time.
+     * It may indicate that the high-level path the vehicle is trying to follow
+     * is no longer valid due to a blocked way. On your user side, you could
+     * call NavEngine::cancel() and/or re-compute an alternative path and issue
+     * a new set of navigation waypoints or just report the error to the user.
+     *
+     * \sa NavEngine::Configuration::timeoutNotGettingCloserGoal
      */
     virtual void on_path_seems_blocked()
     {
         MRPT_LOG_WARN("Default on_path_seems_blocked() event handler called.");
     }
+
     /**
-     * @brief Callback when the NavEngine predicts a collision with an obstacle
+     * @brief Callback when the NavEngine *predicts* a collision with an
+     * obstacle and needed to issue a stop command.
      */
     virtual void on_apparent_collision()
     {
@@ -228,6 +239,7 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
             "handler called (event='%s').",
             waypoint_index, reached_skipped ? "reached" : "skipped");
     }
+
     /**
      * @brief Callback when NavEngine cannot reach a specified target location
      * because there are obstacles at the specified target

libselfdriving/src/algos/NavEngine.cpp

@@ -266,6 +266,7 @@ void NavEngine::suspend()
     if (navigationStatus_ == NavStatus::NAVIGATING)
     {
         navigationStatus_ = NavStatus::SUSPENDED;
+        innerState_.active_plan_reset(true);
 
         if (config_.vehicleMotionInterface)
         {
@@ -393,7 +394,9 @@ void NavEngine::update_robot_kinematic_state()
                innerState_.latestPoses.begin()->first,
                innerState_.latestPoses.rbegin()->first) >
                PREVIOUS_POSES_MAX_AGE)
-    { innerState_.latestPoses.erase(innerState_.latestPoses.begin()); }
+    {
+        innerState_.latestPoses.erase(innerState_.latestPoses.begin());
+    }
     while (innerState_.latestOdomPoses.size() > 1 &&
            mrpt::system::timeDifference(
                innerState_.latestOdomPoses.begin()->first,
@@ -927,7 +930,9 @@ void NavEngine::check_new_planner_output()
 
     // Merge or overwrite current plan:
     if (result.startingFromCurrentPlanNode.has_value())
-    { merge_new_plan_if_better(result); }
+    {
+        merge_new_plan_if_better(result);
+    }
     else
     {
         MRPT_LOG_INFO_STREAM("Taking new path planning result.");
@@ -1748,6 +1753,35 @@ bool NavEngine::approach_target_controller()
 
     const auto atrw = internal_check_about_to_reach_stop_wp();
 
+    // Check for no-getting-closer timeout here, since we have the distance to
+    // goal:
+    if (!_.lastDistanceToGoal.has_value() ||
+        atrw.distanceToWaypoint < *_.lastDistanceToGoal)
+    {
+        // Good: we are making progress:
+        _.lastDistanceToGoal = atrw.distanceToWaypoint;
+        _.lastDistanceToGoalTimestamp =
+            config_.vehicleMotionInterface->robot_time();
+    }
+    else if (_.lastDistanceToGoalTimestamp.has_value())
+    {
+        // we are not making progress:
+        const double age = config_.vehicleMotionInterface->robot_time() -
+                           *_.lastDistanceToGoalTimestamp;
+        if (age > config_.timeoutNotGettingCloserGoal)
+        {
+            MRPT_LOG_INFO_FMT(
+                "Triggering on_path_seems_blocked() event since distance to "
+                "goal could not get shorter than %f for %f seconds.",
+                _.lastDistanceToGoal.value(), age);
+
+            pendingEvents_.emplace_back([this]() {
+                config_.vehicleMotionInterface->on_path_seems_blocked();
+            });
+        }
+    }
+
+    // check about-to-reach-waypoint return values:
     if (atrw.aboutToReach)
     {
         MRPT_LOG_DEBUG_STREAM(