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
00037
00046
00047 #include <ros/ros.h>
00048 #include <pcl/io/io.h>
00049 #include <pcl/io/pcd_io.h>
00050 #include <pcl/point_types.h>
00051
00052 #include "pcl_ros/publisher.h"
00053
00054 using namespace std;
00055
00056 class PCDGenerator
00057 {
00058 protected:
00059 string tf_frame_;
00060 ros::NodeHandle nh_;
00061 ros::NodeHandle private_nh_;
00062 public:
00063
00064
00065 sensor_msgs::PointCloud2 cloud_;
00066
00067 string file_name_, cloud_topic_;
00068 double rate_;
00069
00070 pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
00071
00072 PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
00073 {
00074
00075
00076 cloud_topic_ = "cloud_pcd";
00077 pub_.advertise (nh_, cloud_topic_.c_str (), 1);
00078 private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
00079 ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
00080 }
00081
00082
00083 int start ()
00084 {
00085 if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
00086 return (-1);
00087 cloud_.header.frame_id = tf_frame_;
00088 return (0);
00089 }
00090
00091
00092 bool spin ()
00093 {
00094 int nr_points = cloud_.width * cloud_.height;
00095 string fields_list = pcl::getFieldsList (cloud_);
00096 double interval = rate_ * 1e+6;
00097 while (nh_.ok ())
00098 {
00099 ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
00100 cloud_.header.stamp = ros::Time::now ();
00101
00102 if (pub_.getNumSubscribers () > 0)
00103 {
00104 ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
00105 pub_.publish (cloud_);
00106 }
00107 else
00108 {
00109 ros::Duration (0.001).sleep ();
00110 continue;
00111 }
00112
00113 usleep (interval);
00114
00115 if (interval == 0)
00116 break;
00117 }
00118
00119 ros::Duration (3.0).sleep ();
00120 return (true);
00121 }
00122
00123
00124 };
00125
00126
00127 int
00128 main (int argc, char** argv)
00129 {
00130 if (argc < 3)
00131 {
00132 std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
00133 return (-1);
00134 }
00135
00136 ros::init (argc, argv, "pcd_to_pointcloud");
00137
00138 PCDGenerator c;
00139 c.file_name_ = string (argv[1]);
00140 c.rate_ = atof (argv[2]);
00141
00142 if (c.start () == -1)
00143 {
00144 ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
00145 return (-1);
00146 }
00147 ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ());
00148 c.spin ();
00149
00150 return (0);
00151 }
00152