fix race conditions when updating PlanningScene (moveit#349)

* PSM::waitForCurrentRobotState() + PSM::syncSceneUpdates()

* renamed wall_last_state_update_ to last_robot_state_update_wall_time_

* removed PSM::syncSceneUpdates() (and PSM::spinner_, PSM::callback_queue_)

Due to an upstream bug, it's not possible to start multiple AsyncSpinners from different threads.
Filed PR: ros/ros_comm#867

The spinner is now only needed to serve our own callback_queue_ for
scene updates, which is only required for syncSceneUpdates() that
syncs all kind of scene updates, not only the robot state.

* rviz: execute state update in background

... because we might wait up to 1s for a robot state update

* add robot_state update test

* waitForRobotToStop()

* Revert "wait a second before updating "current" in RViz (moveit#291)"

This reverts commit e3ef9a6.

* addressed Dave's comments

Conflicts:
	moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h
	moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
	moveit_ros/planning_interface/move_group_interface/src/move_group.cpp
	moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp

@@ -335,6 +335,8 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_ms
 {
   setPickupState(PLANNING);
 
+  // before we start planning, ensure that we have the latest robot state received...
+  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
   context_->planning_scene_monitor_->updateFrameTransforms();
 
   moveit_msgs::PickupGoalConstPtr goal;
@@ -380,6 +382,8 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msg
 {
   setPlaceState(PLANNING);
 
+  // before we start planning, ensure that we have the latest robot state received...
+  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
   context_->planning_scene_monitor_->updateFrameTransforms();
 
   moveit_msgs::PlaceResult action_res;

moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

@@ -59,6 +59,8 @@ void move_group::MoveGroupMoveAction::initialize()
 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
 {
   setMoveState(PLANNING);
+  // before we start planning, ensure that we have the latest robot state received...
+  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
   context_->planning_scene_monitor_->updateFrameTransforms();
 
   moveit_msgs::MoveGroupResult action_res;

moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp

@@ -52,6 +52,8 @@ bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotion
                                                           moveit_msgs::GetMotionPlan::Response& res)
 {
   ROS_INFO("Received new planning service request...");
+  // before we start planning, ensure that we have the latest robot state received...
+  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
   context_->planning_scene_monitor_->updateFrameTransforms();
 
   bool solved = false;

moveit_ros/planning/CMakeLists.txt

@@ -5,7 +5,7 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
 
-find_package(Boost REQUIRED system filesystem date_time program_options signals thread)
+find_package(Boost REQUIRED system filesystem date_time program_options signals thread chrono)
 find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_perception

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h

@@ -45,6 +45,7 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/thread/mutex.hpp>
 #include <moveit/macros/deprecation.h>
+#include <boost/thread/condition_variable.hpp>
 
 namespace planning_scene_monitor
 {
@@ -135,8 +136,13 @@ class CurrentStateMonitor
    *  @return Returns the map from joint names to joint state values*/
   std::map<std::string, double> getCurrentStateValues() const;
 
-  /** @brief Wait for at most \e wait_time seconds until the complete current state is known. Return true if the full
-   * state is known */
+  /** @brief Wait for at most \e wait_time seconds (default 1s) for a robot state more recent than t
+   *  @return true on success, false if up-to-date robot state wasn't received within \e wait_time
+  */
+  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
+
+  /** @brief Wait for at most \e wait_time seconds until the complete robot state is known.
+      @return true if the full state is known */
   bool waitForCompleteState(double wait_time) const;
   /** replaced by waitForCompleteState, will be removed in L-turtle: function waits for complete robot state */
   MOVEIT_DEPRECATED bool waitForCurrentState(double wait_time) const;
@@ -194,6 +200,7 @@ class CurrentStateMonitor
   ros::Time last_tf_update_;
 
   mutable boost::mutex state_update_lock_;
+  mutable boost::condition_variable state_update_condition_;
   std::vector<JointStateUpdateCallback> update_callbacks_;
 };
 

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

@@ -355,6 +355,13 @@ class PlanningSceneMonitor : private boost::noncopyable
   /** @brief This function is called every time there is a change to the planning scene */
   void triggerSceneUpdateEvent(SceneUpdateType update_type);
 
+  /** \brief Wait for robot state to become more recent than time t.
+   *
+   * If there is no state monitor active, there will be no scene updates.
+   * Hence, you can specify a timeout to wait for those updates. Default is 1s.
+   */
+  bool waitForCurrentRobotState(const ros::Time &t, double wait_time = 1.);
+
   /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */
   void lockSceneRead();
 
@@ -432,6 +439,9 @@ class PlanningSceneMonitor : private boost::noncopyable
   planning_scene::PlanningSceneConstPtr scene_const_;
   planning_scene::PlanningScenePtr parent_scene_;  /// if diffs are monitored, this is the pointer to the parent scene
   boost::shared_mutex scene_update_mutex_;         /// mutex for stored scene
+  ros::Time last_update_time_;                     /// Last time the state was updated
+  ros::Time last_robot_motion_time_;               /// Last time the robot has moved
+  bool enforce_next_state_update_;                 /// flag to enforce immediate state update in onStateUpdate()
 
   ros::NodeHandle nh_;
   ros::NodeHandle root_nh_;
@@ -492,7 +502,6 @@ class PlanningSceneMonitor : private boost::noncopyable
   boost::recursive_mutex update_lock_;
   std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;  /// List of callbacks to trigger when updates
                                                                            /// are received
-  ros::Time last_update_time_;                                             /// Last time the state was updated
 
 private:
   void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
@@ -532,7 +541,7 @@ class PlanningSceneMonitor : private boost::noncopyable
 
   /// Last time the state was updated from current_state_monitor_
   // Only access this from callback functions (and constructor)
-  ros::WallTime last_state_update_;
+  ros::WallTime last_robot_state_update_wall_time_;
 
   robot_model_loader::RobotModelLoaderPtr rm_loader_;
   robot_model::RobotModelConstPtr robot_model_;

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -261,6 +261,23 @@ bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::D
   return result;
 }
 
+bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
+{
+  ros::WallTime start = ros::WallTime::now();
+  ros::WallDuration elapsed(0, 0);
+  ros::WallDuration timeout(wait_time);
+
+  boost::mutex::scoped_lock lock(state_update_lock_);
+  while (current_state_time_ < t)
+  {
+    state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
+    elapsed = ros::WallTime::now() - start;
+    if (elapsed > timeout)
+      return false;
+  }
+  return true;
+}
+
 bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(double wait_time) const
 {
   double slept_time = 0.0;
@@ -403,4 +420,7 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso
   if (update)
     for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
       update_callbacks_[i](joint_state);
+
+  // notify waitForCurrentState() *after* potential update callbacks
+  state_update_condition_.notify_all();
 }

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -158,6 +158,7 @@ planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor()
   stopStateMonitor();
   stopWorldGeometryMonitor();
   stopSceneMonitor();
+
   delete reconfigure_impl_;
   current_state_monitor_.reset();
   scene_const_.reset();
@@ -171,6 +172,7 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
 {
   moveit::tools::Profiler::ScopedStart prof_start;
   moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
+  enforce_next_state_update_ = false;
 
   if (monitor_name_.empty())
     monitor_name_ = "planning_scene_monitor";
@@ -228,8 +230,8 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
   publish_planning_scene_frequency_ = 2.0;
   new_scene_update_ = UPDATE_NONE;
 
-  last_update_time_ = ros::Time::now();
-  last_state_update_ = ros::WallTime::now();
+  last_update_time_ = last_robot_motion_time_ = ros::Time::now();
+  last_robot_state_update_wall_time_ = ros::WallTime::now();
   dt_state_update_ = ros::WallDuration(0.1);
 
   double temp_wait_time = 0.05;
@@ -383,6 +385,8 @@ void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
               lock = octomap_monitor_->getOcTreePtr()->reading();
             scene_->getPlanningSceneMsg(msg);
           }
+          // also publish timestamp of this robot_state
+          msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
           publish_msg = true;
         }
         new_scene_update_ = UPDATE_NONE;
@@ -511,6 +515,10 @@ bool planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const
     boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
 
     last_update_time_ = ros::Time::now();
+    last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
+    ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
+                           "scene update " << fmod(last_update_time_.toSec(), 10.)
+                                           << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
     old_scene_name = scene_->getName();
     result = scene_->usePlanningSceneMsg(scene);
     if (octomap_monitor_)
@@ -850,6 +858,56 @@ void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallb
   }
 }
 
