Revert "include bug fix for nav2_smac_planner (#5198)"

This reverts commit 6a74ba6.

Author: SteveMacenski

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -112,15 +112,14 @@ class AnalyticExpansion
 
   /**
    * @brief Refined analytic path from the current node to the goal
-   * @param node The node to start the analytic path from. Node head may
-   * change as a result of refinement
+   * @param current_node The node to start the analytic path from
    * @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(
-    NodePtr & node,
+    const NodePtr & current_node,
     const NodePtr & goal_node,
     const NodeGetter & getter,
     AnalyticExpansionNodes & analytic_nodes);

nav2_smac_planner/src/analytic_expansion.cpp

@@ -66,7 +66,6 @@ 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(
@@ -98,15 +97,13 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           current_node->motion_table.state_space);
         if (!analytic_nodes.empty()) {
           found_valid_expansion = true;
-          NodePtr node = current_node;
-          float score = refineAnalyticPath(
-            node, current_goal_node, getter, analytic_nodes);
+          bool score = refineAnalyticPath(
+            current_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;
           }
         }
       }
@@ -119,15 +116,13 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             current_node, current_goal_node, getter,
             current_node->motion_table.state_space);
           if (!analytic_nodes.empty()) {
-            NodePtr node = current_node;
-            float score = refineAnalyticPath(
-              node, current_goal_node, getter, analytic_nodes);
+            bool score = refineAnalyticPath(
+              current_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;
             }
           }
         }
@@ -136,7 +131,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     if (!current_best_analytic_nodes.empty()) {
       return setAnalyticPath(
-        current_best_node, current_best_goal,
+        current_node, current_best_goal,
         current_best_analytic_nodes);
     }
     analytic_iterations--;
@@ -278,11 +273,12 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
 template<typename NodeT>
 float AnalyticExpansion<NodeT>::refineAnalyticPath(
-  NodePtr & node,
+  const NodePtr & current_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++) {
@@ -411,7 +407,7 @@ getAnalyticPath(
 
 template<>
 float AnalyticExpansion<Node2D>::refineAnalyticPath(
-  NodePtr &,
+  const NodePtr &,
   const NodePtr &,
   const NodeGetter &,
   AnalyticExpansionNodes &)

nav2_smac_planner/test/test_a_star.cpp

@@ -141,9 +141,7 @@ TEST(AStarTest, test_a_star_2d)
   int dummy_int2 = 0;
   EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {},
   nullptr, dummy_int1, dummy_int2), nullptr);
-
-  nav2_smac_planner::Node2D * start = nullptr;
-  EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr,
+  EXPECT_EQ(expander.refineAnalyticPath(nullptr, 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);