From 82c3bb6502498da1981a604c45b0bd118b51b410 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 15 Mar 2024 11:25:57 -0700 Subject: [PATCH] Fixing a typo in Smac Heuristic (#4184) Signed-off-by: Steve Macenski Signed-off-by: enricosutera --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 2e81a5f47c..aa1773e682 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -658,10 +658,10 @@ float NodeHybrid::getObstacleHeuristic( if (existing_cost <= 0.0f) { if (motion_table.use_quadratic_cost_penalty) { travel_cost = - (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 64516.0f)); // 254^2 + (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 } else { travel_cost = - ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 254.0f)); + ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); } new_cost = c_cost + travel_cost;