diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index b0dfe433..03c925de 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -5,7 +5,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -109,6 +110,61 @@ namespace ros }; template struct HasHeader > : TrueType {}; + + // This is bad because it will only work for pcl::PointXYZ, but I can't + // find another way around ambiguous partial template specialization... + template<> + struct TimeStamp > + { + // This specialization could be dangerous, but it's the best I can do. + // If this TimeStamp struct is destroyed before they are done with the + // pointer returned by the first functions may go out of scope, but there + // isn't a lot I can do about that. This is a good reason to refuse to + // returning pointers like this... + static ros::Time* pointer(pcl::PointCloud &m) { + header_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_)); + return &(header_->stamp); + } + static ros::Time const* pointer(const pcl::PointCloud& m) { + header_const_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_const_)); + return &(header_const_->stamp); + } + static ros::Time value(const pcl::PointCloud& m) { + return pcl_conversions::fromPCL(m.header).stamp; + } + private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; + }; + + template<> + struct TimeStamp > + { + // This specialization could be dangerous, but it's the best I can do. + // If this TimeStamp struct is destroyed before they are done with the + // pointer returned by the first functions may go out of scope, but there + // isn't a lot I can do about that. This is a good reason to refuse to + // returning pointers like this... + static ros::Time* pointer(pcl::PointCloud &m) { + header_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_)); + return &(header_->stamp); + } + static ros::Time const* pointer(const pcl::PointCloud& m) { + header_const_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_const_)); + return &(header_const_->stamp); + } + static ros::Time value(const pcl::PointCloud& m) { + return pcl_conversions::fromPCL(m.header).stamp; + } + private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; + }; + } // namespace ros::message_traits namespace serialization @@ -156,7 +212,9 @@ namespace ros template inline static void read(Stream& stream, pcl::PointCloud& m) { - stream.next(m.header); + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + stream.next(header); stream.next(m.height); stream.next(m.width);