fix target goal replacement

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

@@ -326,7 +326,7 @@ static void do_plan_path()
 
     // backtrack:
     auto [plannedPath, pathEdges] =
-        plan.motionTree.backtrack_path(plan.goalNodeId);
+        plan.motionTree.backtrack_path(plan.bestNodeId);
 
     if (!arg_noRefine.isSet())
     {
@@ -337,7 +337,7 @@ static void do_plan_path()
     // Visualize:
     selfdriving::VisualizationOptions vizOpts;
 
-    vizOpts.renderOptions.highlight_path_to_node_id = plan.goalNodeId;
+    vizOpts.renderOptions.highlight_path_to_node_id = plan.bestNodeId;
     vizOpts.renderOptions.color_normal_edge         = {0xb0b0b0, 0x20};  // RGBA
 
     vizOpts.renderOptions.showEdgeCosts = arg_showEdgeWeights.isSet();

libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h

@@ -245,6 +245,12 @@ class MotionPrimitivesTree : public mrpt::graphs::CDirectedTree<EDGE_TYPE>
      */
     const node_map_t& nodes() const { return nodes_; }
 
+    /** Write-access to node data (use with caution) */
+    NODE_TYPE_DATA& node_state(const TNodeID nodeId)
+    {
+        return nodes_.at(nodeId);
+    }
+
     /** Builds the path (sequence of nodes, with info about next edge) up-tree
      * from a `target_node` towards the root
      * - path_t nodes are ordered in the direction ROOT -> start_node

libselfdriving/include/selfdriving/data/PlannerOutput.h

@@ -27,8 +27,8 @@ struct PlannerOutput
     /** Time spent (in secs) */
     double computationTime = 0;
 
-    /** Distance from best found path to goal */
-    double goalDistance = std::numeric_limits<double>::max();
+    // Distance from best found path to goal
+    // double goalDistance = std::numeric_limits<double>::max();
 
     /** Total cost of the best found path.
      * Cost is the Euclidean distance, modified by the additional cost map

libselfdriving/src/algos/TPS_Astar.cpp

@@ -162,22 +162,26 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
     }
 
     // Define goal node ID:
-    auto& nodeGoal =
-        getOrCreateNodeByPose(in.stateGoal.asSE2KinState(), nextFreeId);
-    po.goalNodeId = nodeGoal.id.value();
+    // Use a pointer so we can replace it with a pointer to a different node as
+    // needed.
+    auto* nodeGoal =
+        &getOrCreateNodeByPose(in.stateGoal.asSE2KinState(), nextFreeId);
+    po.goalNodeId = nodeGoal->id.value();
 
     // Insert a dummy edge between root -> goal, just to allow new node IDs
     // to be generated in sequence:
+#if 0
     {
         MoveEdgeSE2_TPS dummyEdge;
         dummyEdge.cost      = std::numeric_limits<cost_t>::max();
         dummyEdge.parentId  = tree.root;
         dummyEdge.stateFrom = in.stateStart;
         dummyEdge.stateTo   = in.stateGoal.asSE2KinState();
         tree.insert_node_and_edge(
-            tree.root, nodeGoal.id.value(), in.stateGoal.asSE2KinState(),
+            tree.root, nodeGoal->id.value(), in.stateGoal.asSE2KinState(),
             dummyEdge);
     }
+#endif
 
     // Goal cell indices:
     const auto goalCellIndices =
@@ -221,7 +225,8 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
                 // Correct current node location due to grid discretization,
                 // a latter refine_trajectory() stage will correct PTG path
                 // params if needed:
-                const auto& goalPt   = in.stateGoal.state.point();
+                const auto& goalPt = in.stateGoal.state.point();
+                // Update in our internal A* node:
                 current.state.pose.x = goalPt.x;
                 current.state.pose.y = goalPt.y;
 
@@ -230,18 +235,24 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
                     << in.stateGoal.state.asString() << " ==> "
                     << current.state.asString());
 
-                nodeGoal      = current;
-                po.goalNodeId = nodeGoal.id.value();
-                po.bestNodeId = po.goalNodeId;
+                nodeGoal                = &current;
+                po.goalNodeId           = nodeGoal->id.value();
+                po.bestNodeId           = po.goalNodeId;
+                po.bestNodeIdCostToGoal = 0;
             }
             else
             {
                 // Correct current node location due to grid discretization,
                 // a latter refine_trajectory() stage will correct PTG path
                 // params if needed:
                 const auto& goalPose = in.stateGoal.state.pose();
-                current.state.pose   = goalPose;
+                // Update in our internal A* node:
+                current.state.pose = goalPose;
+
+                po.bestNodeIdCostToGoal = 0;
             }
+            // Update in the tree node also:
+            tree.node_state(*current.id).pose = current.state.pose;
 
             break;
         }
@@ -381,6 +392,9 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
 
             // Keep an updated pointer to the best, so-far, node (under the
             // heuristic criterion):
+            // Note that even if costToGoal==0, we may still need to keep
+            // iterating A*, since a better (shorter) path might be still be
+            // found.
             if (costToGoal < po.bestNodeIdCostToGoal)
             {
                 po.bestNodeIdCostToGoal = costToGoal;
@@ -444,21 +458,19 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
 
     // A* ended, now collect the result:
     // ----------------------------------------
-    const auto [foundPath, pathEdges] =
-        tree.backtrack_path(nodeGoal.id.value());
-    bool foundPathValid = true;
-
-    for (const auto& step : foundPath)
-    {
-        if (step.cost_ == std::numeric_limits<distance_t>::max())
-        {
-            foundPathValid = false;
-            break;
-        }
-    }
-    po.success = foundPathValid;
+#if 0  // debug: dump tree
+    tree.visitBreadthFirst(
+        tree.root, [](const TNodeID parent, const auto& edgeToChild,
+                      const size_t depthLevel) {
+            const MoveEdgeSE2_TPS& e = edgeToChild.data;
+
+            std::cout << "tree level #" << depthLevel << ": parent=" << parent
+                      << " edgeToChild: " << e.asString() << "\n";
+        });
+#endif
 
-    po.pathCost = tree.nodes().at(nodeGoal.id.value()).cost_;
+    po.success  = po.goalNodeId == po.bestNodeId;
+    po.pathCost = tree.nodes().at(po.bestNodeId).cost_;
 
     po.computationTime = mrpt::Clock::nowDouble() - planInitTime;
 

share/mvsim-demo-astar-planner-params.yaml

@@ -1,9 +1,13 @@
 %YAML 1.2
 ---
+# It is safe to use coarse lattice values, since a post-processing path
+# refining stage will normally improve the result, with a very reduced overall
+# time cost:
 grid_resolution_xy: 0.40    # [meters]
 grid_resolution_yaw: 25.0   # [deg]
 
 # find_feasible_paths_to_neighbors() params:
+# Note: path refining will always use *all* PTG trajectories, despite this value
 max_ptg_trajectories_to_explore: 13
 max_ptg_speeds_to_explore: 3
 ptg_sample_timestamps: [0.5, 1.5, 4.0]  # seconds