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 }
00058 }
00059
00060 namespace ros
00061 {
00062
00063
00064
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
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
00115
00116 template<>
00117 struct TimeStamp<pcl::PointCloud<pcl::PointXYZ> >
00118 {
00119
00120
00121
00122
00123
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
00146
00147
00148
00149
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 }
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
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
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
00196 uint8_t is_bigendian = false;
00197 stream.next(is_bigendian);
00198
00199
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
00226 boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
00227 if (!mapping_ptr)
00228 {
00229
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);
00238 uint32_t point_step, row_step;
00239 stream.next(point_step);
00240 stream.next(row_step);
00241
00242
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
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
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
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;
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;
00296 length += fl.length;
00297
00298 length += 1;
00299 length += 4;
00300 length += 4;
00301 length += 4;
00302 length += m.points.size() * sizeof(T);
00303 length += 1;
00304
00305 return length;
00306 }
00307 };
00308 }
00309
00311
00312 }
00313
00314 #endif