37 #ifndef PCL_CONVERSIONS_H__ 38 #define PCL_CONVERSIONS_H__ 44 #include <pcl/conversions.h> 46 #include <pcl/PCLHeader.h> 47 #include <std_msgs/Header.h> 49 #include <pcl/PCLImage.h> 50 #include <sensor_msgs/Image.h> 52 #include <pcl/PCLPointField.h> 53 #include <sensor_msgs/PointField.h> 55 #include <pcl/PCLPointCloud2.h> 56 #include <sensor_msgs/PointCloud2.h> 58 #include <pcl/PointIndices.h> 59 #include <pcl_msgs/PointIndices.h> 61 #include <pcl/ModelCoefficients.h> 62 #include <pcl_msgs/ModelCoefficients.h> 64 #include <pcl/Vertices.h> 65 #include <pcl_msgs/Vertices.h> 67 #include <pcl/PolygonMesh.h> 68 #include <pcl_msgs/PolygonMesh.h> 70 #include <pcl/io/pcd_io.h> 72 #include <Eigen/StdVector> 73 #include <Eigen/Geometry> 88 pcl_stamp = stamp.
toNSec() / 1000ull;
102 pcl::uint64_t pcl_stamp;
103 toPCL(stamp, pcl_stamp);
110 void fromPCL(
const pcl::PCLHeader &pcl_header, std_msgs::Header &header)
112 fromPCL(pcl_header.stamp, header.stamp);
113 header.seq = pcl_header.seq;
114 header.frame_id = pcl_header.frame_id;
118 void toPCL(
const std_msgs::Header &header, pcl::PCLHeader &pcl_header)
120 toPCL(header.stamp, pcl_header.stamp);
121 pcl_header.seq = header.seq;
122 pcl_header.frame_id = header.frame_id;
126 std_msgs::Header
fromPCL(
const pcl::PCLHeader &pcl_header)
134 pcl::PCLHeader
toPCL(
const std_msgs::Header &header)
136 pcl::PCLHeader pcl_header;
137 toPCL(header, pcl_header);
146 fromPCL(pcl_image.header, image.header);
147 image.height = pcl_image.height;
148 image.width = pcl_image.width;
149 image.encoding = pcl_image.encoding;
150 image.is_bigendian = pcl_image.is_bigendian;
151 image.step = pcl_image.step;
155 void fromPCL(
const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
158 image.data = pcl_image.data;
162 void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
165 image.data.swap(pcl_image.data);
171 toPCL(image.header, pcl_image.header);
172 pcl_image.height = image.height;
173 pcl_image.width = image.width;
174 pcl_image.encoding = image.encoding;
175 pcl_image.is_bigendian = image.is_bigendian;
176 pcl_image.step = image.step;
180 void toPCL(
const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
183 pcl_image.data = image.data;
187 void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
190 pcl_image.data.swap(image.data);
196 void fromPCL(
const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf)
198 pf.name = pcl_pf.name;
199 pf.offset = pcl_pf.offset;
200 pf.datatype = pcl_pf.datatype;
201 pf.count = pcl_pf.count;
205 void fromPCL(
const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs)
207 pfs.resize(pcl_pfs.size());
208 std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
210 for(; it != pcl_pfs.end(); ++it, ++i) {
216 void toPCL(
const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf)
218 pcl_pf.name = pf.name;
219 pcl_pf.offset = pf.offset;
220 pcl_pf.datatype = pf.datatype;
221 pcl_pf.count = pf.count;
225 void toPCL(
const std::vector<sensor_msgs::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
227 pcl_pfs.resize(pfs.size());
228 std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();
230 for(; it != pfs.end(); ++it, ++i) {
231 toPCL(*(it), pcl_pfs[i]);
240 fromPCL(pcl_pc2.header, pc2.header);
241 pc2.height = pcl_pc2.height;
242 pc2.width = pcl_pc2.width;
243 fromPCL(pcl_pc2.fields, pc2.fields);
244 pc2.is_bigendian = pcl_pc2.is_bigendian;
245 pc2.point_step = pcl_pc2.point_step;
246 pc2.row_step = pcl_pc2.row_step;
247 pc2.is_dense = pcl_pc2.is_dense;
251 void fromPCL(
const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
254 pc2.data = pcl_pc2.data;
258 void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
261 pc2.data.swap(pcl_pc2.data);
267 toPCL(pc2.header, pcl_pc2.header);
268 pcl_pc2.height = pc2.height;
269 pcl_pc2.width = pc2.width;
270 toPCL(pc2.fields, pcl_pc2.fields);
271 pcl_pc2.is_bigendian = pc2.is_bigendian;
272 pcl_pc2.point_step = pc2.point_step;
273 pcl_pc2.row_step = pc2.row_step;
274 pcl_pc2.is_dense = pc2.is_dense;
278 void toPCL(
const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
281 pcl_pc2.data = pc2.data;
285 void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
288 pcl_pc2.data.swap(pc2.data);
294 void fromPCL(
const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
296 fromPCL(pcl_pi.header, pi.header);
297 pi.indices = pcl_pi.indices;
301 void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
303 fromPCL(pcl_pi.header, pi.header);
304 pi.indices.swap(pcl_pi.indices);
308 void toPCL(
const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
310 toPCL(pi.header, pcl_pi.header);
311 pcl_pi.indices = pi.indices;
315 void moveToPCL(pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
317 toPCL(pi.header, pcl_pi.header);
318 pcl_pi.indices.swap(pi.indices);
324 void fromPCL(
const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
326 fromPCL(pcl_mc.header, mc.header);
327 mc.values = pcl_mc.values;
331 void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
333 fromPCL(pcl_mc.header, mc.header);
334 mc.values.swap(pcl_mc.values);
338 void toPCL(
const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
340 toPCL(mc.header, pcl_mc.header);
341 pcl_mc.values = mc.values;
345 void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
347 toPCL(mc.header, pcl_mc.header);
348 pcl_mc.values.swap(mc.values);
354 void fromPCL(
const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
356 vert.vertices = pcl_vert.vertices;
360 void fromPCL(
const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
362 verts.resize(pcl_verts.size());
363 std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
364 std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
365 for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
371 void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
373 vert.vertices.swap(pcl_vert.vertices);
377 void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
379 verts.resize(pcl_verts.size());
380 std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
381 std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
382 for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
388 void toPCL(
const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
390 pcl_vert.vertices = vert.vertices;
394 void toPCL(
const std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
396 pcl_verts.resize(verts.size());
397 std::vector<pcl_msgs::Vertices>::const_iterator it = verts.begin();
398 std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
399 for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
405 void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
407 pcl_vert.vertices.swap(vert.vertices);
411 void moveToPCL(std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
413 pcl_verts.resize(verts.size());
414 std::vector<pcl_msgs::Vertices>::iterator it = verts.begin();
415 std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
416 for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
424 void fromPCL(
const pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
426 fromPCL(pcl_mesh.header, mesh.header);
427 fromPCL(pcl_mesh.cloud, mesh.cloud);
428 fromPCL(pcl_mesh.polygons, mesh.polygons);
432 void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
434 fromPCL(pcl_mesh.header, mesh.header);
439 void toPCL(
const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
441 toPCL(mesh.header, pcl_mesh.header);
442 toPCL(mesh.cloud, pcl_mesh.cloud);
443 toPCL(mesh.polygons, pcl_mesh.polygons);
447 void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
449 toPCL(mesh.header, pcl_mesh.header);
451 moveToPCL(mesh.polygons, pcl_mesh.polygons);
460 inline int getFieldIndex(
const sensor_msgs::PointCloud2 &cloud,
const std::string &field_name)
463 for (
size_t d = 0;
d < cloud.fields.size(); ++
d) {
464 if (cloud.fields[
d].name == field_name) {
465 return (static_cast<int>(
d));
476 for (
size_t i = 0; i < cloud.fields.size () - 1; ++i) {
477 result += cloud.fields[i].name +
" ";
479 result += cloud.fields[cloud.fields.size () - 1].name;
486 void toROSMsg(
const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
488 pcl::PCLPointCloud2 pcl_cloud;
490 pcl::PCLImage pcl_image;
491 pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
496 void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
498 pcl::PCLPointCloud2 pcl_cloud;
500 pcl::PCLImage pcl_image;
501 pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
505 template<
typename T>
void 506 toROSMsg (
const pcl::PointCloud<T> &cloud, sensor_msgs::Image& msg)
509 if (cloud.width == 0 && cloud.height == 0)
511 throw std::runtime_error(
"Needs to be a dense like cloud!!");
515 if (cloud.points.size () != cloud.width * cloud.height)
516 throw std::runtime_error(
"The width and height do not match the cloud size!");
517 msg.height = cloud.height;
518 msg.width = cloud.width;
522 msg.encoding =
"bgr8";
523 msg.step = msg.width *
sizeof (uint8_t) * 3;
524 msg.data.resize (msg.step * msg.height);
525 for (
size_t y = 0; y < cloud.height; y++)
527 for (
size_t x = 0; x < cloud.width; x++)
529 uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
530 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(uint8_t));
538 void toROSMsg(
const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
540 pcl::PCLPointCloud2 pcl_pc2;
541 pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
546 void fromROSMsg(
const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
548 pcl::PCLPointCloud2 pcl_pc2;
550 pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
554 void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
556 pcl::PCLPointCloud2 pcl_pc2;
558 pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
563 template<
typename Po
intT>
564 void createMapping(
const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
566 std::vector<pcl::PCLPointField> pcl_msg_fields;
568 return createMapping<PointT>(pcl_msg_fields, field_map);
576 savePCDFile(
const std::string &file_name,
const sensor_msgs::PointCloud2 &cloud,
577 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
578 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
579 const bool binary_mode =
false)
581 pcl::PCLPointCloud2 pcl_cloud;
588 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
589 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
590 const bool binary_mode =
false)
592 pcl::PCLPointCloud2 pcl_cloud;
599 inline int loadPCDFile(
const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
601 pcl::PCLPointCloud2 pcl_cloud;
613 const sensor_msgs::PointCloud2 &cloud2,
614 sensor_msgs::PointCloud2 &cloud_out)
617 if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
622 else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
629 for (
size_t i = 0; i < cloud1.fields.size (); ++i)
630 if (cloud1.fields[i].name ==
"_")
633 for (
size_t i = 0; i < cloud2.fields.size (); ++i)
634 if (cloud2.fields[i].name ==
"_")
637 if (!strip && cloud1.fields.size () != cloud2.fields.size ())
639 PCL_ERROR (
"[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
645 size_t nrpts = cloud_out.data.size ();
647 cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
648 cloud_out.height = 1;
649 if (!cloud1.is_dense || !cloud2.is_dense)
650 cloud_out.is_dense =
false;
652 cloud_out.is_dense =
true;
658 std::vector<sensor_msgs::PointField> fields2;
659 std::vector<int> fields2_sizes;
660 for (
size_t j = 0; j < cloud2.fields.size (); ++j)
662 if (cloud2.fields[j].name ==
"_")
665 fields2_sizes.push_back (cloud2.fields[j].count *
666 pcl::getFieldSize (cloud2.fields[j].datatype));
667 fields2.push_back (cloud2.fields[j]);
670 cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
673 for (
size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
676 for (
size_t j = 0; j < fields2.size (); ++j)
678 if (cloud1.fields[i].name ==
"_")
685 if ((cloud1.fields[i].name ==
"rgb" && fields2[j].name ==
"rgba") ||
686 (cloud1.fields[i].name ==
"rgba" && fields2[j].name ==
"rgb") ||
687 (cloud1.fields[i].name == fields2[j].name))
689 memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
690 reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
699 for (
size_t i = 0; i < cloud1.fields.size (); ++i)
702 if ((cloud1.fields[i].name ==
"rgb" && cloud2.fields[i].name ==
"rgba") ||
703 (cloud1.fields[i].name ==
"rgba" && cloud2.fields[i].name ==
"rgb"))
706 if (cloud1.fields[i].name != cloud2.fields[i].name)
708 PCL_ERROR (
"[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
712 cloud_out.data.resize (nrpts + cloud2.data.size ());
713 memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
732 namespace message_traits
738 static const char*
value(
const pcl::PCLPointCloud2&) {
return value(); }
752 static const char*
value(
const pcl::PCLPointCloud2&) {
return value(); }
759 static const char*
value(
const pcl::PCLPointCloud2&) {
return value(); }
765 namespace serialization
773 template<
typename Stream>
774 inline static void write(
Stream& stream,
const pcl::PCLPointCloud2& m)
779 stream.next(m.height);
780 stream.next(m.width);
781 std::vector<sensor_msgs::PointField> pfs;
784 stream.next(m.is_bigendian);
785 stream.next(m.point_step);
786 stream.next(m.row_step);
788 stream.next(m.is_dense);
791 template<
typename Stream>
792 inline static void read(
Stream& stream, pcl::PCLPointCloud2& m)
797 stream.next(m.height);
798 stream.next(m.width);
799 std::vector<sensor_msgs::PointField> pfs;
802 stream.next(m.is_bigendian);
803 stream.next(m.point_step);
804 stream.next(m.row_step);
806 stream.next(m.is_dense);
818 std::vector<sensor_msgs::PointField> pfs;
825 length += m.data.size() *
sizeof(pcl::uint8_t);
838 template<
typename Stream>
839 inline static void write(
Stream& stream,
const pcl::PCLPointField& m)
842 stream.next(m.offset);
843 stream.next(m.datatype);
844 stream.next(m.count);
847 template<
typename Stream>
848 inline static void read(
Stream& stream, pcl::PCLPointField& m)
851 stream.next(m.offset);
852 stream.next(m.datatype);
853 stream.next(m.count);
875 template<
typename Stream>
883 template<
typename Stream>
void createMapping(const std::vector< sensor_msgs::PointField > &msg_fields, MsgFieldMap &field_map)
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
static uint32_t serializedLength(const pcl::PCLPointField &m)
void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
int destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary_mode=false)
Time & fromNSec(uint64_t t)
static const char * value()
static uint32_t serializedLength(const pcl::PCLPointCloud2 &m)
static void write(Stream &stream, const pcl::PCLPointCloud2 &m)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
std_msgs::Header * header(M &m)
static const char * value(const pcl::PCLPointCloud2 &)
static void write(Stream &stream, const pcl::PCLPointField &m)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
#define ROS_STATIC_ASSERT(cond)
static void read(Stream &stream, pcl::PCLPointCloud2 &m)
static void read(Stream &stream, pcl::PCLPointField &m)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
static const char * value(const pcl::PCLPointCloud2 &)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
uint32_t serializationLength(const T &t)
static const char * value()
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
static const char * value()
static const char * value(const pcl::PCLPointCloud2 &)
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
int savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary_mode=false)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)