Fixing a typo in Smac Heuristic (ros-navigation#4184)

Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: enricosutera <[email protected]>
enricosutera · May 19, 2024 · 82c3bb6 · 82c3bb6
1 parent 4a23ff2
commit 82c3bb6
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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;