Fix length reduction conversions

ros-navigation · Jul 20, 2021 · d97bf55 · d97bf55
1 parent bfe33be
commit d97bf55
Showing 1 changed file with 3 additions and 3 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
@@ -267,7 +267,7 @@ class VoxelGrid
       int error_y = abs_dx / 2;
       int error_z = abs_dx / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dx) - abs(min_shift_x);
+      length = (unsigned int)abs(scale * (x1 - min_x0));
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z,
         offset_dx, offset_dy, offset_dz, offset, z_mask, length);
@@ -279,7 +279,7 @@ class VoxelGrid
       int error_x = abs_dy / 2;
       int error_z = abs_dy / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dy) - abs(min_shift_y);
+      length = (unsigned int)abs(scale * (y1 - min_y0));
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z,
         offset_dy, offset_dx, offset_dz, offset, z_mask, length);
@@ -290,7 +290,7 @@ class VoxelGrid
     int error_x = abs_dz / 2;
     int error_y = abs_dz / 2;
     // Since initial point has been updated above, subtracting min_length from the total length
-    length = (unsigned int)(scale * abs_dz) - abs(min_shift_z);
+    length = (unsigned int)abs(scale * (z1 - min_z0));
 
     bresenham3D(
       at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,