Fix ros timers to work when bagfile finishes

Author: plnegre

pointcloud_tools/src/pointcloud_mapper.cpp

@@ -5,7 +5,6 @@
 #include <pcl/point_types.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/filters/passthrough.h>
-#include <boost/thread/mutex.hpp>
 
 /**
  * Stores incoming point clouds in a map transforming
@@ -35,12 +34,11 @@ class PointCloudMapper
     cloud_sub_ = nh_.subscribe<PointCloud>("input", 1, &PointCloudMapper::callback, this);
     bool latched = true;
     cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, latched);
-    pub_timer_ = nh_.createTimer(ros::Duration(3.0), &PointCloudMapper::publishCallback, this);
+    pub_timer_ = nh_.createWallTimer(ros::WallDuration(3.0), &PointCloudMapper::publishCallback, this);
   }
 
   void callback(const PointCloud::ConstPtr& cloud)
   {
-    m_.lock ();
     ROS_INFO_STREAM("Received cloud with " << cloud->points.size() << " points.");
     PointCloud transformed_cloud;
     ros::Time points_time;
@@ -68,10 +66,9 @@ class PointCloudMapper
     }
 
     ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");
-    m_.unlock();
   }
 
-  void publishCallback(const ros::TimerEvent&)
+  void publishCallback(const ros::WallTimerEvent&)
   {
     // Publish the accumulated cloud if last publication was more than 5 seconds before.
     ros::WallDuration elapsed_time = ros::WallTime::now() - last_pub_time_;
@@ -135,15 +132,14 @@ class PointCloudMapper
   ros::NodeHandle nh_;
   ros::NodeHandle nh_priv_;
 
-  ros::Timer pub_timer_;
+  ros::WallTimer pub_timer_;
 
   ros::Subscriber cloud_sub_;
   ros::Publisher cloud_pub_;
 
   std::string fixed_frame_;
   tf::TransformListener tf_listener_;
   PointCloud accumulated_cloud_;
-  boost::mutex m_;
   ros::WallTime last_pub_time_;
 };
 

pointcloud_tools/src/pointcloud_viewer.cpp

@@ -63,7 +63,7 @@ bool viewer_initialized_;
 bool save_cloud_;
 std::string pcd_filename_;
 int counter_;
-ros::Timer save_timer_;
+ros::WallTimer save_timer_;
 
 PointCloud cloud_xyz_;
 PointCloudRGB cloud_xyz_rgb_;
@@ -112,9 +112,9 @@ void updateVisualization()
 
     if(cloud_old_ == cloud_)
       continue;
-    m_.lock ();
 
     // Convert to PointCloud<T>
+    m_.lock ();
     if(pcl::getFieldIndex(*cloud_, "rgb") != -1)
     {
       rgb = true;
@@ -188,7 +188,7 @@ void updateVisualization()
   }
 }
 
-void saveCallback(const ros::TimerEvent&)
+void saveCallback(const ros::WallTimerEvent&)
 {
   if (!save_cloud_) return;
 
@@ -235,10 +235,8 @@ int main(int argc, char** argv)
   ROS_INFO("Subscribing to %s for PointCloud2 messages...", nh.resolveName ("input").c_str ());
 
   signal(SIGINT, sigIntHandler);
-
   boost::thread visualization_thread(&updateVisualization);
-
-  save_timer_ = nh.createTimer(ros::Duration(1), &saveCallback);
+  save_timer_ = nh.createWallTimer(ros::WallDuration(1), &saveCallback);
 
   // Spin
   ros::spin();