first nav events; cleaner and safer code

apps/path-planner-cli/path-planner-cli.cpp

@@ -324,9 +324,15 @@ static void do_plan_path()
               << " overall edges, " << plan.motionTree.nodes().size()
               << " nodes\n";
 
+    if (!plan.bestNodeId.has_value())
+    {
+        std::cerr << "No bestNodeId in plan output.\n";
+        return;
+    }
+
     // backtrack:
     auto [plannedPath, pathEdges] =
-        plan.motionTree.backtrack_path(plan.bestNodeId);
+        plan.motionTree.backtrack_path(*plan.bestNodeId);
 
     if (!arg_noRefine.isSet())
     {

libselfdriving/include/selfdriving/algos/NavEngine.h

@@ -275,7 +275,8 @@ class NavEngine : public mrpt::system::COutputLogger
 
     /** Update the GUI with a partial or final path only (no whole tree) */
     void send_path_to_viz_and_navlog(
-        const MotionPrimitivesTreeSE2& tree, const TNodeID finalNode,
+        const MotionPrimitivesTreeSE2&         tree,
+        const std::optional<TNodeID>&          finalNode,
         const PlannerInput&                    originalPlanInput,
         const std::vector<CostEvaluator::Ptr>& costEvaluators);
 
@@ -536,6 +537,8 @@ class NavEngine : public mrpt::system::COutputLogger
 
     void merge_new_plan_if_better(const PathPlannerOutput& result);
 
+    void internal_mark_current_wp_as_reached();
+
     struct AboutToReachWpInfo
     {
         AboutToReachWpInfo() = default;

libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h

@@ -20,12 +20,12 @@
 #include <cstdint>
 #include <deque>
 #include <list>
+#include <optional>
 #include <set>
 #include <tuple>
 
 namespace selfdriving
 {
-using mrpt::graphs::INVALID_NODEID;
 using mrpt::graphs::TNodeID;
 
 /** Generic base for metrics */

libselfdriving/include/selfdriving/data/PlannerOutput.h

@@ -10,6 +10,7 @@
 #include <selfdriving/data/MotionPrimitivesTree.h>
 #include <selfdriving/data/PlannerInput.h>
 
+#include <optional>
 #include <set>
 
 namespace selfdriving
@@ -37,14 +38,15 @@ struct PlannerOutput
     double pathCost = std::numeric_limits<double>::max();
 
     /** The tree node ID for the goal pose */
-    TNodeID goalNodeId = mrpt::graphs::INVALID_NODEID;
+    std::optional<TNodeID> goalNodeId;
 
-    /** The tree node with the smallest cost-to-goal. This will be identical
-     *  to goalNodeId for successful plans reaching the desired goal state, or
-     *  something different for unfinished or unsuccessful plans.
+    /** The tree node with the smallest cost-to-goal. This will be
+     * identical to goalNodeId for successful plans reaching the desired
+     * goal state, or something different for unfinished or unsuccessful
+     * plans.
      */
-    TNodeID bestNodeId           = mrpt::graphs::INVALID_NODEID;
-    cost_t  bestNodeIdCostToGoal = std::numeric_limits<cost_t>::max();
+    std::optional<TNodeID> bestNodeId;
+    cost_t bestNodeIdCostToGoal = std::numeric_limits<cost_t>::max();
 
     /** The generated motion tree that explores free space starting at "start"
      */

libselfdriving/include/selfdriving/data/ProgressCallbackData.h

@@ -24,7 +24,7 @@ struct ProgressCallbackData
     cost_t bestCostFromStart = std::numeric_limits<cost_t>::max();
     cost_t bestCostToGoal    = std::numeric_limits<cost_t>::max();
     MotionPrimitivesTreeSE2::edge_sequence_t bestPath;
-    TNodeID                                  bestFinalNode     = INVALID_NODEID;
+    std::optional<TNodeID>                   bestFinalNode;
     const MotionPrimitivesTreeSE2*           tree              = nullptr;
     const PlannerInput*                      originalPlanInput = nullptr;
     const std::vector<CostEvaluator::Ptr>*   costEvaluators    = nullptr;
@@ -33,4 +33,4 @@ struct ProgressCallbackData
 using planner_progress_callback_t =
     std::function<void(const ProgressCallbackData&)>;
 
-}  // namespace selfdriving
+}  // namespace selfdriving

libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h

@@ -192,10 +192,10 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
         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
+     * @brief Callback if navigation ended by an accepted trigger or reached the
+     * last specified waypoint
      */
-    virtual void on_nav_end() 
+    virtual void on_nav_end()
     {
         MRPT_LOG_WARN("Default on_nav_end() event handler called.");
     }
@@ -215,16 +215,18 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger,
         MRPT_LOG_WARN("Default on_apparent_collision() event handler called.");
     }
     /**
-     * @brief Callback upon reaching a waypoint in a waypoint sequeunce. 
+     * @brief Callback upon reaching a waypoint in a waypoint sequeunce.
      * Mostly used for logging
      * @param waypoint_index index of waypoint in the sequence
-     * @param reached_skipped bool indicator if waypoint was reached or skipped
+     * @param reached_skipped whether it was reached(true) or skipped (false)
      */
