diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index 167537ee..711b8307 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -109,29 +109,30 @@ namespace ros static const char* value(const pcl::PointCloud&) { return value(); } }; - template struct HasHeader > : TrueType {}; + // pcl point clouds message don't have a ROS compatible header + // the specialized meta functions below (TimeStamp and FrameId) + // can be used to get the header data. + template struct HasHeader > : FalseType {}; - // 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 > + 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) { + static ros::Time* pointer(typename 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) { + static ros::Time const* pointer(const typename 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) { + static ros::Time value(const typename pcl::PointCloud& m) { return pcl_conversions::fromPCL(m.header).stamp; } private: @@ -139,30 +140,12 @@ namespace ros static boost::shared_ptr header_const_; }; - template<> - struct TimeStamp > + template + struct FrameId > { - // 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_; + static std::string* pointer(pcl::PointCloud& m) { return &m.header.frame_id; } + static std::string const* pointer(const pcl::PointCloud& m) { return &m.header.frame_id; } + static std::string value(const pcl::PointCloud& m) { return m.header.frame_id; } }; } // namespace ros::message_traits