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