38 #ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H 39 #define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H 41 #include <sensor_msgs/PointCloud.h> 42 #include <sensor_msgs/PointCloud2.h> 59 for (
size_t d = 0;
d < cloud.fields.size (); ++
d)
60 if (cloud.fields[
d].name == field_name)
72 output.header = input.header;
73 output.width = input.points.size ();
75 output.fields.resize (3 + input.channels.size ());
77 output.fields[0].name =
"x"; output.fields[1].name =
"y"; output.fields[2].name =
"z";
80 for (
size_t d = 0;
d < output.fields.size (); ++
d, offset += 4)
82 output.fields[
d].offset = offset;
83 output.fields[
d].datatype = sensor_msgs::PointField::FLOAT32;
84 output.fields[
d].count = 1;
86 output.point_step = offset;
87 output.row_step = output.point_step * output.width;
89 for (
size_t d = 0;
d < input.channels.size (); ++
d)
90 output.fields[3 +
d].name = input.channels[
d].name;
91 output.data.resize (input.points.size () * output.point_step);
92 output.is_bigendian =
false;
93 output.is_dense =
false;
96 for (
size_t cp = 0; cp < input.points.size (); ++cp)
98 memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (
float));
99 memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (
float));
100 memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (
float));
101 for (
size_t d = 0;
d < input.channels.size (); ++
d)
103 if (input.channels[
d].values.size() == input.points.size())
105 memcpy (&output.data[cp * output.point_step + output.fields[3 +
d].offset], &input.channels[
d].values[cp], sizeof (
float));
120 output.header = input.header;
121 output.points.resize (input.width * input.height);
122 output.channels.resize (input.fields.size () - 3);
127 if (x_idx == -1 || y_idx == -1 || z_idx == -1)
129 std::cerr <<
"x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
132 int x_offset = input.fields[x_idx].offset;
133 int y_offset = input.fields[y_idx].offset;
134 int z_offset = input.fields[z_idx].offset;
135 uint8_t x_datatype = input.fields[x_idx].datatype;
136 uint8_t y_datatype = input.fields[y_idx].datatype;
137 uint8_t z_datatype = input.fields[z_idx].datatype;
141 for (
size_t d = 0;
d < input.fields.size (); ++
d)
143 if ((
int)input.fields[
d].offset == x_offset || (int)input.fields[
d].offset == y_offset || (
int)input.fields[
d].offset == z_offset)
145 output.channels[cur_c].name = input.fields[
d].name;
146 output.channels[cur_c].values.resize (output.points.size ());
151 for (
size_t cp = 0; cp < output.points.size (); ++cp)
154 output.points[cp].x = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + x_offset], x_datatype);
155 output.points[cp].y = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + y_offset], y_datatype);
156 output.points[cp].z = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + z_offset], z_datatype);
159 for (
size_t d = 0;
d < input.fields.size (); ++
d)
161 if ((
int)input.fields[
d].offset == x_offset || (int)input.fields[
d].offset == y_offset || (
int)input.fields[
d].offset == z_offset)
163 output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + input.fields[
d].offset], input.fields[
d].datatype);
169 #endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
static int getPointCloud2FieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
static bool convertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
Convert a sensor_msgs::PointCloud2 message to a sensor_msgs::PointCloud message.
Tools for manipulating sensor_msgs.
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
Convert a sensor_msgs::PointCloud message to a sensor_msgs::PointCloud2 message.