+bool planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState(const ros::Time &t, double wait_time)
+{
+  if (t.isZero())
+    return false;
+  ros::WallTime start = ros::WallTime::now();
+  ros::WallDuration timeout(wait_time);
+
+  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
+
+  if (current_state_monitor_)
+  {
+    // Wait for next robot update in state monitor. Those updates are only passed to PSM when robot actually moved!
+    enforce_next_state_update_ = true;  // enforce potential updates to be directly applied
+    bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
+    enforce_next_state_update_ =
+        false;  // back to normal throttling behavior, not applying incoming updates immediately
+
+    /* If the robot doesn't move, we will never receive an update from CSM in planning scene.
+       As we ensured that an update, if it is triggered by CSM, is directly passed to the scene,
+       we can early return true here (if we successfully received a CSM update). Otherwise return false. */
+    if (success)
+      return true;
+
+    ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
+    return false;
+  }
+
+  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
+  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
+  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
+  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
+  ros::Time prev_robot_motion_time = last_robot_motion_time_;
+  while (last_robot_motion_time_ < t &&  // Wait until the state update actually reaches the scene.
+         timeout > ros::WallDuration())
+  {
+    ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
+    new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
+    timeout -= ros::WallTime::now() - start;  // compute remaining wait_time
+  }
+  bool success = last_robot_motion_time_ >= t;
+  // suppress warning if we received an update at all
+  if (!success && prev_robot_motion_time != last_robot_motion_time_)
+    ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
+                   (t - last_robot_motion_time_).toSec());
+
+  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
+                                                              << " scene update: " << (t - last_update_time_).toSec());
+  return success;
+}
+
 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
 {
   scene_update_mutex_.lock_shared();
@@ -1036,7 +1094,7 @@ void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::
   if (scene_)
   {
     if (!current_state_monitor_)
-      current_state_monitor_.reset(new CurrentStateMonitor(getRobotModel(), tf_));
+      current_state_monitor_.reset(new CurrentStateMonitor(getRobotModel(), tf_, root_nh_));
     current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
     current_state_monitor_->startStateMonitor(joint_states_topic);
 
@@ -1078,24 +1136,23 @@ void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(
     const sensor_msgs::JointStateConstPtr& /* joint_state */)
 {
   const ros::WallTime& n = ros::WallTime::now();
-  ros::WallDuration dt = n - last_state_update_;
+  ros::WallDuration dt = n - last_robot_state_update_wall_time_;
 
-  bool update = false;
+  bool update = enforce_next_state_update_;
   {
     boost::mutex::scoped_lock lock(state_pending_mutex_);
 
-    if (dt < dt_state_update_)
+    if (dt < dt_state_update_ && !update)
     {
       state_update_pending_ = true;
     }
     else
     {
       state_update_pending_ = false;
-      last_state_update_ = n;
+      last_robot_state_update_wall_time_ = n;
       update = true;
     }
   }
-
   // run the state update with state_pending_mutex_ unlocked
   if (update)
     updateSceneWithCurrentState();
@@ -1108,22 +1165,27 @@ void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback(cons
     bool update = false;
 
     const ros::WallTime& n = ros::WallTime::now();
-    ros::WallDuration dt = n - last_state_update_;
+    ros::WallDuration dt = n - last_robot_state_update_wall_time_;
 
     {
       // 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();
+        last_robot_state_update_wall_time_ = ros::WallTime::now();
         update = true;
+        ROS_DEBUG_STREAM_NAMED(LOGNAME,
+                               "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
       }
     }
 
     // run the state update with state_pending_mutex_ unlocked
     if (update)
+    {
       updateSceneWithCurrentState();
+      ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
+    }
   }
 }
 
@@ -1191,8 +1253,9 @@ void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()
 
     {
       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
+      last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime();
+      ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
       current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
-      last_update_time_ = ros::Time::now();
       scene_->getCurrentStateNonConst().update();  // compute all transforms
     }
     triggerSceneUpdateEvent(UPDATE_STATE);

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -293,6 +293,7 @@ class TrajectoryExecutionManager
   void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
                      bool auto_clear);
   bool executePart(std::size_t part_index);
+  bool waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time = 1.0);
   void continuousExecutionThread();
 
   void stopExecutionInternal();