point_cloud.h
Go to the documentation of this file.
1 #ifndef pcl_ROS_POINT_CLOUD_H_
2 #define pcl_ROS_POINT_CLOUD_H_
3 
4 #include <ros/ros.h>
5 #include <pcl/point_cloud.h>
6 #include <pcl/point_traits.h>
7 #include <pcl/for_each_type.h>
8 #include <pcl/conversions.h>
10 #include <sensor_msgs/PointCloud2.h>
11 #include <boost/mpl/size.hpp>
12 #include <boost/ref.hpp>
13 
14 namespace pcl
15 {
16  namespace detail
17  {
18  template<typename Stream, typename PointT>
20  {
21  FieldStreamer(Stream& stream) : stream_(stream) {}
22 
23  template<typename U> void operator() ()
24  {
25  const char* name = traits::name<PointT, U>::value;
26  uint32_t name_length = strlen(name);
27  stream_.next(name_length);
28  if (name_length > 0)
29  memcpy(stream_.advance(name_length), name, name_length);
30 
31  uint32_t offset = traits::offset<PointT, U>::value;
32  stream_.next(offset);
33 
34  uint8_t datatype = traits::datatype<PointT, U>::value;
35  stream_.next(datatype);
36 
37  uint32_t count = traits::datatype<PointT, U>::size;
38  stream_.next(count);
39  }
40 
41  Stream& stream_;
42  };
43 
44  template<typename PointT>
45  struct FieldsLength
46  {
47  FieldsLength() : length(0) {}
48 
49  template<typename U> void operator() ()
50  {
51  uint32_t name_length = strlen(traits::name<PointT, U>::value);
52  length += name_length + 13;
53  }
54 
55  uint32_t length;
56  };
57  } // namespace pcl::detail
58 } // namespace pcl
59 
60 namespace ros
61 {
62  // In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
63  // on the subscriber side. This allows us to generate the mapping between message
64  // data and object fields only once and reuse it.
65 #if ROS_VERSION_MINIMUM(1, 3, 1)
66  template<typename T>
67  struct DefaultMessageCreator<pcl::PointCloud<T> >
68  {
70 
71  DefaultMessageCreator()
72  : mapping_( boost::make_shared<pcl::MsgFieldMap>() )
73  {
74  }
75 
77  {
78  boost::shared_ptr<pcl::PointCloud<T> > msg (new pcl::PointCloud<T> ());
79  pcl::detail::getMapping(*msg) = mapping_;
80  return msg;
81  }
82  };
83 #endif
84 
85  namespace message_traits
86  {
87  template<typename T> struct MD5Sum<pcl::PointCloud<T> >
88  {
89  static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
90  static const char* value(const pcl::PointCloud<T>&) { return value(); }
91 
92  static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
93  static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
94 
95  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
96  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
97  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
98  };
99 
100  template<typename T> struct DataType<pcl::PointCloud<T> >
101  {
102  static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
103  static const char* value(const pcl::PointCloud<T>&) { return value(); }
104  };
105 
106  template<typename T> struct Definition<pcl::PointCloud<T> >
107  {
108  static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
109  static const char* value(const pcl::PointCloud<T>&) { return value(); }
110  };
111 
112  // pcl point clouds message don't have a ROS compatible header
113  // the specialized meta functions below (TimeStamp and FrameId)
114  // can be used to get the header data.
115  template<typename T> struct HasHeader<pcl::PointCloud<T> > : FalseType {};
116 
117  template<typename T>
118  struct TimeStamp<pcl::PointCloud<T> >
119  {
120  // This specialization could be dangerous, but it's the best I can do.
121  // If this TimeStamp struct is destroyed before they are done with the
122  // pointer returned by the first functions may go out of scope, but there
123  // isn't a lot I can do about that. This is a good reason to refuse to
124  // returning pointers like this...
125  static ros::Time* pointer(typename pcl::PointCloud<T> &m) {
126  header_.reset(new std_msgs::Header());
127  pcl_conversions::fromPCL(m.header, *(header_));
128  return &(header_->stamp);
129  }
130  static ros::Time const* pointer(const typename pcl::PointCloud<T>& m) {
131  header_const_.reset(new std_msgs::Header());
132  pcl_conversions::fromPCL(m.header, *(header_const_));
133  return &(header_const_->stamp);
134  }
135  static ros::Time value(const typename pcl::PointCloud<T>& m) {
136  return pcl_conversions::fromPCL(m.header).stamp;
137  }
138  private:
141  };
142 
143  template<typename T>
144  struct FrameId<pcl::PointCloud<T> >
145  {
146  static std::string* pointer(pcl::PointCloud<T>& m) { return &m.header.frame_id; }
147  static std::string const* pointer(const pcl::PointCloud<T>& m) { return &m.header.frame_id; }
148  static std::string value(const pcl::PointCloud<T>& m) { return m.header.frame_id; }
149  };
150 
151  } // namespace ros::message_traits
152 
153  namespace serialization
154  {
155  template<typename T>
156  struct Serializer<pcl::PointCloud<T> >
157  {
158  template<typename Stream>
159  inline static void write(Stream& stream, const pcl::PointCloud<T>& m)
160  {
161  stream.next(m.header);
162 
163  // Ease the user's burden on specifying width/height for unorganized datasets
164  uint32_t height = m.height, width = m.width;
165  if (height == 0 && width == 0) {
166  width = m.points.size();
167  height = 1;
168  }
169  stream.next(height);
170  stream.next(width);
171 
172  // Stream out point field metadata
173  typedef typename pcl::traits::fieldList<T>::type FieldList;
174  uint32_t fields_size = boost::mpl::size<FieldList>::value;
175  stream.next(fields_size);
176  pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
177 
178  // Assume little-endian...
179  uint8_t is_bigendian = false;
180  stream.next(is_bigendian);
181 
182  // Write out point data as binary blob
183  uint32_t point_step = sizeof(T);
184  stream.next(point_step);
185  uint32_t row_step = point_step * width;
186  stream.next(row_step);
187  uint32_t data_size = row_step * height;
188  stream.next(data_size);
189  memcpy(stream.advance(data_size), &m.points[0], data_size);
190 
191  uint8_t is_dense = m.is_dense;
192  stream.next(is_dense);
193  }
194 
195  template<typename Stream>
196  inline static void read(Stream& stream, pcl::PointCloud<T>& m)
197  {
198  std_msgs::Header header;
199  stream.next(header);
200  pcl_conversions::toPCL(header, m.header);
201  stream.next(m.height);
202  stream.next(m.width);
203 
205  std::vector<sensor_msgs::PointField> fields;
206  stream.next(fields);
207 
208  // Construct field mapping if deserializing for the first time
209  boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
210  if (!mapping_ptr)
211  {
212  // This normally should get allocated by DefaultMessageCreator, but just in case
213  mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
214  }
215  pcl::MsgFieldMap& mapping = *mapping_ptr;
216  if (mapping.empty())
217  pcl::createMapping<T> (fields, mapping);
218 
219  uint8_t is_bigendian;
220  stream.next(is_bigendian); // ignoring...
221  uint32_t point_step, row_step;
222  stream.next(point_step);
223  stream.next(row_step);
224 
225  // Copy point data
226  uint32_t data_size;
227  stream.next(data_size);
228  assert(data_size == m.height * m.width * point_step);
229  m.points.resize(m.height * m.width);
230  uint8_t* m_data = reinterpret_cast<uint8_t*>(&m.points[0]);
231  // If the data layouts match, can copy a whole row in one memcpy
232  if (mapping.size() == 1 &&
233  mapping[0].serialized_offset == 0 &&
234  mapping[0].struct_offset == 0 &&
235  point_step == sizeof(T))
236  {
237  uint32_t m_row_step = sizeof(T) * m.width;
238  // And if the row steps match, can copy whole point cloud in one memcpy
239  if (m_row_step == row_step)
240  {
241  memcpy (m_data, stream.advance(data_size), data_size);
242  }
243  else
244  {
245  for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
246  memcpy (m_data, stream.advance(row_step), m_row_step);
247  }
248  }
249  else
250  {
251  // If not, do a lot of memcpys to copy over the fields
252  for (uint32_t row = 0; row < m.height; ++row) {
253  const uint8_t* stream_data = stream.advance(row_step);
254  for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
255  BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
256  memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
257  }
258  m_data += sizeof(T);
259  }
260  }
261  }
262 
263  uint8_t is_dense;
264  stream.next(is_dense);
265  m.is_dense = is_dense;
266  }
267 
268  inline static uint32_t serializedLength(const pcl::PointCloud<T>& m)
269  {
270  uint32_t length = 0;
271 
272  length += serializationLength(m.header);
273  length += 8; // height/width
274 
276  typedef typename pcl::traits::fieldList<T>::type FieldList;
277  pcl::for_each_type<FieldList>(boost::ref(fl));
278  length += 4; // size of 'fields'
279  length += fl.length;
280 
281  length += 1; // is_bigendian
282  length += 4; // point_step
283  length += 4; // row_step
284  length += 4; // size of 'data'
285  length += m.points.size() * sizeof(T); // data
286  length += 1; // is_dense
287 
288  return length;
289  }
290  };
291  } // namespace ros::serialization
292 
294 
295 } // namespace ros
296 
297 #endif
static ros::Time * pointer(typename pcl::PointCloud< T > &m)
Definition: point_cloud.h:125
static ros::Time value(const typename pcl::PointCloud< T > &m)
Definition: point_cloud.h:135
static void write(Stream &stream, const pcl::PointCloud< T > &m)
Definition: point_cloud.h:159
static boost::shared_ptr< std_msgs::Header > header_const_
Definition: point_cloud.h:140
sensor_msgs::PointCloud2 PointCloud
Definition: bag_to_pcd.cpp:58
ROS_FORCE_INLINE uint8_t * advance(uint32_t len)
static const char * value(const pcl::PointCloud< T > &)
Definition: point_cloud.h:103
static uint32_t serializedLength(const pcl::PointCloud< T > &m)
Definition: point_cloud.h:268
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
#define ROS_STATIC_ASSERT(cond)
static std::string * pointer(pcl::PointCloud< T > &m)
Definition: point_cloud.h:146
FieldStreamer(Stream &stream)
Definition: point_cloud.h:21
static std::string const * pointer(const pcl::PointCloud< T > &m)
Definition: point_cloud.h:147
static boost::shared_ptr< std_msgs::Header > header_
Definition: point_cloud.h:139
static ros::Time const * pointer(const typename pcl::PointCloud< T > &m)
Definition: point_cloud.h:130
static const char * value(const pcl::PointCloud< T > &)
Definition: point_cloud.h:90
static std::string value(const pcl::PointCloud< T > &m)
Definition: point_cloud.h:148
static void read(Stream &stream, pcl::PointCloud< T > &m)
Definition: point_cloud.h:196
uint32_t serializationLength(const T &t)
static const char * value(const pcl::PointCloud< T > &)
Definition: point_cloud.h:109
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
const std::string header
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Jun 5 2019 19:52:52