From b45381d34941982750af64cf3db4148bff614e80 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 22 Sep 2022 15:02:44 -0700
Subject: [PATCH] removing hypotf from smac planner heuristic computation
 (#3217)

* removing hypotf

* swapping to node2d sqrt
---
 nav2_smac_planner/src/node_2d.cpp     | 4 +++-
 nav2_smac_planner/src/node_hybrid.cpp | 6 +++---
 tools/planner_benchmarking/metrics.py | 2 ++
 3 files changed, 8 insertions(+), 4 deletions(-)

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