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/ros/point_traits.h>
00007 #include <pcl/ros/for_each_type.h>
00008 #include <pcl/ros/conversions.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 #include <boost/mpl/size.hpp>
00011 #include <boost/ref.hpp>
00012 
00013 namespace pcl 
00014 {
00015   namespace detail 
00016   {
00017     template<typename Stream, typename PointT>
00018     struct FieldStreamer
00019     {
00020       FieldStreamer(Stream& stream) : stream_(stream) {}
00021 
00022       template<typename U> void operator() ()
00023       {
00024         const char* name = traits::name<PointT, U>::value;
00025         uint32_t name_length = strlen(name);
00026         stream_.next(name_length);
00027         if (name_length > 0)
00028           memcpy(stream_.advance(name_length), name, name_length);
00029 
00030         uint32_t offset = traits::offset<PointT, U>::value;
00031         stream_.next(offset);
00032 
00033         uint8_t datatype = traits::datatype<PointT, U>::value;
00034         stream_.next(datatype);
00035 
00036         uint32_t count = traits::datatype<PointT, U>::size;
00037         stream_.next(count);
00038       }
00039 
00040       Stream& stream_;
00041     };
00042 
00043     template<typename PointT>
00044     struct FieldsLength
00045     {
00046       FieldsLength() : length(0) {}
00047 
00048       template<typename U> void operator() ()
00049       {
00050         uint32_t name_length = strlen(traits::name<PointT, U>::value);
00051         length += name_length + 13;
00052       }
00053 
00054       uint32_t length;
00055     };
00056   } // namespace pcl::detail
00057 } // namespace pcl
00058 
00059 namespace ros 
00060 {
00061   // In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
00062   // on the subscriber side. This allows us to generate the mapping between message
00063   // data and object fields only once and reuse it.
00064 #if ROS_VERSION_MINIMUM(1, 3, 1)
00065   template<typename T>
00066   struct DefaultMessageCreator<pcl::PointCloud<T> >
00067   {
00068     boost::shared_ptr<pcl::MsgFieldMap> mapping_;
00069 
00070     DefaultMessageCreator()
00071       : mapping_( boost::make_shared<pcl::MsgFieldMap>() )
00072     {
00073     }
00074     
00075     boost::shared_ptr<pcl::PointCloud<T> > operator() ()
00076     {
00077       boost::shared_ptr<pcl::PointCloud<T> > msg (new pcl::PointCloud<T> ());
00078       pcl::detail::getMapping(*msg) = mapping_;
00079       return msg;
00080     }
00081   };
00082 #endif
00083 
00084   namespace message_traits 
00085   {
00086     template<typename T> struct MD5Sum<pcl::PointCloud<T> >
00087     {
00088       static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
00089       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00090 
00091       static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
00092       static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
00093       
00094       // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
00095       ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
00096       ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
00097     };
00098 
00099     template<typename T> struct DataType<pcl::PointCloud<T> >
00100     {
00101       static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
00102       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00103     };
00104 
00105     template<typename T> struct Definition<pcl::PointCloud<T> >
00106     {
00107       static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
00108       static const char* value(const pcl::PointCloud<T>&) { return value(); }
00109     };
00110 
00111     template<typename T> struct HasHeader<pcl::PointCloud<T> > : TrueType {};
00112   } // namespace ros::message_traits
00113 
00114   namespace serialization 
00115   {
00116     template<typename T>
00117     struct Serializer<pcl::PointCloud<T> >
00118     {
00119       template<typename Stream>
00120       inline static void write(Stream& stream, const pcl::PointCloud<T>& m)
00121       {
00122         stream.next(m.header);
00123         
00124         // Ease the user's burden on specifying width/height for unorganized datasets
00125         uint32_t height = m.height, width = m.width;
00126         if (height == 0 && width == 0) {
00127           width = m.points.size();
00128           height = 1;
00129         }
00130         stream.next(height);
00131         stream.next(width);
00132 
00133         // Stream out point field metadata
00134         typedef typename pcl::traits::fieldList<T>::type FieldList;
00135         uint32_t fields_size = boost::mpl::size<FieldList>::value;
00136         stream.next(fields_size);
00137         pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
00138 
00139         // Assume little-endian...
00140         uint8_t is_bigendian = false;
00141         stream.next(is_bigendian);
00142 
00143         // Write out point data as binary blob
00144         uint32_t point_step = sizeof(T);
00145         stream.next(point_step);
00146         uint32_t row_step = point_step * width;
00147         stream.next(row_step);
00148         uint32_t data_size = row_step * height;
00149         stream.next(data_size);
00150         memcpy(stream.advance(data_size), &m.points[0], data_size);
00151 
00152         uint8_t is_dense = m.is_dense;
00153         stream.next(is_dense);
00154       }
00155 
00156       template<typename Stream>
00157       inline static void read(Stream& stream, pcl::PointCloud<T>& m)
00158       {
00159         stream.next(m.header);
00160         stream.next(m.height);
00161         stream.next(m.width);
00162 
00164         std::vector<sensor_msgs::PointField> fields;
00165         stream.next(fields);
00166 
00167         // Construct field mapping if deserializing for the first time
00168         boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
00169         if (!mapping_ptr)
00170         {
00171           // This normally should get allocated by DefaultMessageCreator, but just in case
00172           mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
00173         }
00174         pcl::MsgFieldMap& mapping = *mapping_ptr;
00175         if (mapping.empty())
00176           pcl::createMapping<T> (fields, mapping);
00177 
00178         uint8_t is_bigendian;
00179         stream.next(is_bigendian); // ignoring...
00180         uint32_t point_step, row_step;
00181         stream.next(point_step);
00182         stream.next(row_step);
00183 
00184         // Copy point data
00185         uint32_t data_size;
00186         stream.next(data_size);
00187         assert(data_size == m.height * m.width * point_step);
00188         m.points.resize(m.height * m.width);
00189         uint8_t* m_data = reinterpret_cast<uint8_t*>(&m.points[0]);
00190         // If the data layouts match, can copy a whole row in one memcpy
00191         if (mapping.size() == 1 &&
00192             mapping[0].serialized_offset == 0 &&
00193             mapping[0].struct_offset == 0 &&
00194             point_step == sizeof(T))
00195         {
00196           uint32_t m_row_step = sizeof(T) * m.width;
00197           // And if the row steps match, can copy whole point cloud in one memcpy
00198           if (m_row_step == row_step)
00199           {
00200             memcpy (m_data, stream.advance(data_size), data_size);
00201           }
00202           else
00203           {
00204             for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
00205               memcpy (m_data, stream.advance(row_step), m_row_step);
00206           }
00207         }
00208         else
00209         {
00210           // If not, do a lot of memcpys to copy over the fields
00211           for (uint32_t row = 0; row < m.height; ++row) {
00212             const uint8_t* stream_data = stream.advance(row_step);
00213             for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
00214               BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
00215                 memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
00216               }
00217               m_data += sizeof(T);
00218             }
00219           }
00220         }
00221 
00222         uint8_t is_dense;
00223         stream.next(is_dense);
00224         m.is_dense = is_dense;
00225       }
00226 
00227       inline static uint32_t serializedLength(const pcl::PointCloud<T>& m)
00228       {
00229         uint32_t length = 0;
00230 
00231         length += serializationLength(m.header);
00232         length += 8; // height/width
00233 
00234         pcl::detail::FieldsLength<T> fl;
00235         typedef typename pcl::traits::fieldList<T>::type FieldList;
00236         pcl::for_each_type<FieldList>(boost::ref(fl));
00237         length += 4; // size of 'fields'
00238         length += fl.length;
00239 
00240         length += 1; // is_bigendian
00241         length += 4; // point_step
00242         length += 4; // row_step
00243         length += 4; // size of 'data'
00244         length += m.points.size() * sizeof(T); // data
00245         length += 1; // is_dense
00246 
00247         return length;
00248       }
00249     };
00250   } // namespace ros::serialization
00251 
00253 
00254 } // namespace ros
00255 
00256 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_ros
Author(s): Maintained by Radu Bogdan Rusu
autogenerated on Tue Mar 5 2013 13:54:41