cherry-pick moveit#824 from kinetic-devel

explicitly enforce updateSceneWithCurrentState() in waitForCurrentRobotState()
ubi-agni · Apr 19, 2018 · 053eb7d · 053eb7d
1 parent 558b66a
commit 053eb7d
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 1 deletion.
diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
@@ -137,7 +137,7 @@ void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor()
   if (state_monitor_started_)
   {
     joint_state_subscriber_.shutdown();
-    ROS_DEBUG("No longer listening o joint states");
+    ROS_DEBUG("No longer listening for joint states");
     state_monitor_started_ = false;
   }
 }

diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -860,9 +860,24 @@ bool planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState(cons
 
   if (current_state_monitor_)
   {
+    // Wait for next robot update in state monitor.
     bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
+
+    /* As robot updates are passed to the planning scene only in throttled fashion, there might
+       be still an update pending. If so, explicitly update the planning scene here.
+       If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
     if (success)
+    {
+      boost::mutex::scoped_lock lock(state_pending_mutex_);
+      if (state_update_pending_)  // enforce state update
+      {
+        state_update_pending_ = false;
+        last_robot_state_update_wall_time_ = ros::WallTime::now();
+        lock.unlock();
+        updateSceneWithCurrentState();
+      }
       return true;
+    }
 
     ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
     return false;