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/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 }
00057 }
00058
00059 namespace ros
00060 {
00061
00062
00063
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
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 }
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
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
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
00140 uint8_t is_bigendian = false;
00141 stream.next(is_bigendian);
00142
00143
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
00168 boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
00169 if (!mapping_ptr)
00170 {
00171
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);
00180 uint32_t point_step, row_step;
00181 stream.next(point_step);
00182 stream.next(row_step);
00183
00184
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
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
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
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;
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;
00238 length += fl.length;
00239
00240 length += 1;
00241 length += 4;
00242 length += 4;
00243 length += 4;
00244 length += m.points.size() * sizeof(T);
00245 length += 1;
00246
00247 return length;
00248 }
00249 };
00250 }
00251
00253
00254 }
00255
00256 #endif