matched meta-data to the message's memory layout

Author: exo-core

src/k4a_ros_device.cpp

@@ -666,8 +666,9 @@ k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, sensor_msg
 k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image,
                                                sensor_msgs::PointCloud2Ptr& point_cloud)
 {
-  point_cloud->height = pointcloud_image.get_height_pixels();
-  point_cloud->width = pointcloud_image.get_width_pixels();
+  // Flip height and width to match expected memory layout
+  point_cloud->height = pointcloud_image.get_width_pixels();
+  point_cloud->width = pointcloud_image.get_height_pixels();
   point_cloud->is_dense = false;
   point_cloud->is_bigendian = false;
 
@@ -693,8 +694,9 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag
   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->height = pointcloud_image.get_width_pixels();
+  point_cloud->width = pointcloud_image.get_height_pixels();
+  point_cloud->row_step = pointcloud_image.get_height_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();
@@ -728,8 +730,9 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag
 
 k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::PointCloud2Ptr& point_cloud)
 {
-  point_cloud->height = pointcloud_image.get_height_pixels();
-  point_cloud->width = pointcloud_image.get_width_pixels();
+  // Flip height and width to match expected memory layout
+  point_cloud->height = pointcloud_image.get_width_pixels();
+  point_cloud->width = pointcloud_image.get_height_pixels();
   point_cloud->is_dense = false;
   point_cloud->is_bigendian = false;
 
@@ -745,8 +748,9 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se
   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->height = pointcloud_image.get_width_pixels();
+  point_cloud->width = pointcloud_image.get_height_pixels();
+  point_cloud->row_step = pointcloud_image.get_height_pixels() * point_cloud->point_step;
 
   const int16_t* point_cloud_buffer = reinterpret_cast<const int16_t*>(pointcloud_image.get_buffer());