Fix missing cost consideration in first expansion of SmacPlannerHybrid (#5626)

wang-fire · wang-fire · commit da8a4e1b8a10 · 2025-11-05T19:07:22.000+08:00
Signed-off-by: wang-fire &lt;wdf19921004@gmail.com&gt;
diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp
@@ -48,9 +48,9 @@ class GoalManager
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
 
-   /**
-   * @brief Constructor: Initializes empty goal state. sets and coordinate lists.
-   */
+  /**
+  * @brief Constructor: Initializes empty goal state. sets and coordinate lists.
+  */
   GoalManager()
   : _goals_set(NodeSet()),
     _goals_state(GoalStateVector()),
@@ -134,7 +134,7 @@ class GoalManager
     const auto size_x = collision_checker->getCostmap()->getSizeInCellsX();
     const auto size_y = collision_checker->getCostmap()->getSizeInCellsY();
 
-    auto getIndexFromPoint = [&size_x] (const Coordinates & point) {
+    auto getIndexFromPoint = [&size_x](const Coordinates & point) {
         unsigned int index = 0;
 
         const auto mx = static_cast<unsigned int>(point.x);
@@ -197,8 +197,9 @@ class GoalManager
   {
     // Make sure that there was a  goal clear before this was run
     if (!_goals_set.empty() || !_goals_coordinate.empty()) {
-      throw std::runtime_error("Goal set should be cleared before calling "
-        "removeinvalidgoals");
+      throw std::runtime_error(
+              "Goal set should be cleared before calling "
+              "removeinvalidgoals");
     }
     for (unsigned int i = 0; i < _goals_state.size(); i++) {
       if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) ||
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -350,7 +350,8 @@ bool AStarAlgorithm<NodeT>::createPath(
   }
 
   NodeVector coarse_check_goals, fine_check_goals;
-  _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals,
+  _goal_manager.prepareGoalsForAnalyticExpansion(
+    coarse_check_goals, fine_check_goals,
     _coarse_search_resolution);
 
   // 0) Add starting point to the open set
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -349,10 +349,10 @@ float AnalyticExpansion<NodeT>::refineAnalyticPath(
 
       float score = 0.0;
       float normalized_cost = 0.0;
-    // Analytic expansions are consistently spaced
+      // Analytic expansions are consistently spaced
       const float distance = hypotf(
-      expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x,
-      expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y);
+        expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x,
+        expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y);
       const float & weight = expansion.nodes[0].node->motion_table.cost_penalty;
       for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) {
         normalized_cost = iter->node->getCost() / 252.0f;
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
@@ -116,7 +116,7 @@ bool GridCollisionChecker::inCollision(
 
   // Assumes setFootprint already set
   center_cost_ = static_cast<float>(costmap_->getCost(
-    static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
+      static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
   if (!footprint_is_radius_) {
     // if footprint, then we check for the footprint's points, but first see
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
@@ -397,11 +397,6 @@ float NodeHybrid::getTraversalCost(const NodePtr & child)
             "cost without a known SE2 collision cost!");
   }
 
-  // this is the first node
-  if (getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max()) {
-    return NodeHybrid::travel_distance_cost;
-  }
-
   const TurnDirection & child_turn_dir = child->getTurnDirection();
   float travel_cost_raw = motion_table.travel_costs[child->getMotionPrimitiveIndex()];
   float travel_cost = 0.0;
@@ -419,7 +414,9 @@ float NodeHybrid::getTraversalCost(const NodePtr & child)
     // New motion is a straight motion, no additional costs to be applied
     travel_cost = travel_cost_raw;
   } else {
-    if (getTurnDirection() == child_turn_dir) {
+    if (getTurnDirection() == child_turn_dir ||
+      getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max())
+    {
       // Turning motion but keeps in same direction: encourages to commit to turning if starting it
       travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
     } else {
diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
@@ -230,8 +230,9 @@ bool NodeLattice::isNodeValid(
   // Check primitive end pose
   // Convert grid quantization of primitives to radians, then collision checker quantization
   static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();
-  const double angle = std::fmod(motion_table.getAngleFromBin(this->pose.theta),
-      2.0 * M_PI) / bin_size;
+  const double angle = std::fmod(
+    motion_table.getAngleFromBin(this->pose.theta),
+    2.0 * M_PI) / bin_size;
   if (collision_checker->inCollision(
       this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown))
   {
diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -362,7 +362,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
   for (auto parameter : parameters) {
     const auto & param_type = parameter.get_type();
     const auto & param_name = parameter.get_name();
-    if(param_name.find(_name + ".") != 0) {
+    if (param_name.find(_name + ".") != 0) {
       continue;
     }
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -404,10 +404,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   // Set starting point, in A* bin search coordinates
   float mx_start, my_start, mx_goal, my_goal;
   if (!costmap->worldToMapContinuous(
-    start.pose.position.x,
-    start.pose.position.y,
-    mx_start,
-    my_start))
+      start.pose.position.x,
+      start.pose.position.y,
+      mx_start,
+      my_start))
   {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
@@ -428,10 +428,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   // Set goal point, in A* bin search coordinates
   if (!costmap->worldToMapContinuous(
-    goal.pose.position.x,
-    goal.pose.position.y,
-    mx_goal,
-    my_goal))
+      goal.pose.position.x,
+      goal.pose.position.y,
+      mx_goal,
+      my_goal))
   {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
@@ -447,7 +447,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   }
   unsigned int goal_orientation_bin_int =
     static_cast<unsigned int>(goal_orientation_bin);
-  _a_star->setGoal(mx_goal, my_goal, static_cast<unsigned int>(goal_orientation_bin_int),
+  _a_star->setGoal(
+    mx_goal, my_goal, static_cast<unsigned int>(goal_orientation_bin_int),
     _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
@@ -625,7 +626,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
   for (auto parameter : parameters) {
     const auto & param_type = parameter.get_type();
     const auto & param_name = parameter.get_name();
-    if(param_name.find(_name + ".") != 0 && param_name != "resolution") {
+    if (param_name.find(_name + ".") != 0 && param_name != "resolution") {
       continue;
     }
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
@@ -756,8 +757,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         if (_angle_quantizations % _coarse_search_resolution != 0) {
           RCLCPP_WARN(
             _logger,
-              "coarse iteration should be an increment of the "
-              "number of angular bins configured. Disabling course research!"
+            "coarse iteration should be an increment of the "
+            "number of angular bins configured. Disabling course research!"
           );
           _coarse_search_resolution = 1;
         }
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -329,10 +329,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   // Set starting point, in A* bin search coordinates
   float mx_start, my_start, mx_goal, my_goal;
   if (!_costmap->worldToMapContinuous(
-    start.pose.position.x,
-    start.pose.position.y,
-    mx_start,
-    my_start))
+      start.pose.position.x,
+      start.pose.position.y,
+      mx_start,
+      my_start))
   {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
@@ -344,10 +344,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
 
   // Set goal point, in A* bin search coordinates
   if (!_costmap->worldToMapContinuous(
-    goal.pose.position.x,
-    goal.pose.position.y,
-    mx_goal,
-    my_goal))
+      goal.pose.position.x,
+      goal.pose.position.y,
+      mx_goal,
+      my_goal))
   {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
@@ -357,7 +357,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
     NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation));
   _a_star->setGoal(
     mx_goal, my_goal, goal_bin,
-      _goal_heading_mode, _coarse_search_resolution);
+    _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -546,7 +546,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
   for (auto parameter : parameters) {
     const auto & param_type = parameter.get_type();
     const auto & param_name = parameter.get_name();
-    if(param_name.find(_name + ".") != 0) {
+    if (param_name.find(_name + ".") != 0) {
       continue;
     }
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
@@ -638,8 +638,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         if (_metadata.number_of_headings % _coarse_search_resolution != 0) {
           RCLCPP_WARN(
             _logger,
-              "coarse iteration should be an increment of the number<"
-              " of angular bins configured. Disabling course research!"
+            "coarse iteration should be an increment of the number<"
+            " of angular bins configured. Disabling course research!"
           );
           _coarse_search_resolution = 1;
         }
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
@@ -64,8 +64,9 @@ bool Smoother::smooth(
   bool success = true, reversing_segment;
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
-  std::vector<PathSegment> path_segments = nav2_util::findDirectionalPathSegments(path,
-      is_holonomic_);
+  std::vector<PathSegment> path_segments = nav2_util::findDirectionalPathSegments(
+    path,
+    is_holonomic_);
 
   for (unsigned int i = 0; i != path_segments.size(); i++) {
     if (path_segments[i].end - path_segments[i].start > 10) {
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
@@ -139,12 +139,16 @@ TEST(AStarTest, test_a_star_2d)
   EXPECT_EQ(expander.setAnalyticPath(nullptr, nullptr, analytic_expansion_nodes), nullptr);
   int dummy_int1 = 0;
   int dummy_int2 = 0;
-  EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {},
-  nullptr, dummy_int1, dummy_int2), nullptr);
+  EXPECT_EQ(
+    expander.tryAnalyticExpansion(
+      nullptr, {}, {}, {},
+      nullptr, dummy_int1, dummy_int2), nullptr);
 
   nav2_smac_planner::Node2D * start = nullptr;
-  EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr,
-    analytic_expansion_nodes), std::numeric_limits<float>::max());
+  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);
   EXPECT_EQ(expected_nodes.nodes.size(), 0);
@@ -490,15 +494,17 @@ TEST(AStarTest, test_goal_heading_mode)
     };
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
   EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), 2);
-  EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(),
+  EXPECT_EQ(
+    a_star.getGoalManager().getGoalsState().size(),
     a_star.getGoalManager().getGoalsCoordinates().size());
 
 
   // ALL_DIRECTION goal heading mode
   unsigned int coarse_search_resolution = 16;
   a_star.setCollisionChecker(checker.get());
   a_star.setStart(10u, 10u, 0u);
-  a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION,
+  a_star.setGoal(
+    80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION,
     coarse_search_resolution);
   EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution);
 
@@ -508,14 +514,15 @@ TEST(AStarTest, test_goal_heading_mode)
   unsigned int num_valid_goals = 0;
   auto goals_state = a_star.getGoalManager().getGoalsState();
   for (unsigned int i = 0; i < goals_state.size(); i++) {
-    if(goals_state[i].is_valid) {
+    if (goals_state[i].is_valid) {
       num_valid_goals++;
     }
   }
   EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_bins);
   EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_valid_goals);
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
-  EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() ==
+  EXPECT_TRUE(
+    a_star.getGoalManager().getGoalsState().size() ==
     a_star.getGoalManager().getGoalsCoordinates().size());
 
   // UNKNOWN goal heading mode
@@ -570,7 +577,7 @@ TEST(AStarTest, test_constants)
     nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp
@@ -215,7 +215,7 @@ TEST(collision_footprint, test_point_and_line_cost)
   delete costmap_;
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_costmap_downsampler.cpp b/nav2_smac_planner/test/test_costmap_downsampler.cpp
@@ -58,7 +58,7 @@ TEST(CostmapDownsampler, costmap_downsample_test)
   downsampler.resizeCostmap();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp
@@ -183,8 +183,9 @@ TEST(GoalManagerTest, test_goal_manager)
 
   // expect throw if we try to call removeinvalidgoals twice
   // without clearing the goal manager first
-  EXPECT_THROW(goal_manager.removeInvalidGoals(
-    tolerance, checker.get(), allow_unknow),
+  EXPECT_THROW(
+    goal_manager.removeInvalidGoals(
+      tolerance, checker.get(), allow_unknow),
     std::runtime_error
   );
 
@@ -193,7 +194,7 @@ TEST(GoalManagerTest, test_goal_manager)
 }
 
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp
@@ -150,7 +150,7 @@ TEST(Node2DTest, test_node_2d_neighbors)
   EXPECT_EQ(neighbors.size(), 0u);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp
@@ -43,7 +43,7 @@ TEST(NodeBasicTest, test_node_basic)
   EXPECT_EQ(node3.graph_node_ptr, nullptr);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp
@@ -411,7 +411,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp
@@ -388,7 +388,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
   delete costmap;
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp
@@ -151,7 +151,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) {
     results);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp

Original file line number	Diff line number	Diff line change
`@@ -350,7 +350,8 @@ bool AStarAlgorithm<NodeT>::createPath(`
`350`	`350`	`}`
`351`	`351`
`352`	`352`	`NodeVector coarse_check_goals, fine_check_goals;`
`353`		`- _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals,`
	`353`	`+ _goal_manager.prepareGoalsForAnalyticExpansion(`
	`354`	`+ coarse_check_goals, fine_check_goals,`
`354`	`355`	`_coarse_search_resolution);`
`355`	`356`
`356`	`357`	`// 0) Add starting point to the open set`
Original file line number	Diff line number	Diff line change
`@@ -362,7 +362,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete`
`362`	`362`	`for (auto parameter : parameters) {`
`363`	`363`	`const auto & param_type = parameter.get_type();`
`364`	`364`	`const auto & param_name = parameter.get_name();`
`365`		`- if(param_name.find(_name + ".") != 0) {`
	`365`	`+ if (param_name.find(_name + ".") != 0) {`
`366`	`366`	`continue;`
`367`	`367`	`}`
`368`	`368`	`if (param_type == ParameterType::PARAMETER_DOUBLE) {`