From 2fccc7c886a062bc3ca42ac300b536e038bce32e Mon Sep 17 00:00:00 2001 From: wang-fire Date: Sat, 25 Oct 2025 22:08:05 +0800 Subject: [PATCH 1/2] Fix missing cost consideration in first expansion of SmacPlannerHybrid (#5626) Signed-off-by: wang-fire --- nav2_smac_planner/src/node_hybrid.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 24b32f527ba..e3f0c6b0d10 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -309,11 +309,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; - } - float travel_cost = 0.0; float travel_cost_raw = NodeHybrid::travel_distance_cost * From b68f3947e54795a8cf12029d566ed082b848f3df Mon Sep 17 00:00:00 2001 From: wang-fire Date: Mon, 27 Oct 2025 22:18:38 +0800 Subject: [PATCH 2/2] Fix nullptr child handling in getTraversalCost() to prevent segmentation fault in smac planner Signed-off-by: wang-fire --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index f088f39bc50..d6317432bf0 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -390,6 +390,10 @@ bool NodeHybrid::isNodeValid( float NodeHybrid::getTraversalCost(const NodePtr & child) { + if (child == nullptr) { + return 0.0f; + } + const float normalized_cost = child->getCost() / 252.0f; if (std::isnan(normalized_cost)) { throw std::runtime_error(