euclidean distance: by default use 2d mode (#2865)

Signed-off-by: Vinny Ruia <[email protected]>

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -43,48 +43,64 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
 }
 
 /**
- * @brief Get the L2 distance between 2 geometry_msgs::Points
+ * @brief Get the euclidean distance between 2 geometry_msgs::Points
  * @param pos1 First point
  * @param pos1 Second point
+ * @param is_3d True if a true L2 distance is desired (default false)
  * @return double L2 distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::Point & pos1,
-  const geometry_msgs::msg::Point & pos2)
+  const geometry_msgs::msg::Point & pos2,
+  const bool is_3d = false)
 {
   double dx = pos1.x - pos2.x;
   double dy = pos1.y - pos2.y;
-  double dz = pos1.z - pos2.z;
-  return std::hypot(dx, dy, dz);
+
+  if (is_3d) {
+    double dz = pos1.z - pos2.z;
+    return std::hypot(dx, dy, dz);
+  }
+
+  return std::hypot(dx, dy);
 }
 
 /**
  * @brief Get the L2 distance between 2 geometry_msgs::Poses
  * @param pos1 First pose
  * @param pos1 Second pose
- * @return double L2 distance
+ * @param is_3d True if a true L2 distance is desired (default false)
+ * @return double euclidean distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::Pose & pos1,
-  const geometry_msgs::msg::Pose & pos2)
+  const geometry_msgs::msg::Pose & pos2,
+  const bool is_3d = false)
 {
   double dx = pos1.position.x - pos2.position.x;
   double dy = pos1.position.y - pos2.position.y;
-  double dz = pos1.position.z - pos2.position.z;
-  return std::hypot(dx, dy, dz);
+
+  if (is_3d) {
+    double dz = pos1.position.z - pos2.position.z;
+    return std::hypot(dx, dy, dz);
+  }
+
+  return std::hypot(dx, dy);
 }
 
 /**
  * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
  * @param pos1 First pose
  * @param pos1 Second pose
+ * @param is_3d True if a true L2 distance is desired (default false)
  * @return double L2 distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::PoseStamped & pos1,
-  const geometry_msgs::msg::PoseStamped & pos2)
+  const geometry_msgs::msg::PoseStamped & pos2,
+  const bool is_3d = false)
 {
-  return euclidean_distance(pos1.pose, pos2.pose);
+  return euclidean_distance(pos1.pose, pos2.pose, is_3d);
 }
 
 /**

nav2_util/test/test_geometry_utils.cpp

@@ -22,7 +22,7 @@
 using nav2_util::geometry_utils::euclidean_distance;
 using nav2_util::geometry_utils::calculate_path_length;
 
-TEST(GeometryUtils, euclidean_distance_point)
+TEST(GeometryUtils, euclidean_distance_point_3d)
 {
   geometry_msgs::msg::Point point1;
   point1.x = 3.0;
@@ -34,10 +34,40 @@ TEST(GeometryUtils, euclidean_distance_point)
   point2.y = 2.0;
   point2.z = 3.0;
 
-  ASSERT_NEAR(euclidean_distance(point1, point2), 2.82843, 1e-5);
+  ASSERT_NEAR(euclidean_distance(point1, point2, true), 2.82843, 1e-5);
 }
 
-TEST(GeometryUtils, euclidean_distance_pose)
+TEST(GeometryUtils, euclidean_distance_point_2d)
+{
+  geometry_msgs::msg::Point point1;
+  point1.x = 3.0;
+  point1.y = 2.0;
+  point1.z = 1.0;
+
+  geometry_msgs::msg::Point point2;
+  point2.x = 1.0;
+  point2.y = 2.0;
+  point2.z = 3.0;
+
+  ASSERT_NEAR(euclidean_distance(point1, point2), 2.0, 1e-5);
+}
+
+TEST(GeometryUtils, euclidean_distance_pose_3d)
+{
+  geometry_msgs::msg::Pose pose1;
+  pose1.position.x = 7.0;
+  pose1.position.y = 4.0;
+  pose1.position.z = 3.0;
+
+  geometry_msgs::msg::Pose pose2;
+  pose2.position.x = 17.0;
+  pose2.position.y = 6.0;
+  pose2.position.z = 2.0;
+
+  ASSERT_NEAR(euclidean_distance(pose1, pose2, true), 10.24695, 1e-5);
+}
+
+TEST(GeometryUtils, euclidean_distance_pose_2d)
 {
   geometry_msgs::msg::Pose pose1;
   pose1.position.x = 7.0;
@@ -49,7 +79,7 @@ TEST(GeometryUtils, euclidean_distance_pose)
   pose2.position.y = 6.0;
   pose2.position.z = 2.0;
 
-  ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5);
+  ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.19804, 1e-5);
 }
 
 TEST(GeometryUtils, calculate_path_length)