From fe24a1ec21bfdbff59afee12b968210d39ea2922 Mon Sep 17 00:00:00 2001 From: Maksim Derbasov Date: Tue, 31 Dec 2024 01:19:10 +0900 Subject: [PATCH] Enable packing w/o padding for pointclouds Signed-off-by: Maksim Derbasov --- src/DepthCameraSensor.cc | 6 +----- src/GpuLidarSensor.cc | 7 +------ src/RgbdCameraSensor.cc | 6 +----- test/integration/gpu_lidar_sensor.cc | 6 +++--- test/integration/rgbd_camera.cc | 2 +- 5 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 9623ba0c..9b11d5f5 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -439,14 +439,10 @@ bool DepthCameraSensor::CreateCamera() std::placeholders::_4, std::placeholders::_5)); // Initialize the point message. - // \todo(anyone) The true value in the following function call forces - // the xyz and rgb fields to be aligned to memory boundaries. This is need - // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory - // alignment should be configured. msgs::InitPointCloudPacked( this->dataPtr->pointMsg, this->OpticalFrameId(), - true, + false, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 83e44d0d..684c2b14 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -129,12 +129,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) } // Initialize the point message. - // \todo(anyone) The true value in the following function call forces - // the xyz and rgb fields to be aligned to memory boundaries. This is need - // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory - // alignment should be configured. This same problem is in the - // RgbdCameraSensor. - msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true, + msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), false, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"intensity", msgs::PointCloudPacked::Field::FLOAT32}, {"ring", msgs::PointCloudPacked::Field::UINT16}}); diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 251ef7b7..54024136 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -395,12 +395,8 @@ bool RgbdCameraSensor::CreateCameras() std::placeholders::_4, std::placeholders::_5)); // Initialize the point message. - // \todo(anyone) The true value in the following function call forces - // the xyz and rgb fields to be aligned to memory boundaries. This is need - // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory - // alignment should be configured. msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->OpticalFrameId(), - true, + false, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc index 102d2389..d31bcd11 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -431,10 +431,10 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_EQ(static_cast(vertSamples), pointMsgs.back().height()); EXPECT_EQ(static_cast(horzSamples), pointMsgs.back().width()); EXPECT_FALSE(pointMsgs.back().is_bigendian()); - EXPECT_EQ(32u, pointMsgs.back().point_step()); - EXPECT_EQ(32u * horzSamples, pointMsgs.back().row_step()); + EXPECT_EQ(18u, pointMsgs.back().point_step()); + EXPECT_EQ(18u * horzSamples, pointMsgs.back().row_step()); EXPECT_FALSE(pointMsgs.back().is_dense()); - EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size()); + EXPECT_EQ(18u * horzSamples * vertSamples, pointMsgs.back().data().size()); EXPECT_TRUE(pointMsgs.back().has_header()); EXPECT_LT(1, pointMsgs.back().header().data().size()); diff --git a/test/integration/rgbd_camera.cc b/test/integration/rgbd_camera.cc index bc1fd184..aa936395 100644 --- a/test/integration/rgbd_camera.cc +++ b/test/integration/rgbd_camera.cc @@ -475,7 +475,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( { // init the point cloud msg to be filled msgs::PointCloudPacked pointsMsg; - msgs::InitPointCloudPacked(pointsMsg, "depth2Image", true, + msgs::InitPointCloudPacked(pointsMsg, "depth2Image", false, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); pointsMsg.set_width(rgbdSensor->ImageWidth());