From 26c7d21b00867742c751649a63cc882a3ed8f99f Mon Sep 17 00:00:00 2001 From: Jan Weber Date: Tue, 25 Jan 2022 10:47:41 +0100 Subject: [PATCH 1/3] Fixing the issue #2781: raytraceLine with same start and end point no longer causes segmentation fault --- .../include/nav2_voxel_grid/voxel_grid.hpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index 9e5ea94101..9e48510c90 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/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; + } + // dist can be 0 if [x0, y0, z0]==[x1, y1, z1]. In this case only this voxel should be processed. + else{ + 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 From 570bf02049518d5c28ba1987f106d9f4c2ac9369 Mon Sep 17 00:00:00 2001 From: Jan Weber Date: Tue, 25 Jan 2022 17:38:59 +0100 Subject: [PATCH 2/3] Some whitespace modifications to make the code pass release_test --- nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index 9e48510c90..baff10d684 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -233,16 +233,16 @@ class VoxelGrid return; } double scale, min_x0, min_y0, min_z0; - if(dist > 0.0){ + 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; - } - // dist can be 0 if [x0, y0, z0]==[x1, y1, z1]. In this case only this voxel should be processed. - else{ + } 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; From be5d43e9ae564f9a616ed62ace40b9849807508f Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Wed, 9 Feb 2022 18:23:17 +0300 Subject: [PATCH 3/3] Add testcase for raytraceLine the same point --- .../test/voxel_grid_bresenham_3d.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp index 10ff23dd87..49aa5830f0 100644 --- a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp +++ b/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);