Fix voxel visualization (#213)

* fix voxel viz * generalize to any voxel size * Update src/spatio_temporal_voxel_grid.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
SteveMacenski · Oct 1, 2021 · 7bf1e12 · 7bf1e12
1 parent be3a2d3
commit 7bf1e12
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp
@@ -355,13 +355,14 @@ void SpatioTemporalVoxelGrid::GetOccupancyPointCloud(
   sensor_msgs::PointCloud2Iterator<float> iter_y(*pc2, "y");
   sensor_msgs::PointCloud2Iterator<float> iter_z(*pc2, "z");
 
+  const double & center_offset = _voxel_size / 2.0;
   for (std::vector<geometry_msgs::msg::Point32>::iterator it =
     _grid_points->begin();
     it != _grid_points->end(); ++it)
   {
     const geometry_msgs::msg::Point32 & pt = *it;
-    *iter_x = pt.x;
-    *iter_y = pt.y;
+    *iter_x = pt.x + center_offset;
+    *iter_y = pt.y + center_offset;
     *iter_z = pt.z;
     ++iter_x; ++iter_y; ++iter_z;
   }