1 #ifndef pcl_ROS_POINT_CLOUD_H_ 2 #define pcl_ROS_POINT_CLOUD_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> 18 template<
typename Stream,
typename Po
intT>
25 const char* name = traits::name<PointT, U>::value;
26 uint32_t name_length = strlen(name);
29 memcpy(
stream_.advance(name_length), name, name_length);
31 uint32_t offset = traits::offset<PointT, U>::value;
34 uint8_t datatype = traits::datatype<PointT, U>::value;
37 uint32_t count = traits::datatype<PointT, U>::size;
44 template<
typename Po
intT>
51 uint32_t name_length = strlen(traits::name<PointT, U>::value);
52 length += name_length + 13;
65 #if ROS_VERSION_MINIMUM(1, 3, 1) 67 struct DefaultMessageCreator<pcl::
PointCloud<T> >
71 DefaultMessageCreator()
72 : mapping_( boost::make_shared<pcl::MsgFieldMap>() )
79 pcl::detail::getMapping(*msg) = mapping_;
85 namespace message_traits
90 static const char*
value(
const pcl::PointCloud<T>&) {
return value(); }
103 static const char*
value(
const pcl::PointCloud<T>&) {
return value(); }
109 static const char*
value(
const pcl::PointCloud<T>&) {
return value(); }
126 header_.reset(
new std_msgs::Header());
128 return &(header_->stamp);
131 header_const_.reset(
new std_msgs::Header());
133 return &(header_const_->stamp);
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; }
153 namespace serialization
158 template<
typename Stream>
159 inline static void write(
Stream& stream,
const pcl::PointCloud<T>& m)
161 stream.next(m.header);
164 uint32_t height = m.height, width = m.width;
165 if (height == 0 && width == 0) {
166 width = m.points.size();
173 typedef typename pcl::traits::fieldList<T>::type FieldList;
174 uint32_t fields_size = boost::mpl::size<FieldList>::value;
175 stream.next(fields_size);
179 uint8_t is_bigendian =
false;
180 stream.next(is_bigendian);
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);
191 uint8_t is_dense = m.is_dense;
192 stream.next(is_dense);
195 template<
typename Stream>
196 inline static void read(
Stream& stream, pcl::PointCloud<T>& m)
201 stream.next(m.height);
202 stream.next(m.width);
205 std::vector<sensor_msgs::PointField> fields;
213 mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
215 pcl::MsgFieldMap& mapping = *mapping_ptr;
217 pcl::createMapping<T> (fields, mapping);
219 uint8_t is_bigendian;
220 stream.next(is_bigendian);
221 uint32_t point_step, row_step;
222 stream.next(point_step);
223 stream.next(row_step);
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]);
232 if (mapping.size() == 1 &&
233 mapping[0].serialized_offset == 0 &&
234 mapping[0].struct_offset == 0 &&
235 point_step ==
sizeof(T))
237 uint32_t m_row_step =
sizeof(T) * m.width;
239 if (m_row_step == row_step)
241 memcpy (m_data, stream.
advance(data_size), data_size);
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);
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);
264 stream.next(is_dense);
265 m.is_dense = is_dense;
276 typedef typename pcl::traits::fieldList<T>::type FieldList;
277 pcl::for_each_type<FieldList>(boost::ref(fl));
285 length += m.points.size() *
sizeof(T);
static ros::Time * pointer(typename pcl::PointCloud< T > &m)
static ros::Time value(const typename pcl::PointCloud< T > &m)
static void write(Stream &stream, const pcl::PointCloud< T > &m)
static boost::shared_ptr< std_msgs::Header > header_const_
sensor_msgs::PointCloud2 PointCloud
ROS_FORCE_INLINE uint8_t * advance(uint32_t len)
static const char * value(const pcl::PointCloud< T > &)
static const char * value()
static uint32_t serializedLength(const pcl::PointCloud< T > &m)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
#define ROS_STATIC_ASSERT(cond)
static std::string * pointer(pcl::PointCloud< T > &m)
FieldStreamer(Stream &stream)
static const char * value()
static std::string const * pointer(const pcl::PointCloud< T > &m)
static boost::shared_ptr< std_msgs::Header > header_
static ros::Time const * pointer(const typename pcl::PointCloud< T > &m)
static const char * value(const pcl::PointCloud< T > &)
static std::string value(const pcl::PointCloud< T > &m)
static void read(Stream &stream, pcl::PointCloud< T > &m)
uint32_t serializationLength(const T &t)
static const char * value()
static const char * value(const pcl::PointCloud< T > &)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)