point_cloud.h
Go to the documentation of this file.
00001 #ifndef pcl_ROS_POINT_CLOUD_H_
00002 #define pcl_ROS_POINT_CLOUD_H_
00003 
00004 #include <ros/ros.h>
00005 #include <pcl/point_cloud.h>
00006 #include <pcl/point_traits.h>
00007 #include <pcl/for_each_type.h>
00008 #include <pcl/conversions.h>
00009 #include <pcl_conversions/pcl_conversions.h>
00010 #include <sensor_msgs/PointCloud2.h>
00011 #include <boost/mpl/size.hpp>
00012 #include <boost/ref.hpp>
00013 
00014 namespace pcl 
00015 {
00016   namespace detail 
00017   {
00018     template<typename Stream, typename PointT>
00019     struct FieldStreamer
00020     {
00021       FieldStreamer(Stream& stream) : stream_(stream) {}
00022 
00023       template<typename U> void operator() ()
00024       {
00025         const char* name = traits::name<PointT, U>::value;
00026         uint32_t name_length = strlen(name);
00027         stream_.next(name_length);
00028         if (name_length > 0)
00029           memcpy(stream_.advance(name_length), name, name_length);
00030 
00031         uint32_t offset = traits::offset<PointT, U>::value;
00032         stream_.next(offset);
00033 
00034         uint8_t datatype = traits::datatype<PointT, U>::value;
00035         stream_.next(datatype);
00036 
00037         uint32_t count = traits::datatype<PointT, U>::size;
00038         stream_.next(count);
00039       }
00040 
00041       Stream& stream_;
00042     };
00043 
00044     template<typename PointT>
00045     struct FieldsLength
00046     {
00047       FieldsLength() : length(0) {}
00048 
00049       template<typename U> void operator() ()
00050       {
00051         uint32_t name_length = strlen(traits::name<PointT, U>::value);
00052         length += name_length + 13;
00053       }
00054 
00055       uint32_t length;
00056     };
00057   } // namespace pcl::detail
00058 } // namespace pcl
00059 
00060 namespace ros 
00061 {
00062   // In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
00063   // on the subscriber side. This allows us to generate the mapping between message
00064   // data and object fields only once and reuse it.
00065 #if ROS_VERSION_MINIMUM(1, 3, 1)
00066   template<typename T>
00067   struct DefaultMessageCreator<pcl::PointCloud<T> >
00068   {
00069     boost::shared_ptr<pcl::MsgFieldMap> mapping_;
00070 
00071     DefaultMessageCreator()
00072       : mapping_( boost::make_shared<pcl::MsgFieldMap>() )
00073     {
00074     }
00075     
00076     boost::shared_ptr<pcl::PointCloud<T> > operator() ()
00077     {
00078       boost::shared_ptr<pcl::PointCloud<T> > msg (new pcl::PointCloud<T> ());
00079       pcl::detail::getMapping(*msg) = mapping_;
00080       return msg;
00081     }
00082   };
00083 #endif
00084 
00085   namespace message_traits 
00086   {
00087     template<typename T> struct MD5Sum<pcl::PointCloud<T> >
00088     {
00089       static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
00090       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00091 
00092       static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
00093       static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
00094       
00095       // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
00096       ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
00097       ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
00098     };
00099 
00100     template<typename T> struct DataType<pcl::PointCloud<T> >
00101     {
00102       static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
00103       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00104     };
00105 
00106     template<typename T> struct Definition<pcl::PointCloud<T> >
00107     {
00108       static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
00109       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00110     };
00111 
00112     template<typename T> struct HasHeader<pcl::PointCloud<T> > : TrueType {};
00113 
00114     // This is bad because it will only work for pcl::PointXYZ, but I can't
00115     // find another way around ambiguous partial template specialization...
00116     template<>
00117     struct TimeStamp<pcl::PointCloud<pcl::PointXYZ> >
00118     {
00119       // This specialization could be dangerous, but it's the best I can do.
00120       // If this TimeStamp struct is destroyed before they are done with the
00121       // pointer returned by the first functions may go out of scope, but there
00122       // isn't a lot I can do about that. This is a good reason to refuse to
00123       // returning pointers like this...
00124       static ros::Time* pointer(pcl::PointCloud<pcl::PointXYZ> &m) {
00125         header_.reset(new std_msgs::Header());
00126         pcl_conversions::fromPCL(m.header, *(header_));
00127         return &(header_->stamp);
00128       }
00129       static ros::Time const* pointer(const pcl::PointCloud<pcl::PointXYZ>& m) {
00130         header_const_.reset(new std_msgs::Header());
00131         pcl_conversions::fromPCL(m.header, *(header_const_));
00132         return &(header_const_->stamp);
00133       }
00134       static ros::Time value(const pcl::PointCloud<pcl::PointXYZ>& m) {
00135         return pcl_conversions::fromPCL(m.header).stamp;
00136       }
00137     private:
00138       static boost::shared_ptr<std_msgs::Header> header_;
00139       static boost::shared_ptr<std_msgs::Header> header_const_;
00140     };
00141 
00142     template<>
00143     struct TimeStamp<pcl::PointCloud<pcl::Normal> >
00144     {
00145       // This specialization could be dangerous, but it's the best I can do.
00146       // If this TimeStamp struct is destroyed before they are done with the
00147       // pointer returned by the first functions may go out of scope, but there
00148       // isn't a lot I can do about that. This is a good reason to refuse to
00149       // returning pointers like this...
00150       static ros::Time* pointer(pcl::PointCloud<pcl::Normal> &m) {
00151         header_.reset(new std_msgs::Header());
00152         pcl_conversions::fromPCL(m.header, *(header_));
00153         return &(header_->stamp);
00154       }
00155       static ros::Time const* pointer(const pcl::PointCloud<pcl::Normal>& m) {
00156         header_const_.reset(new std_msgs::Header());
00157         pcl_conversions::fromPCL(m.header, *(header_const_));
00158         return &(header_const_->stamp);
00159       }
00160       static ros::Time value(const pcl::PointCloud<pcl::Normal>& m) {
00161         return pcl_conversions::fromPCL(m.header).stamp;
00162       }
00163     private:
00164       static boost::shared_ptr<std_msgs::Header> header_;
00165       static boost::shared_ptr<std_msgs::Header> header_const_;
00166     };
00167 
00168   } // namespace ros::message_traits
00169 
00170   namespace serialization 
00171   {
00172     template<typename T>
00173     struct Serializer<pcl::PointCloud<T> >
00174     {
00175       template<typename Stream>
00176       inline static void write(Stream& stream, const pcl::PointCloud<T>& m)
00177       {
00178         stream.next(m.header);
00179         
00180         // Ease the user's burden on specifying width/height for unorganized datasets
00181         uint32_t height = m.height, width = m.width;
00182         if (height == 0 && width == 0) {
00183           width = m.points.size();
00184           height = 1;
00185         }
00186         stream.next(height);
00187         stream.next(width);
00188 
00189         // Stream out point field metadata
00190         typedef typename pcl::traits::fieldList<T>::type FieldList;
00191         uint32_t fields_size = boost::mpl::size<FieldList>::value;
00192         stream.next(fields_size);
00193         pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
00194 
00195         // Assume little-endian...
00196         uint8_t is_bigendian = false;
00197         stream.next(is_bigendian);
00198 
00199         // Write out point data as binary blob
00200         uint32_t point_step = sizeof(T);
00201         stream.next(point_step);
00202         uint32_t row_step = point_step * width;
00203         stream.next(row_step);
00204         uint32_t data_size = row_step * height;
00205         stream.next(data_size);
00206         memcpy(stream.advance(data_size), &m.points[0], data_size);
00207 
00208         uint8_t is_dense = m.is_dense;
00209         stream.next(is_dense);
00210       }
00211 
00212       template<typename Stream>
00213       inline static void read(Stream& stream, pcl::PointCloud<T>& m)
00214       {
00215         std_msgs::Header header;
00216         stream.next(header);
00217         pcl_conversions::toPCL(header, m.header);
00218         stream.next(m.height);
00219         stream.next(m.width);
00220 
00222         std::vector<sensor_msgs::PointField> fields;
00223         stream.next(fields);
00224 
00225         // Construct field mapping if deserializing for the first time
00226         boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
00227         if (!mapping_ptr)
00228         {
00229           // This normally should get allocated by DefaultMessageCreator, but just in case
00230           mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
00231         }
00232         pcl::MsgFieldMap& mapping = *mapping_ptr;
00233         if (mapping.empty())
00234           pcl::createMapping<T> (fields, mapping);
00235 
00236         uint8_t is_bigendian;
00237         stream.next(is_bigendian); // ignoring...
00238         uint32_t point_step, row_step;
00239         stream.next(point_step);
00240         stream.next(row_step);
00241 
00242         // Copy point data
00243         uint32_t data_size;
00244         stream.next(data_size);
00245         assert(data_size == m.height * m.width * point_step);
00246         m.points.resize(m.height * m.width);
00247         uint8_t* m_data = reinterpret_cast<uint8_t*>(&m.points[0]);
00248         // If the data layouts match, can copy a whole row in one memcpy
00249         if (mapping.size() == 1 &&
00250             mapping[0].serialized_offset == 0 &&
00251             mapping[0].struct_offset == 0 &&
00252             point_step == sizeof(T))
00253         {
00254           uint32_t m_row_step = sizeof(T) * m.width;
00255           // And if the row steps match, can copy whole point cloud in one memcpy
00256           if (m_row_step == row_step)
00257           {
00258             memcpy (m_data, stream.advance(data_size), data_size);
00259           }
00260           else
00261           {
00262             for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
00263               memcpy (m_data, stream.advance(row_step), m_row_step);
00264           }
00265         }
00266         else
00267         {
00268           // If not, do a lot of memcpys to copy over the fields
00269           for (uint32_t row = 0; row < m.height; ++row) {
00270             const uint8_t* stream_data = stream.advance(row_step);
00271             for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
00272               BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
00273                 memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
00274               }
00275               m_data += sizeof(T);
00276             }
00277           }
00278         }
00279 
00280         uint8_t is_dense;
00281         stream.next(is_dense);
00282         m.is_dense = is_dense;
00283       }
00284 
00285       inline static uint32_t serializedLength(const pcl::PointCloud<T>& m)
00286       {
00287         uint32_t length = 0;
00288 
00289         length += serializationLength(m.header);
00290         length += 8; // height/width
00291 
00292         pcl::detail::FieldsLength<T> fl;
00293         typedef typename pcl::traits::fieldList<T>::type FieldList;
00294         pcl::for_each_type<FieldList>(boost::ref(fl));
00295         length += 4; // size of 'fields'
00296         length += fl.length;
00297 
00298         length += 1; // is_bigendian
00299         length += 4; // point_step
00300         length += 4; // row_step
00301         length += 4; // size of 'data'
00302         length += m.points.size() * sizeof(T); // data
00303         length += 1; // is_dense
00304 
00305         return length;
00306       }
00307     };
00308   } // namespace ros::serialization
00309 
00311 
00312 } // namespace ros
00313 
00314 #endif


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu Jun 6 2019 21:01:44