Return early from edge interpolation for zero-length edges (#5453)

* Return early from edge interpolation for zero-length edges

Signed-off-by: Emerson Knapp <[email protected]>

* Move the check and add a test

Signed-off-by: Emerson Knapp <[email protected]>

---------

Signed-off-by: Emerson Knapp <[email protected]>

Author: emersonknapp

nav2_route/src/path_converter.cpp

@@ -141,6 +141,11 @@ void PathConverter::interpolateEdge(
   // Find number of points to populate by given density
   const float mag = hypotf(x1 - x0, y1 - y0);
   const unsigned int num_pts = ceil(mag / density_);
+  // For zero-length edges, we can just push the start point and return
+  if (num_pts < 1) {
+    return;
+  }
+
   const float iterpolated_dist = mag / num_pts;
 
   // Find unit vector direction

nav2_route/test/test_path_converter.cpp

@@ -163,3 +163,15 @@ TEST(PathConverterTest, test_path_converter_interpolation)
         poses[i].pose.position.y - poses[i + 1].pose.position.y), 0.05);
   }
 }
+
+TEST(PathConverterTest, test_path_converter_zero_length_edge)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("edge_scorer_test");
+  PathConverter converter;
+  converter.configure(node);
+
+  float x0 = 10.0, y0 = 10.0, x1 = 10.0, y1 = 10.0;
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  converter.interpolateEdge(x0, y0, x1, y1, poses);
+  ASSERT_TRUE(poses.empty());
+}