include bug fix for nav2_smac_planner (#5198)

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>

Author: stevedanomodolor

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -112,14 +112,15 @@ class AnalyticExpansion
 
   /**
    * @brief Refined analytic path from the current node to the goal
-   * @param current_node The node to start the analytic path from
+   * @param node The node to start the analytic path from. Node head may
+   * change as a result of refinement
    * @param goal_node The goal node to plan to
    * @param getter The function object that gets valid nodes from the graph
    * @param analytic_nodes The set of analytic nodes to refine
    * @return The score of the refined path
    */
   float refineAnalyticPath(
-    const NodePtr & current_node,
+    NodePtr & node,
     const NodePtr & goal_node,
     const NodeGetter & getter,
     AnalyticExpansionNodes & analytic_nodes);

nav2_smac_planner/src/analytic_expansion.cpp

@@ -66,6 +66,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     AnalyticExpansionNodes current_best_analytic_nodes = {};
     NodePtr current_best_goal = nullptr;
+    NodePtr current_best_node = nullptr;
     float current_best_score = std::numeric_limits<float>::max();
 
     closest_distance = std::min(
@@ -97,13 +98,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           current_node->motion_table.state_space);
         if (!analytic_nodes.empty()) {
           found_valid_expansion = true;
-          bool score = refineAnalyticPath(
-            current_node, current_goal_node, getter, analytic_nodes);
+          NodePtr node = current_node;
+          float score = refineAnalyticPath(
+            node, current_goal_node, getter, analytic_nodes);
           // Update the best score if we found a better path
           if (score < current_best_score) {
             current_best_analytic_nodes = analytic_nodes;
             current_best_goal = current_goal_node;
             current_best_score = score;
+            current_best_node = node;
           }
         }
       }
@@ -116,13 +119,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             current_node, current_goal_node, getter,
             current_node->motion_table.state_space);
           if (!analytic_nodes.empty()) {
-            bool score = refineAnalyticPath(
-              current_node, current_goal_node, getter, analytic_nodes);
+            NodePtr node = current_node;
+            float score = refineAnalyticPath(
+              node, current_goal_node, getter, analytic_nodes);
             // Update the best score if we found a better path
             if (score < current_best_score) {
               current_best_analytic_nodes = analytic_nodes;
               current_best_goal = current_goal_node;
               current_best_score = score;
+              current_best_node = node;
             }
           }
         }
@@ -131,7 +136,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     if (!current_best_analytic_nodes.empty()) {
       return setAnalyticPath(
-        current_node, current_best_goal,
+        current_best_node, current_best_goal,
         current_best_analytic_nodes);
     }
     analytic_iterations--;
@@ -273,12 +278,11 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
 template<typename NodeT>
 float AnalyticExpansion<NodeT>::refineAnalyticPath(
-  const NodePtr & current_node,
+  NodePtr & node,
   const NodePtr & goal_node,
   const NodeGetter & getter,
   AnalyticExpansionNodes & analytic_nodes)
 {
-  NodePtr node = current_node;
   NodePtr test_node = node;
   AnalyticExpansionNodes refined_analytic_nodes;
   for (int i = 0; i < 8; i++) {
@@ -407,7 +411,7 @@ getAnalyticPath(
 
 template<>
 float AnalyticExpansion<Node2D>::refineAnalyticPath(
-  const NodePtr &,
+  NodePtr &,
   const NodePtr &,
   const NodeGetter &,
   AnalyticExpansionNodes &)

nav2_smac_planner/test/test_a_star.cpp

@@ -141,7 +141,9 @@ TEST(AStarTest, test_a_star_2d)
   int dummy_int2 = 0;
   EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {},
   nullptr, dummy_int1, dummy_int2), nullptr);
-  EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr,
+
+  nav2_smac_planner::Node2D * start = nullptr;
+  EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr,
     analytic_expansion_nodes), std::numeric_limits<float>::max());
   nav2_smac_planner::AnalyticExpansion<nav2_smac_planner::Node2D>::AnalyticExpansionNodes
     expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr);