Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

sensor_msgs: Generate optimal msg lentgth when both "xyz" and "rgb(a)… #77

Open
wants to merge 2 commits into
base: jade-devel
Choose a base branch
from
Open

sensor_msgs: Generate optimal msg lentgth when both "xyz" and "rgb(a)… #77

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
684ed50
sensor_msgs: Generate optimal msg lentgth when both "xyz" and "rgb(a)…
tmoinel Jul 21, 2015
d4778fb
sensor_msgs: the type of "rgba" field must be uint32 and not float32
tmoinel Aug 10, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 14 additions & 4 deletions sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -211,16 +213,24 @@ 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")) {
if (field_name == "rgb") {
offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset);
offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
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");
}
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;
Expand Down