Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
00039 #define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
00040
00041 #include <sensor_msgs/PointCloud.h>
00042 #include <sensor_msgs/PointCloud2.h>
00043
00048 namespace sensor_msgs
00049 {
00051
00055 static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
00056 {
00057
00058 for (size_t d = 0; d < cloud.fields.size (); ++d)
00059 if (cloud.fields[d].name == field_name)
00060 return (d);
00061 return (-1);
00062 }
00063
00065
00069 static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
00070 {
00071 output.header = input.header;
00072 output.width = input.points.size ();
00073 output.height = 1;
00074 output.fields.resize (3 + input.channels.size ());
00075
00076 output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00077 int offset = 0;
00078
00079 for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00080 {
00081 output.fields[d].offset = offset;
00082 output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00083 output.fields[d].count = 1;
00084 }
00085 output.point_step = offset;
00086 output.row_step = output.point_step * output.width;
00087
00088 for (size_t d = 0; d < input.channels.size (); ++d)
00089 output.fields[3 + d].name = input.channels[d].name;
00090 output.data.resize (input.points.size () * output.point_step);
00091 output.is_bigendian = false;
00092 output.is_dense = false;
00093
00094
00095 for (size_t cp = 0; cp < input.points.size (); ++cp)
00096 {
00097 memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00098 memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00099 memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00100 for (size_t d = 0; d < input.channels.size (); ++d)
00101 {
00102 if (input.channels[d].values.size() == input.points.size())
00103 {
00104 memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00105 }
00106 }
00107 }
00108 return (true);
00109 }
00110
00112
00116 static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
00117 {
00118
00119 for (size_t d = 0; d < input.fields.size (); ++d)
00120 {
00121 if (input.fields[d].datatype != sensor_msgs::PointField::FLOAT32)
00122 {
00123 std::cerr << boost::str(boost::format("sensor_msgs::PointCloud accepts only float32 values, but field %d (%s) has field type %d!")%(int)d% input.fields[d].name%input.fields[d].datatype) << std::endl;
00124 }
00125 }
00126
00127 output.header = input.header;
00128 output.points.resize (input.width * input.height);
00129 output.channels.resize (input.fields.size () - 3);
00130
00131 int x_idx = getPointCloud2FieldIndex (input, "x");
00132 int y_idx = getPointCloud2FieldIndex (input, "y");
00133 int z_idx = getPointCloud2FieldIndex (input, "z");
00134 if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00135 {
00136 std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
00137 return (false);
00138 }
00139 int x_offset = input.fields[x_idx].offset;
00140 int y_offset = input.fields[y_idx].offset;
00141 int z_offset = input.fields[z_idx].offset;
00142
00143
00144 int cur_c = 0;
00145 for (size_t d = 0; d < input.fields.size (); ++d)
00146 {
00147 if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00148 continue;
00149 output.channels[cur_c].name = input.fields[d].name;
00150 output.channels[cur_c].values.resize (output.points.size ());
00151 cur_c++;
00152 }
00153
00154
00155 for (size_t cp = 0; cp < output.points.size (); ++cp)
00156 {
00157
00158 memcpy (&output.points[cp].x, &input.data[cp * input.point_step + x_offset], sizeof (float));
00159 memcpy (&output.points[cp].y, &input.data[cp * input.point_step + y_offset], sizeof (float));
00160 memcpy (&output.points[cp].z, &input.data[cp * input.point_step + z_offset], sizeof (float));
00161
00162 int cur_c = 0;
00163 for (size_t d = 0; d < input.fields.size (); ++d)
00164 {
00165 if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00166 continue;
00167 memcpy (&output.channels[cur_c++].values[cp], &input.data[cp * input.point_step + input.fields[d].offset], sizeof (float));
00168 }
00169 }
00170 return (true);
00171 }
00172 }
00173 #endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H