-    virtual void on_waypoint_reached(const size_t waypoint_index, 
-                                    [[maybe_unused]]bool reached_skipped)
+    virtual void on_waypoint_reached(
+        const size_t waypoint_index, bool reached_skipped)
     {
-        MRPT_LOG_WARN(mrpt::format("Default on_waypoint_reached() index = %zu event handler called.",
-        waypoint_index).c_str());
+        MRPT_LOG_WARN_FMT(
+            "Default on_waypoint_reached() index = %zu event "
+            "handler called (event='%s').",
+            waypoint_index, reached_skipped ? "reached" : "skipped");
     }
     /**
      * @brief Callback when NavEngine cannot reach a specified target location

libselfdriving/src/algos/NavEngine.cpp

@@ -917,7 +917,7 @@ void NavEngine::check_new_planner_output()
         _.active_plan_reset();
 
         auto [path, edges] = _.activePlanOutput.po.motionTree.backtrack_path(
-            _.activePlanOutput.po.bestNodeId);
+            *_.activePlanOutput.po.bestNodeId);
 
         // Correct PTG arguments according to the final actual poses.
         // Needed to correct for lattice approximations:
@@ -1342,7 +1342,8 @@ void NavEngine::send_planner_output_to_viz(const PathPlannerOutput& ppo)
 }
 
 void NavEngine::send_path_to_viz_and_navlog(
-    const MotionPrimitivesTreeSE2& tree, const TNodeID finalNode,
+    const MotionPrimitivesTreeSE2&         tree,
+    const std::optional<TNodeID>&          finalNode,
     const PlannerInput&                    originalPlanInput,
     const std::vector<CostEvaluator::Ptr>& costEvaluators)
 {
@@ -1487,7 +1488,8 @@ void NavEngine::send_current_state_to_viz_and_navlog()
     }
 
     if (const auto& triggOdom = _.lastEnqueuedTriggerOdometry;
-        triggOdom.has_value() && !_.activePlanPath.empty())
+        triggOdom.has_value() && !_.activePlanPath.empty() &&
+        _.activePlanInitOdometry.has_value())
     {
         auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.15);
         glCorner->setPose(
@@ -1757,28 +1759,35 @@ bool NavEngine::approach_target_controller()
     const auto out = config_.targetApproachController->execute(tacIn);
 
     MRPT_LOG_INFO_FMT(
-        "Controller towards target executed since atrw.aboutToReach==true, "
+        "Controller towards target executed since atrw. aboutToReach=true, "
         "results: targetDist=%f handled=%s reachedDetected=%s",
         atrw.distanceToWaypoint, out.handled ? "YES" : "NO",
         out.reachedDetected ? "YES" : "NO");
 
-    MRPT_TODO("Check out.reachedDetected");
-
     if (out.handled)
     {
-        ASSERT_(config_.vehicleMotionInterface);
-        if (out.generatedMotion)
+        if (out.reachedDetected)
         {
-            config_.vehicleMotionInterface->motion_execute(
-                out.generatedMotion, std::nullopt);
-
-            // log record copy:
-            _.sentOutCmdInThisIteration = out.generatedMotion;
+            // end of navigation to waypoint:
+            internal_mark_current_wp_as_reached();
         }
         else
         {
-            config_.vehicleMotionInterface->motion_execute(
-                std::nullopt, std::nullopt);
+            // It's a motion command:
+            ASSERT_(config_.vehicleMotionInterface);
+            if (out.generatedMotion)
+            {
+                config_.vehicleMotionInterface->motion_execute(
+                    out.generatedMotion, std::nullopt);
+
+                // log record copy:
+                _.sentOutCmdInThisIteration = out.generatedMotion;
+            }
+            else
+            {
+                config_.vehicleMotionInterface->motion_execute(
+                    std::nullopt, std::nullopt);
+            }
         }
     }
 
@@ -1844,8 +1853,15 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result)
     const double currentPlanDistToGoal =
         (goal - _.activePlanPath.rbegin()->pose).translation().norm();
 
+    if (!result.po.bestNodeId.has_value())
+    {
+        // error: no good plan.
+        MRPT_LOG_WARN("Dropping new path planning result, no bestNodeID.");
+        return;
+    }
+
     const double newPlanDistToGoal =
-        (goal - result.po.motionTree.nodes().at(result.po.bestNodeId).pose)
+        (goal - result.po.motionTree.nodes().at(*result.po.bestNodeId).pose)
             .translation()
             .norm();
 
@@ -1866,7 +1882,7 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result)
     // for-the-future segment that was just received:
 
     auto [newPath, newEdges] =
-        result.po.motionTree.backtrack_path(result.po.bestNodeId);
+        result.po.motionTree.backtrack_path(*result.po.bestNodeId);
 
     // Correct PTG arguments according to the final actual poses.
     // Needed to correct for lattice approximations:
@@ -1924,3 +1940,48 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result)
         _.activePlanEdgesSentOut.insert(i);
     _.activePlanInitOdometry = formerEdgeInitOdometry;
 }
+
+void NavEngine::internal_mark_current_wp_as_reached()
+{
+    auto& _ = innerState_;
+
+    // sanity checks:
+    ASSERT_(_.pathPlannerTargetWpIdx.has_value());
+    ASSERT_LT_(*_.pathPlannerTargetWpIdx, _.waypointNavStatus.waypoints.size());
+
+    const waypoint_idx_t reachedIdx = *_.pathPlannerTargetWpIdx;
+
+    // We are about to mark "reachedIdx" as reached.
+    // First, go over the former ones, since the last "reached" and mark them as
+    // skipped:
+    {
+        waypoint_idx_t lastReached = reachedIdx;
+        while (lastReached > 0 &&
+               !_.waypointNavStatus.waypoints.at(lastReached).reached)
+        {  // go back:
+            lastReached--;
+        }
+        // now, mark all in the range [lastReached+1, ..., reachedIdx-1] as
+        // skipped:
+        for (waypoint_idx_t i = lastReached + 1; i + 1 <= reachedIdx; i++)
+        {
+            // mark as skipped:
+            _.waypointNavStatus.waypoints.at(i).skipped = true;
+
+            // user callbacks:
+            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*/);
+
+    // clear statuses so we can launch a new plan in the next iteration:
+    _.pathPlannerTargetWpIdx.reset();
+    _.active_plan_reset();
+}

libselfdriving/src/algos/TPS_Astar.cpp

@@ -424,11 +424,11 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
             tLastCallback = tNow;
 
             const auto [foundPath, pathEdges] =
-                tree.backtrack_path(po.bestNodeId);
+                tree.backtrack_path(*po.bestNodeId);
 
             // call user callback:
             ProgressCallbackData pcd;
-            pcd.bestCostFromStart = tree.nodes().at(po.bestNodeId).cost_;
+            pcd.bestCostFromStart = tree.nodes().at(*po.bestNodeId).cost_;
             pcd.bestCostToGoal    = po.bestNodeIdCostToGoal;
             pcd.bestFinalNode     = po.bestNodeId;
             pcd.bestPath          = std::move(pathEdges);
@@ -455,8 +455,7 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
 #endif
 
     po.success = po.goalNodeId == po.bestNodeId;
-    if (po.bestNodeId != INVALID_NODEID)
-        po.pathCost = tree.nodes().at(po.bestNodeId).cost_;
+    if (po.bestNodeId) po.pathCost = tree.nodes().at(*po.bestNodeId).cost_;
 
     po.computationTime = mrpt::Clock::nowDouble() - planInitTime;