Fixing the issue ros-navigation#2781: raytraceLine with same start an…

…d end point (ros-navigation#2784)

* Fixing the issue ros-navigation#2781: raytraceLine with same start and end point no longer causes segmentation fault

* Some whitespace modifications to make the code pass release_test

* Add testcase for raytraceLine the same point

Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>

nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp

@@ -232,12 +232,22 @@ class VoxelGrid
     if ((unsigned int)(dist) < min_length) {
       return;
     }
-    double scale = std::min(1.0, max_length / dist);
-
-    // Updating starting point to the point at distance min_length from the initial point
-    double min_x0 = x0 + (x1 - x0) / dist * min_length;
-    double min_y0 = y0 + (y1 - y0) / dist * min_length;
-    double min_z0 = z0 + (z1 - z0) / dist * min_length;
+    double scale, min_x0, min_y0, min_z0;
+    if (dist > 0.0) {
+      scale = std::min(1.0, max_length / dist);
+
+      // Updating starting point to the point at distance min_length from the initial point
+      min_x0 = x0 + (x1 - x0) / dist * min_length;
+      min_y0 = y0 + (y1 - y0) / dist * min_length;
+      min_z0 = z0 + (z1 - z0) / dist * min_length;
+    } else {
+      // dist can be 0 if [x0, y0, z0]==[x1, y1, z1].
+      // In this case only this voxel should be processed.
+      scale = 1.0;
+      min_x0 = x0;
+      min_y0 = y0;
+      min_z0 = z0;
+    }
 
     int dx = int(x1) - int(min_x0);  // NOLINT
     int dy = int(y1) - int(min_y0);  // NOLINT

nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp

@@ -49,6 +49,10 @@ class TestVoxel
     ASSERT_TRUE(off < size_);
     data_[off] = val;
   }
+  inline unsigned int operator()(unsigned int off)
+  {
+    return data_[off];
+  }
 
 private:
   uint32_t * data_;
@@ -122,6 +126,29 @@ TEST(voxel_grid, bresenham3DBoundariesCheck)
   }
 }
 
+TEST(voxel_grid, bresenham3DSamePoint)
+{
+  const int sz_x = 60;
+  const int sz_y = 60;
+  const int sz_z = 2;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 0;
+  nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z);
+  TestVoxel tv(vg.getData(), sz_x, sz_y);
+
+  // Initial point
+  const double x0 = 2.2;
+  const double y0 = 3.8;
+  const double z0 = 0.4;
+
+  unsigned int offset = int(y0) * sz_x + int(x0);
+  unsigned int val_before = tv(offset);
+  // Same point to check
+  vg.raytraceLine(tv, x0, y0, z0, x0, y0, z0, max_length, min_length);
+  unsigned int val_after = tv(offset);
+  ASSERT_FALSE(val_before == val_after);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);