resizing point cloud data after initializing iterators was a bad idea

src/k4a_ros_device.cpp

@@ -680,6 +680,11 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag
   sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud);
   pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
 
+  point_cloud->height = pointcloud_image.get_height_pixels();
+  point_cloud->width = pointcloud_image.get_width_pixels();
+  point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step;
+  point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step);
+
   sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud, "x");
   sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud, "y");
   sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud, "z");
@@ -688,13 +693,6 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag
   sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*point_cloud, "g");
   sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*point_cloud, "b");
 
-  pcd_modifier.resize(point_count);
-
-  // Restore actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1
-  point_cloud->height = pointcloud_image.get_height_pixels();
-  point_cloud->width = pointcloud_image.get_width_pixels();
-  point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step;
-
   const int16_t* point_cloud_buffer = reinterpret_cast<const int16_t*>(pointcloud_image.get_buffer());
   const uint8_t* color_buffer = color_image.get_buffer();
 
@@ -735,16 +733,14 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se
   sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud);
   pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
 
-  sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud, "z");
-
-  pcd_modifier.resize(point_count);
-
-  // Set actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1
   point_cloud->height = pointcloud_image.get_height_pixels();
   point_cloud->width = pointcloud_image.get_width_pixels();
   point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step;
+  point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud, "z");
 
   const int16_t* point_cloud_buffer = reinterpret_cast<const int16_t*>(pointcloud_image.get_buffer());