removing hypotf from smac planner heuristic computation (ros-navigati…

…on#3217)

* removing hypotf

* swapping to node2d sqrt

nav2_smac_planner/src/node_2d.cpp

@@ -86,7 +86,9 @@ float Node2D::getHeuristicCost(
 {
   // Using Moore distance as it more accurately represents the distances
   // even a Van Neumann neighborhood robot can navigate.
-  return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y);
+  auto dx = goal_coordinates.x - node_coords.x;
+  auto dy = goal_coordinates.y - node_coords.y;
+  return std::sqrt(dx * dx + dy * dy);
 }
 
 void Node2D::initMotionModel(

nav2_smac_planner/src/node_hybrid.cpp

@@ -381,9 +381,9 @@ inline float distanceHeuristic2D(
   const unsigned int idx, const unsigned int size_x,
   const unsigned int target_x, const unsigned int target_y)
 {
-  return std::hypotf(
-    static_cast<int>(idx % size_x) - static_cast<int>(target_x),
-    static_cast<int>(idx / size_x) - static_cast<int>(target_y));
+  int dx = static_cast<int>(idx % size_x) - static_cast<int>(target_x);
+  int dy = static_cast<int>(idx / size_x) - static_cast<int>(target_y);
+  return std::sqrt(dx * dx + dy * dy);
 }
 
 void NodeHybrid::resetObstacleHeuristic(

tools/planner_benchmarking/metrics.py

@@ -38,6 +38,8 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners):
         path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
         if path is not None:
             results.append(path)
+        else:
+            return results
     return results