From 45811d591aa4dd70d18d35789e0fc662e4b280a8 Mon Sep 17 00:00:00 2001 From: wang-fire Date: Wed, 5 Nov 2025 19:33:20 +0800 Subject: [PATCH] Fix missing cost consideration in first expansion of SmacPlannerHybrid (#5626) Signed-off-by: wang-fire --- nav2_smac_planner/src/node_hybrid.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index bbbae87ae7c..a9b1ec73a54 100644 --- 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::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::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 {