From 684ed50d3b8cb593adb935ebf2bcf239b7b517dd Mon Sep 17 00:00:00 2001 From: Thomas Moinel Date: Tue, 21 Jul 2015 19:59:35 +0200 Subject: [PATCH 1/2] sensor_msgs: Generate optimal msg lentgth when both "xyz" and "rgb(a)" are given to PointCloud2Modifier. PointCloud2Modifier::setPointCloud2FieldsByString make PointCloud2 message with an optimal length by removing unnecessary trailling offset. It cut in half the msg length. --- .../include/sensor_msgs/impl/point_cloud2_iterator.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h index c8d7de8a..c66ae1ab 100644 --- a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +++ b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h @@ -201,6 +201,8 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...) va_list vl; va_start(vl, n_fields); int offset = 0; + bool has_xyz = false; + bool has_rgb = false; for (int i = 0; i < n_fields; ++i) { // Create the corresponding PointFields std::string @@ -211,16 +213,21 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...) offset = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, offset); offset = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, offset); offset = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, offset); - offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32); + has_xyz = true; } else if ((field_name == "rgb") || (field_name == "rgba")) { offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset); - offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32); + has_rgb = true; } else throw std::runtime_error("Field " + field_name + " does not exist"); } va_end(vl); + if (has_xyz && !has_rgb) + offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32); + else if (!has_xyz && has_rgb) + offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32); + // Resize the point cloud accordingly cloud_msg_.point_step = offset; cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step; From d4778fb06814e550fb51ba5243446d4dc063177f Mon Sep 17 00:00:00 2001 From: Thomas Moinel Date: Mon, 10 Aug 2015 15:48:08 +0200 Subject: [PATCH 2/2] sensor_msgs: the type of "rgba" field must be uint32 and not float32 --- .../include/sensor_msgs/impl/point_cloud2_iterator.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h index c66ae1ab..7ae8fddf 100644 --- a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +++ b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h @@ -189,7 +189,7 @@ inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...) * internals of the PointCloud2 * @param n_fields the number of fields to add. The fields are given as * strings: "xyz" (3 floats), "rgb" (3 uchar stacked in a float), - * "rgba" (4 uchar stacked in a float) + * "rgba" (4 uchar stacked in a uint32) * @return void * * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY @@ -215,9 +215,12 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...) offset = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, offset); has_xyz = true; } else - if ((field_name == "rgb") || (field_name == "rgba")) { + if (field_name == "rgb") { offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset); has_rgb = true; + } else if (field_name == "rgba") { + offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::UINT32, offset); + has_rgb = true; } else throw std::runtime_error("Field " + field_name + " does not exist"); }