Publish the pointcloud at the same rate

Author: plnegre

pointcloud_tools/src/pointcloud_mapper.cpp

@@ -5,6 +5,7 @@
 #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
@@ -31,15 +32,16 @@ class PointCloudMapper
     nh_priv_.param("voxel_size", voxel_size_, 0.1);
     nh_priv_.param("filter_map", filter_map_, false);
 
-    cloud_sub_ = nh_.subscribe<PointCloud>("input", 15, &PointCloudMapper::callback, this);
+    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(10.0), &PointCloudMapper::publishCallback, this);
   }
 
   void callback(const PointCloud::ConstPtr& cloud)
   {
-    ROS_INFO_STREAM("received cloud with " << cloud->points.size() << " points.");
+    m_.lock ();
+    ROS_INFO_STREAM("Received cloud with " << cloud->points.size() << " points.");
     PointCloud transformed_cloud;
     ros::Time points_time;
     points_time.fromNSec((*cloud).header.stamp);
@@ -48,27 +50,33 @@ class PointCloudMapper
     if (success)
     {
       accumulated_cloud_ += transformed_cloud;
+
+      if(filter_map_)
+      {
+        PointCloud::Ptr cloud_downsampled = filter(accumulated_cloud_.makeShared());
+        accumulated_cloud_ = *cloud_downsampled;
+      }
+
+      // Publish the cloud
+      if (cloud_pub_.getNumSubscribers() > 0)
+        cloud_pub_.publish(accumulated_cloud_);
+      last_pub_time_ = ros::WallTime::now();
     }
     else
     {
       ROS_ERROR("Could not transform point cloud to %s", fixed_frame_.c_str());
     }
 
-    if(filter_map_){
-      PointCloud::Ptr cloud_downsampled = filter(accumulated_cloud_.makeShared());
-      accumulated_cloud_ = *cloud_downsampled;
-    }
-
-
     ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");
+    m_.unlock();
   }
 
   void publishCallback(const ros::TimerEvent&)
   {
-    if (cloud_pub_.getNumSubscribers() > 0)
-    {
+    // Publish the accumulated cloud if last publication was more than 10 seconds before.
+    ros::WallDuration elapsed_time = ros::WallTime::now() - last_pub_time_;
+    if (cloud_pub_.getNumSubscribers() > 0 && elapsed_time.toSec() > 10.0)
       cloud_pub_.publish(accumulated_cloud_);
-    }
   }
 
   PointCloud::Ptr filter(PointCloud::Ptr cloud)
@@ -127,14 +135,16 @@ class PointCloudMapper
   ros::NodeHandle nh_;
   ros::NodeHandle nh_priv_;
 
+  ros::Timer pub_timer_;
+
   ros::Subscriber cloud_sub_;
   ros::Publisher cloud_pub_;
 
-  ros::Timer pub_timer_;
-
   std::string fixed_frame_;
   tf::TransformListener tf_listener_;
   PointCloud accumulated_cloud_;
+  boost::mutex m_;
+  ros::WallTime last_pub_time_;
 };
 
 int main(int argc, char **argv)

pointcloud_tools/src/pointcloud_viewer.cpp

@@ -48,8 +48,6 @@
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/features/feature.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/passthrough.h>
 
 using pcl::visualization::PointCloudColorHandlerGenericField;
 
@@ -60,38 +58,18 @@ typedef pcl::PointCloud<PointRGB> PointCloudRGB;
 
 // Global data
 sensor_msgs::PointCloud2ConstPtr cloud_, cloud_old_;
-boost::mutex m;
+boost::mutex m_;
 bool viewer_initialized_;
 bool save_cloud_;
 std::string pcd_filename_;
 int counter_;
 
 void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud)
 {
-  m.lock ();
-  printf("\rPointCloud with %d data points (%s), stamp %f, and frame %s.",
-      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
-      cloud->header.stamp.toSec(), cloud->header.frame_id.c_str());
+  m_.lock ();
+  ROS_INFO_STREAM("Pointcloud viewer has received a pointcloud.");
   cloud_ = cloud;
-  m.unlock();
-}
-
-PointCloudRGB::Ptr filter(PointCloudRGB::Ptr cloud, double voxel_size)
-{
-  PointCloudRGB::Ptr cloud_filtered_ptr(new PointCloudRGB);
-
-  // Downsampling using voxel grid
-  pcl::VoxelGrid<PointRGB> grid_;
-  PointCloudRGB::Ptr cloud_downsampled_ptr(new PointCloudRGB);
-
-  grid_.setLeafSize(voxel_size,
-                    voxel_size,
-                    voxel_size);
-  grid_.setDownsampleAllData(true);
-  grid_.setInputCloud(cloud);
-  grid_.filter(*cloud_downsampled_ptr);
-
-  return cloud_downsampled_ptr;
+  m_.unlock();
 }
 
 void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
@@ -111,7 +89,6 @@ void updateVisualization()
 
   ros::WallDuration d(0.01);
   bool rgb = false;
-  //std::vector<sensor_msgs::PointField> fields;
   std::vector<pcl::PCLPointField> fields;
 
   // Create the visualizer
@@ -133,7 +110,7 @@ void updateVisualization()
 
     if(cloud_old_ == cloud_)
       continue;
-    m.lock ();
+    m_.lock ();
 
     // Convert to PointCloud<T>
     if(pcl::getFieldIndex(*cloud_, "rgb") != -1)
@@ -148,7 +125,7 @@ void updateVisualization()
       pcl::getFields(cloud_xyz, fields);
     }
     cloud_old_ = cloud_;
-    m.unlock();
+    m_.unlock();
 
     // Delete the previous point cloud
     viewer.removePointCloud("cloud");