diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp
index 158a359870..14a901aded 100644
--- a/nav2_smac_planner/src/node_2d.cpp
+++ b/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(
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
index ebbe357559..24b32f527b 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/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(
diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py
index 4cfcf721e3..c4b556e5d7 100644
--- a/tools/planner_benchmarking/metrics.py
+++ b/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