$search
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