Adding Max update rate parameter for octomap updaters (moveit#920)

* adding the max update rate parameter to the point cloud octomap updater

* adding the max update rate parameter to the depth image octomap updater

* made max_update_rate parameter optional

* walltime to time

moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h

@@ -78,6 +78,8 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
   image_transport::CameraPublisher pub_filtered_depth_image_;
   image_transport::CameraPublisher pub_filtered_label_image_;
 
+  ros::Time last_update_time_;
+
   std::string filtered_cloud_topic_;
   std::string sensor_type_;
   std::string image_topic_;
@@ -87,6 +89,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
   double shadow_threshold_;
   double padding_scale_;
   double padding_offset_;
+  double max_update_rate_;
   unsigned int skip_vertical_pixels_;
   unsigned int skip_horizontal_pixels_;
 

moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -59,6 +59,7 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater()
   , shadow_threshold_(0.04)
   , padding_scale_(0.0)
   , padding_offset_(0.02)
+  , max_update_rate_(0)
   , skip_vertical_pixels_(4)
   , skip_horizontal_pixels_(6)
   , image_callback_count_(0)
@@ -93,6 +94,8 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
     readXmlParam(params, "shadow_threshold", &shadow_threshold_);
     readXmlParam(params, "padding_scale", &padding_scale_);
     readXmlParam(params, "padding_offset", &padding_offset_);
+    if (params.hasMember("max_update_rate"))
+      readXmlParam(params, "max_update_rate", &max_update_rate_);
     readXmlParam(params, "skip_vertical_pixels", &skip_vertical_pixels_);
     readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_);
     if (params.hasMember("filtered_cloud_topic"))
@@ -207,9 +210,16 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
 {
   ROS_DEBUG("Received a new depth image message (frame = '%s', encoding='%s')", depth_msg->header.frame_id.c_str(),
             depth_msg->encoding.c_str());
-
   ros::WallTime start = ros::WallTime::now();
 
+  if (max_update_rate_ > 0)
+  {
+    // ensure we are not updating the octomap representation too often
+    if (ros::Time::now() - last_update_time_ <= ros::Duration(1.0 / max_update_rate_))
+      return;
+    last_update_time_ = ros::Time::now();
+  }
+
   // measure the frequency at which we receive updates
   if (image_callback_count_ < 1000)
   {

moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h

@@ -76,12 +76,15 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater
   ros::NodeHandle private_nh_;
   boost::shared_ptr<tf::Transformer> tf_;
 
+  ros::Time last_update_time_;
+
   /* params */
   std::string point_cloud_topic_;
   double scale_;
   double padding_;
   double max_range_;
   unsigned int point_subsample_;
+  double max_update_rate_;
   std::string filtered_cloud_topic_;
   ros::Publisher filtered_cloud_publisher_;
 

moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp

@@ -52,6 +52,7 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater()
   , padding_(0.0)
   , max_range_(std::numeric_limits<double>::infinity())
   , point_subsample_(1)
+  , max_update_rate_(0)
   , point_cloud_subscriber_(NULL)
   , point_cloud_filter_(NULL)
 {
@@ -74,6 +75,8 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
     readXmlParam(params, "padding_offset", &padding_);
     readXmlParam(params, "padding_scale", &scale_);
     readXmlParam(params, "point_subsample", &point_subsample_);
+    if (params.hasMember("max_update_rate"))
+      readXmlParam(params, "max_update_rate", &max_update_rate_);
     if (params.hasMember("filtered_cloud_topic"))
       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
   }
@@ -168,6 +171,14 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::
   ROS_DEBUG("Received a new point cloud message");
   ros::WallTime start = ros::WallTime::now();
 
+  if (max_update_rate_ > 0)
+  {
+    // ensure we are not updating the octomap representation too often
+    if (ros::Time::now() - last_update_time_ <= ros::Duration(1.0 / max_update_rate_))
+      return;
+    last_update_time_ = ros::Time::now();
+  }
+
   if (monitor_->getMapFrame().empty())
     monitor_->setMapFrame(cloud_msg->header.frame_id);