correct event triggering; replan on errors

MRPT · Dec 21, 2022 · f578e9f · f578e9f
1 parent ff9bfd7
commit f578e9f
Show file tree

Hide file tree

Showing 2 changed files with 67 additions and 16 deletions.
diff --git a/libselfdriving/include/selfdriving/algos/NavEngine.h b/libselfdriving/include/selfdriving/algos/NavEngine.h
@@ -434,12 +434,20 @@ class NavEngine : public mrpt::system::COutputLogger
          * for viz purposed only */
         std::optional<VehicleOdometryState> lastEnqueuedTriggerOdometry;
 
-        void active_plan_reset()
+        void active_plan_reset(bool alsoClearComputedPath = false)
         {
             activePlanEdgeIndex.reset();
             activePlanEdgeSentIndex.reset();
             activePlanEdgesSentOut.clear();
             activePlanInitOdometry.reset();
+
+            if (alsoClearComputedPath)
+            {
+                activePlanOutput = {};
+                activePlanPath.clear();
+                activePlanPathEdges.clear();
+                pathPlannerTargetWpIdx.reset();
+            }
         }
 
         /** 0-based index of which edge in activePlanPathEdges[] is currently
@@ -539,6 +547,9 @@ class NavEngine : public mrpt::system::COutputLogger
 
     void internal_mark_current_wp_as_reached();
 
+    /** Returns true if all waypoints has been reached successfully. */
+    bool check_all_waypoints_are_done();
+
     struct AboutToReachWpInfo
     {
         AboutToReachWpInfo() = default;

diff --git a/libselfdriving/src/algos/NavEngine.cpp b/libselfdriving/src/algos/NavEngine.cpp
@@ -238,7 +238,7 @@ void NavEngine::cancel()
 
     MRPT_LOG_DEBUG("NavEngine::cancel() called.");
     navigationStatus_ = NavStatus::IDLE;
-    innerState_.active_plan_reset();
+    innerState_.active_plan_reset(true);
 
     if (config_.vehicleMotionInterface)
     {
@@ -411,6 +411,24 @@ void NavEngine::impl_navigation_step()
     if (lastNavigationState_ != NavStatus::NAVIGATING)
         internal_on_start_new_navigation();
 
+    // navigation ended?
+    if (check_all_waypoints_are_done())
+    {
+        MRPT_LOG_INFO("All waypoints reached, status NAVIGATING -> IDLE.");
+        navigationStatus_ = NavStatus::IDLE;
+        innerState_.active_plan_reset();
+        if (config_.vehicleMotionInterface)
+        {
+            config_.vehicleMotionInterface->stop(STOP_TYPE::REGULAR);
+            config_.vehicleMotionInterface->stop_watchdog();
+        }
+
+        pendingEvents_.emplace_back(
+            [this]() { config_.vehicleMotionInterface->on_nav_end(); });
+
+        return;
+    }
+
     // Get current robot kinematic state:
     update_robot_kinematic_state();
 
@@ -519,11 +537,13 @@ void NavEngine::check_immediate_collision()
 
         config_.vehicleMotionInterface->stop(STOP_TYPE::EMERGENCY);
 
-#if 0
-        // Re-plan from scratch:
-        _.pathPlannerTargetWpIdx.reset();
-        _.active_plan_reset();
-#endif
+        // clear path and recompute:
+        _.active_plan_reset(true);
+
+        // user callbacks:
+        pendingEvents_.emplace_back([this]() {
+            config_.vehicleMotionInterface->on_apparent_collision();
+        });
     }
 }
 
@@ -974,9 +994,16 @@ void NavEngine::send_next_motion_cmd_or_nop()
     // Following error due to enqueued motion time out?
     if (config_.vehicleMotionInterface->enqeued_motion_timed_out())
     {
-        // Abort path planning:
-        cancel();
-        MRPT_TODO("Re-plan and/or send error event out?");
+        // stop the robot, report, and try to recover:
+        if (config_.vehicleMotionInterface)
+            config_.vehicleMotionInterface->stop(STOP_TYPE::EMERGENCY);
+
+        MRPT_LOG_ERROR(
+            "Enqueued motion primitive did not triggered (TIMEOUT). Stopping "
+            "the robot and replanning.");
+
+        innerState_.active_plan_reset();
+
         return;
     }
 
@@ -1969,19 +1996,32 @@ void NavEngine::internal_mark_current_wp_as_reached()
             _.waypointNavStatus.waypoints.at(i).skipped = true;
 
             // user callbacks:
-            config_.vehicleMotionInterface->on_waypoint_reached(
-                i, false /* =skipped */);
+            pendingEvents_.emplace_back([this, i]() {
+                config_.vehicleMotionInterface->on_waypoint_reached(
+                    i, false /* =skipped */);
+            });
         }
     }
 
     // mark final one as reached:
     _.waypointNavStatus.waypoints.at(reachedIdx).reached = true;
 
     // user callbacks:
-    config_.vehicleMotionInterface->on_waypoint_reached(
-        reachedIdx, true /* =reached*/);
+    pendingEvents_.emplace_back([this, reachedIdx]() {
+        config_.vehicleMotionInterface->on_waypoint_reached(
+            reachedIdx, true /* =reached */);
+    });
 
     // clear statuses so we can launch a new plan in the next iteration:
-    _.pathPlannerTargetWpIdx.reset();
-    _.active_plan_reset();
+    _.active_plan_reset(true);
+}
+
+bool NavEngine::check_all_waypoints_are_done()
+{
+    auto& _ = innerState_;
+    if (_.waypointNavStatus.waypoints.empty())
+        return false;  // should never happen...
+
+    // Was the last waypoint reached?
+    return _.waypointNavStatus.waypoints.rbegin()->reached;
 }