Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00045 #ifndef PCL_ROS_PUBLISHER_H_
00046 #define PCL_ROS_PUBLISHER_H_
00047
00048 #include <ros/ros.h>
00049 #include <sensor_msgs/PointCloud2.h>
00050 #include <pcl/conversions.h>
00051
00052 #include <pcl_conversions/pcl_conversions.h>
00053
00054 namespace pcl_ros
00055 {
00056 class BasePublisher
00057 {
00058 public:
00059 void
00060 advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
00061 {
00062 pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
00063 }
00064
00065 std::string
00066 getTopic ()
00067 {
00068 return (pub_.getTopic ());
00069 }
00070
00071 uint32_t
00072 getNumSubscribers () const
00073 {
00074 return (pub_.getNumSubscribers ());
00075 }
00076
00077 void
00078 shutdown ()
00079 {
00080 pub_.shutdown ();
00081 }
00082
00083 operator void*() const
00084 {
00085 return (pub_) ? (void*)1 : (void*)0;
00086 }
00087
00088 protected:
00089 ros::Publisher pub_;
00090 };
00091
00092 template <typename PointT>
00093 class Publisher : public BasePublisher
00094 {
00095 public:
00096 Publisher () {}
00097
00098 Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
00099 {
00100 advertise (nh, topic, queue_size);
00101 }
00102
00103 ~Publisher () {}
00104
00105 inline void
00106 publish (const boost::shared_ptr<const pcl::PointCloud<PointT> > &point_cloud) const
00107 {
00108 publish (*point_cloud);
00109 }
00110
00111 inline void
00112 publish (const pcl::PointCloud<PointT>& point_cloud) const
00113 {
00114
00115 sensor_msgs::PointCloud2 msg;
00116 pcl::toROSMsg (point_cloud, msg);
00117 pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (msg));
00118 }
00119 };
00120
00121 template <>
00122 class Publisher<sensor_msgs::PointCloud2> : public BasePublisher
00123 {
00124 public:
00125 Publisher () {}
00126
00127 Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
00128 {
00129 advertise (nh, topic, queue_size);
00130 }
00131
00132 ~Publisher () {}
00133
00134 void
00135 publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const
00136 {
00137 pub_.publish (point_cloud);
00138
00139 }
00140
00141 void
00142 publish (const sensor_msgs::PointCloud2& point_cloud) const
00143 {
00144 pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud));
00145
00146 }
00147 };
00148 }
00149 #endif //#ifndef PCL_ROS_PUBLISHER_H_