From 2e89da5996faa826d8ed3355b55a11d818990200 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:34:15 -0700 Subject: [PATCH] Custom serializer for PCLPointCloud2 and PCLHeader --- include/pcl_conversions/pcl_conversions.h | 231 ++++++++++++++++++++++ 1 file changed, 231 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index ae9e1b3c..6ad5e27a 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -54,6 +54,11 @@ #include #include +#include + +#include +#include + namespace pcl_conversions { /** PCLHeader <=> Header **/ @@ -178,4 +183,230 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) } // namespace pcl_conversions +namespace pcl { + +/** Overload pcl::getFieldIndex **/ +inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name) +{ + // Get the index we need + for (size_t d = 0; d < cloud.fields.size(); ++d) { + if (cloud.fields[d].name == field_name) { + return (static_cast(d)); + } + } + return (-1); + +} + +/** Overload pcl::getFieldsList **/ +inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) +{ + std::string result; + for (size_t i = 0; i < cloud.fields.size () - 1; ++i) { + result += cloud.fields[i].name + " "; + } + result += cloud.fields[cloud.fields.size () - 1].name; + return (result); +} + +/** Provide pcl::toROSMsg **/ +void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) +{ + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::toPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + pcl_conversions::fromPCL(pcl_image, image); +} + +/** Overload pcl::createMapping **/ +template +void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) +{ + std::vector pcl_msg_fields; + pcl_conversions::toPCL(msg_fields, pcl_msg_fields); + return createMapping(pcl_msg_fields, field_map); +} + +namespace io { + +/** Overload pcl::io::savePCDFile **/ +inline 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) +{ + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::toPCL(cloud, pcl_cloud); + return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); +} + +/** Overload pcl::io::loadPCDFile **/ +inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud) +{ + pcl::PCLPointCloud2 pcl_cloud; + int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); + pcl_conversions::fromPCL(pcl_cloud, cloud); + return ret; +} + +} // namespace io + +} // namespace pcl + +/* + * Provide a custom serialization for pcl::PCLPointCloud2 + */ + +namespace ros +{ + // In ROS 1.3.1+, we can specialize the functor used to create PointCloud objects + // on the subscriber side. This allows us to generate the mapping between message + // data and object fields only once and reuse it. +#if ROS_VERSION_MINIMUM(1, 3, 1) + template<> + struct DefaultMessageCreator + { + boost::shared_ptr operator() () + { + boost::shared_ptr msg(new pcl::PCLPointCloud2()); + return msg; + } + }; +#endif + + namespace message_traits + { + template<> + struct MD5Sum + { + static const char* value() { return MD5Sum::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. + ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); + ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); + }; + + template<> + struct DataType + { + static const char* value() { return DataType::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + }; + + template<> + struct Definition + { + static const char* value() { return Definition::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + }; + + template<> struct HasHeader : TrueType {}; + } // namespace ros::message_traits + + namespace serialization + { + template<> + struct Serializer + { + template + inline static void write(Stream& stream, const pcl::PCLPointCloud2& m) + { + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + stream.next(header); + stream.next(m.height); + stream.next(m.width); + std::vector pfs; + pcl_conversions::fromPCL(m.fields, pfs); + stream.next(pfs); + stream.next(m.is_bigendian); + stream.next(m.point_step); + stream.next(m.row_step); + stream.next(m.data); + stream.next(m.is_dense); + } + + template + inline static void read(Stream& stream, pcl::PCLPointCloud2& m) + { + std_msgs::Header header; + stream.next(header); + pcl_conversions::toPCL(header, m.header); + stream.next(m.height); + stream.next(m.width); + std::vector pfs; + stream.next(pfs); + pcl_conversions::toPCL(pfs, m.fields); + stream.next(m.fields); + stream.next(m.is_bigendian); + stream.next(m.point_step); + stream.next(m.row_step); + stream.next(m.data); + stream.next(m.is_dense); + } + + inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m) + { + uint32_t length = 0; + + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + length += serializationLength(header); + length += 4; // height + length += 4; // width + std::vector pfs; + pcl_conversions::fromPCL(m.fields, pfs); + length += serializationLength(pfs); // fields + length += 1; // is_bigendian + length += 4; // point_step + length += 4; // row_step + length += 4; // data's size + length += m.data.size() * sizeof(pcl::uint8_t); + length += 1; // is_dense + + return length; + } + }; + + template<> + struct Serializer + { + template + inline static void write(Stream& stream, const pcl::PCLHeader& m) + { + std_msgs::Header header; + pcl_conversions::fromPCL(m, header); + stream.next(header); + } + + template + inline static void read(Stream& stream, pcl::PCLHeader& m) + { + std_msgs::Header header; + stream.next(header); + pcl_conversions::toPCL(header, m); + } + + inline static uint32_t serializedLength(const pcl::PCLHeader& m) + { + uint32_t length = 0; + + std_msgs::Header header; + pcl_conversions::fromPCL(m, header); + length += serializationLength(header); + + return length; + } + }; + } // namespace ros::serialization + +} // namespace ros + + #endif \ No newline at end of file