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
00037
00046 #include <ros/ros.h>
00047 #include <boost/thread.hpp>
00048 #include <sensor_msgs/PointCloud.h>
00049 #include <sensor_msgs/PointCloud2.h>
00050 #include <sensor_msgs/point_cloud_conversion.h>
00051
00052 using namespace std;
00053
00054 class PointCloudConverter
00055 {
00056 private:
00057 boost::mutex m_mutex_;
00058
00059 ros::NodeHandle nh_;
00060 ros::Subscriber sub_points_, sub_points2_;
00061 ros::Publisher pub_points_, pub_points2_;
00062
00063 int queue_size_;
00064 string points_in_, points2_in_, points_out_, points2_out_;
00065
00066 public:
00068 PointCloudConverter () : nh_ ("~"), queue_size_ (100),
00069 points_in_ ("/points_in"), points2_in_ ("/points2_in"),
00070 points_out_ ("/points_out"), points2_out_ ("/points2_out")
00071 {
00072
00073 sub_points_ = nh_.subscribe (points_in_, queue_size_, &PointCloudConverter::cloud_cb_points, this);
00074 sub_points2_ = nh_.subscribe (points2_in_, queue_size_, &PointCloudConverter::cloud_cb_points2, this);
00075 pub_points_ = nh_.advertise<sensor_msgs::PointCloud> (points_out_, queue_size_);
00076 pub_points2_ = nh_.advertise<sensor_msgs::PointCloud2> (points2_out_, queue_size_);
00077 ROS_INFO ("PointCloudConverter initialized to transform from PointCloud (%s) to PointCloud2 (%s).", nh_.resolveName (points_in_).c_str (), nh_.resolveName (points2_out_).c_str ());
00078 ROS_INFO ("PointCloudConverter initialized to transform from PointCloud2 (%s) to PointCloud (%s).", nh_.resolveName (points2_in_).c_str (), nh_.resolveName (points_out_).c_str ());
00079 }
00080
00082
00085 inline std::string
00086 getFieldsList (const sensor_msgs::PointCloud2 &cloud)
00087 {
00088 std::string result;
00089 for (size_t i = 0; i < cloud.fields.size () - 1; ++i)
00090 result += cloud.fields[i].name + " ";
00091 result += cloud.fields[cloud.fields.size () - 1].name;
00092 return (result);
00093 }
00094
00096
00099 inline std::string
00100 getFieldsList (const sensor_msgs::PointCloud &points)
00101 {
00102 std::string result = "x y z";
00103 for (size_t i = 0; i < points.channels.size (); i++)
00104 result = result + " " + points.channels[i].name;
00105 return (result);
00106 }
00107
00109
00110 void
00111 cloud_cb_points2 (const sensor_msgs::PointCloud2ConstPtr &msg)
00112 {
00113 if (pub_points_.getNumSubscribers () <= 0)
00114 {
00115
00116 return;
00117 }
00118
00119 ROS_DEBUG ("[point_cloud_converter] Got a PointCloud2 with %d points (%s) on %s.", msg->width * msg->height, getFieldsList (*msg).c_str (), nh_.resolveName (points2_in_).c_str ());
00120
00121 sensor_msgs::PointCloud output;
00122
00123 if (!sensor_msgs::convertPointCloud2ToPointCloud (*msg, output))
00124 {
00125 ROS_ERROR ("[point_cloud_converter] Conversion from sensor_msgs::PointCloud2 to sensor_msgs::PointCloud failed!");
00126 return;
00127 }
00128 ROS_DEBUG ("[point_cloud_converter] Publishing a PointCloud with %d points on %s.", (int)output.points.size (), nh_.resolveName (points_out_).c_str ());
00129 pub_points_.publish (output);
00130 }
00131
00133
00134 void
00135 cloud_cb_points (const sensor_msgs::PointCloudConstPtr &msg)
00136 {
00137 if (pub_points2_.getNumSubscribers () <= 0)
00138 {
00139
00140 return;
00141 }
00142
00143 ROS_DEBUG ("[point_cloud_converter] Got a PointCloud with %d points (%s) on %s.", (int)msg->points.size (), getFieldsList (*msg).c_str (), nh_.resolveName (points_in_).c_str ());
00144
00145 sensor_msgs::PointCloud2 output;
00146
00147 if (!sensor_msgs::convertPointCloudToPointCloud2 (*msg, output))
00148 {
00149 ROS_ERROR ("[point_cloud_converter] Conversion from sensor_msgs::PointCloud to sensor_msgs::PointCloud2 failed!");
00150 return;
00151 }
00152 ROS_DEBUG ("[point_cloud_converter] Publishing a PointCloud2 with %d points on %s.", output.height * output.width, nh_.resolveName (points2_out_).c_str ());
00153 pub_points2_.publish (output);
00154 }
00155
00156 };
00157
00158
00159 int
00160 main (int argc, char** argv)
00161 {
00162
00163 ros::init (argc, argv, "point_cloud_converter", ros::init_options::AnonymousName);
00164
00165 PointCloudConverter p;
00166 ros::spin ();
00167
00168 return (0);
00169 }
00170