planning_scene_monitor: fix state update bug

The rate of state updates is limited to dt_state_update_ per second.
When an update arrived it was not processed if another was recently
processed.  This meant that if a quick sequence of state updates
arrived and then no updates arrive for a while that the last update(s)
were not seen until another arrives (which may be much later or
never). This fixes the bug by periodically checking for pending
updates and running them if they have been pending longer than
dt_state_update_.

planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -310,7 +310,7 @@ class PlanningSceneMonitor : private boost::noncopyable
   /** @brief Get the topic names that the monitor is listening to */
   void getMonitoredTopics(std::vector<std::string> &topics) const;
 
-  /** \brief Return the time when the last update was made to the planning scene (by the monitor) */
+  /** \brief Return the time when the last update was made to the planning scene (by \e any monitor) */
   const ros::Time& getLastUpdateTime() const
   {
     return last_update_time_;
@@ -450,15 +450,34 @@ class PlanningSceneMonitor : private boost::noncopyable
 
   void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped> &transforms);
 
+  // publish planning scene update diffs (runs in its own thread)
   void scenePublishingThread();
 
+  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
   void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state);
 
-  /// the amount of time to wait in between updates to the robot state (in seconds)
-  double dt_state_update_;
+  // called by state_update_timer_ when a state update it pending
+  void stateUpdateTimerCallback(const ros::WallTimerEvent& event);
 
-  /// the planning scene state is updated at a maximum specified frequency,
-  /// and this timestamp is used to implement that functionality
+
+  // Lock for state_update_pending_ and dt_state_update_
+  boost::mutex state_pending_mutex_;
+
+  /// True when we need to update the RobotState from current_state_monitor_
+  // This field is protected by state_pending_mutex_
+  volatile bool state_update_pending_;
+
+  /// the amount of time to wait in between updates to the robot state
+  // This field is protected by state_pending_mutex_
+  ros::WallDuration dt_state_update_;
+
+  /// timer for state updates.
+  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
+  // Not safe to access from callback functions.
+  ros::WallTimer state_update_timer_;
+
+  /// Last time the state was updated from current_state_monitor_
+  // Only access this from callback functions (and constructor)
   ros::WallTime last_state_update_;
 
   robot_model_loader::RobotModelLoaderPtr rm_loader_;

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -211,7 +211,13 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
 
   last_update_time_ = ros::Time::now();
   last_state_update_ = ros::WallTime::now();
-  dt_state_update_ = 0.1;
+  dt_state_update_ = ros::WallDuration(0.1);
+  state_update_pending_ = false;
+  state_update_timer_ = nh_.createWallTimer(dt_state_update_,
+                                            &PlanningSceneMonitor::stateUpdateTimerCallback,
+                                            this,
+                                            false,    // not a oneshot timer
+                                            false);   // do not start the timer yet
 
   reconfigure_impl_ = new DynamicReconfigureImpl(this);
 }
@@ -865,6 +871,12 @@ void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::
     current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
     current_state_monitor_->startStateMonitor(joint_states_topic);
 
+    {
+      boost::mutex::scoped_lock lock(state_pending_mutex_);
+      if (!dt_state_update_.isZero())
+        state_update_timer_.start();
+    }
+
     if (!attached_objects_topic.empty())
     {
       // using regular message filter as there's no header
@@ -882,16 +894,64 @@ void planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor()
     current_state_monitor_->stopStateMonitor();
   if (attached_collision_object_subscriber_)
     attached_collision_object_subscriber_.shutdown();
+
+  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
+  state_update_timer_.stop();
+  {
+    boost::mutex::scoped_lock lock(state_pending_mutex_);
+    state_update_pending_ = false;
+  }
 }
 
 void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr & /* joint_state */ )
 {
   const ros::WallTime &n = ros::WallTime::now();
-  const double t = (n - last_state_update_).toSec();
-  if (t >= dt_state_update_ && dt_state_update_ > std::numeric_limits<double>::epsilon())
+  ros::WallDuration dt = n - last_state_update_;
+
+  bool update = false;
   {
-    last_state_update_ = n;
+    boost::mutex::scoped_lock lock(state_pending_mutex_);
+
+    if (dt < dt_state_update_)
+    {
+      state_update_pending_ = true;
+    }
+    else
+    {
+      state_update_pending_ = false;
+      last_state_update_ = n;
+      update = true;
+    }
+  }
+
+  // run the state update with state_pending_mutex_ unlocked
+  if (update)
     updateSceneWithCurrentState();
+}
+
+void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& event)
+{
+  if (state_update_pending_)
+  {
+    bool update = false;
+
+    const ros::WallTime &n = ros::WallTime::now();
+    ros::WallDuration dt = n - last_state_update_;
+
+    {
+      // lock for access to dt_state_update_ and state_update_pending_
+      boost::mutex::scoped_lock lock(state_pending_mutex_);
+      if (state_update_pending_ && dt >= dt_state_update_)
+      {
+        state_update_pending_ = false;
+        last_state_update_ = ros::WallTime::now();
+        update = true;
+      }
+    }
+
+    // run the state update with state_pending_mutex_ unlocked
+    if (update)
+      updateSceneWithCurrentState();
   }
 }
 
@@ -918,11 +978,27 @@ void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback()
 
 void planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency(double hz)
 {
+  bool update = false;
   if (hz > std::numeric_limits<double>::epsilon())
-    dt_state_update_ = 1.0 / hz;
+  {
+    boost::mutex::scoped_lock lock(state_pending_mutex_);
+    dt_state_update_.fromSec(1.0 / hz);
+    state_update_timer_.setPeriod(dt_state_update_);
+    state_update_timer_.start();
+  }
   else
-    dt_state_update_ = 0.0;
-  ROS_INFO("Updating internal planning scene state at most every %lf seconds", dt_state_update_);
+  {
+    // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
+    state_update_timer_.stop(); 
+    boost::mutex::scoped_lock lock(state_pending_mutex_);
+    dt_state_update_ = ros::WallDuration(0,0);
+    if (state_update_pending_)
+      update = true;
+  }
+  ROS_INFO("Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
+
+  if (update)
+    updateSceneWithCurrentState();
 }
 
 void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()