From c6a48a583f1b93eedbcc3d8d33290fb8a9f8b7fa Mon Sep 17 00:00:00 2001 From: Kai Franke Date: Wed, 20 Feb 2013 08:59:27 -0800 Subject: [PATCH] now also works without specifying publishing interval like described in the wiki. Also renamed rate_ to time_ since it is no rate --- pcl_ros/tools/pcd_to_pointcloud.cpp | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index b707557b..bbb3e769 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -65,7 +65,7 @@ class PCDGenerator sensor_msgs::PointCloud2 cloud_; string file_name_, cloud_topic_; - double rate_; + double wait_; pcl_ros::Publisher pub_; @@ -97,7 +97,7 @@ class PCDGenerator { int nr_points = cloud_.width * cloud_.height; string fields_list = pcl::getFieldsList (cloud_); - double interval = rate_ * 1e+6; + double interval = wait_ * 1e+6; while (nh_.ok ()) { 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 ()); @@ -110,17 +110,20 @@ class PCDGenerator } else { - ros::Duration (0.001).sleep (); + // check once a second if there is any subscriber + ros::Duration (1).sleep (); continue; } usleep (interval); - if (interval == 0) // We only publish once if a 0 seconds interval is given + if (interval == 0) // We only publish once if a 0 seconds interval is given + { + // Give subscribers 3 seconds until point cloud decays... a little ugly! + ros::Duration (3.0).sleep (); break; + } } - - ros::Duration (3.0).sleep (); return (true); } @@ -131,7 +134,7 @@ class PCDGenerator int main (int argc, char** argv) { - if (argc < 3) + if (argc < 2) { std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << std::endl; return (-1); @@ -141,7 +144,15 @@ int PCDGenerator c; c.file_name_ = string (argv[1]); - c.rate_ = atof (argv[2]); + // check if publishing interval is given + if (argc == 2) + { + c.wait_ = 0; + } + else + { + c.wait_ = atof (argv[2]); + } if (c.start () == -1) {