Merge pull request moveit#716 from ros-planning/fix-#442

fix race conditions when updating PlanningScene state, fixes moveit#442

move_group/src/default_capabilities/execute_service_capability.cpp

@@ -86,14 +86,14 @@ bool move_group::MoveGroupExecuteService::executeTrajectoryService(moveit_msgs::
       moveit_controller_manager::ExecutionStatus es = context_->trajectory_execution_manager_->waitForExecution();
       if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
         res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
+      else if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
+        res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
+      else if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
+        res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
       else
-        if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
-          res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
-        else
-          if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
-            res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
-          else
-            res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
+        res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
+      // wait for all planning scene updates to be processed
+      context_->planning_scene_monitor_->syncSceneUpdates();
       ROS_INFO_STREAM("Execution completed: " << es.asString());
     }
     else

move_group/src/default_capabilities/move_action_capability.cpp

@@ -61,7 +61,6 @@ void move_group::MoveGroupMoveAction::initialize()
 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
 {
   setMoveState(PLANNING);
-  context_->planning_scene_monitor_->updateFrameTransforms();
 
   moveit_msgs::MoveGroupResult action_res;
   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
@@ -94,6 +93,7 @@ void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const m
 
   if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff))
   {
+    context_->planning_scene_monitor_->syncSceneUpdates();
     planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
     const robot_state::RobotState &current_state = lscene->getCurrentState();
 
@@ -141,6 +141,7 @@ void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_
 {
   ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");
 
+  context_->planning_scene_monitor_->syncSceneUpdates();
   planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); // lock the scene so that it does not modify the world representation while diff() is called
   const planning_scene::PlanningSceneConstPtr &the_scene = (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
     static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) : lscene->diff(goal->planning_options.planning_scene_diff);
@@ -169,7 +170,6 @@ bool move_group::MoveGroupMoveAction::planUsingPlanningPipeline(const planning_i
 {
   setMoveState(PLANNING);
 
-  planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
   bool solved = false;
   planning_interface::MotionPlanResponse res;
   try

move_group/src/default_capabilities/plan_service_capability.cpp

@@ -51,7 +51,7 @@ void move_group::MoveGroupPlanService::initialize()
 bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res)
 {
   ROS_INFO("Received new planning service request...");
-  context_->planning_scene_monitor_->updateFrameTransforms();
+  context_->planning_scene_monitor_->syncSceneUpdates();
 
   bool solved = false;
   planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);

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

planning/plan_execution/src/plan_execution.cpp

@@ -457,6 +457,8 @@ void plan_execution::PlanExecution::planningSceneUpdatedCallback(const planning_
 
 void plan_execution::PlanExecution::doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
 {
+  // sync all planning scene updates before continuing
+  planning_scene_monitor_->syncSceneUpdates();
   execution_complete_ = true;
 }
 

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

@@ -336,6 +336,9 @@ 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 until all pending scene updates with timestamps < t are incorporated */
+  void syncSceneUpdates(const ros::Time &t = ros::Time::now());
+
   /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */
   void lockSceneRead();
 
@@ -505,7 +508,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 wall_last_state_update_;
 
   robot_model_loader::RobotModelLoaderPtr rm_loader_;
   robot_model::RobotModelConstPtr robot_model_;
@@ -514,6 +517,11 @@ class PlanningSceneMonitor : private boost::noncopyable
 
   class DynamicReconfigureImpl;
   DynamicReconfigureImpl *reconfigure_impl_;
+
+  ros::CallbackQueue                    callback_queue_;
+  boost::scoped_ptr<ros::AsyncSpinner>  spinner_;
+  ros::Time                             last_robot_motion_time_; /// Last time the robot has moved
+  bool                                  enforce_next_state_update_;
 };
 
 typedef boost::shared_ptr<PlanningSceneMonitor> PlanningSceneMonitorPtr;

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -149,6 +149,8 @@ planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor()
   stopStateMonitor();
   stopWorldGeometryMonitor();
   stopSceneMonitor();
+  spinner_->stop();
+
   delete reconfigure_impl_;
   current_state_monitor_.reset();
   scene_const_.reset();
@@ -163,6 +165,11 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
   moveit::tools::Profiler::ScopedStart prof_start;
   moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
 
+  // start our own spinner listening on our own callback_queue to become independent of any global callback queue
+  root_nh_.setCallbackQueue(&callback_queue_);
+  spinner_.reset(new ros::AsyncSpinner(1 /* threads */, &callback_queue_));
+  spinner_->start();
+
   if (monitor_name_.empty())
     monitor_name_ = "planning_scene_monitor";
   robot_description_ = rm_loader_->getRobotDescription();
@@ -213,8 +220,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();
+  wall_last_state_update_ = ros::WallTime::now();
   dt_state_update_ = ros::WallDuration(0.1);
 
   double temp_wait_time;
@@ -225,6 +232,7 @@ void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_sce
   shape_transform_cache_lookup_wait_time_ = ros::Duration(temp_wait_time);
 
   state_update_pending_ = false;
+  enforce_next_state_update_ = false;
   state_update_timer_ = nh_.createWallTimer(dt_state_update_,
                                             &PlanningSceneMonitor::stateUpdateTimerCallback,
                                             this,
@@ -358,6 +366,8 @@ void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
             if (octomap_monitor_) 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;
@@ -491,6 +501,7 @@ 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;
     old_scene_name = scene_->getName();
     result = scene_->usePlanningSceneMsg(scene);
     if (octomap_monitor_)
@@ -808,6 +819,33 @@ void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallb
     }
 }
 
+void planning_scene_monitor::PlanningSceneMonitor::syncSceneUpdates(const ros::Time &t)
+{
+  if (t.isZero())
+    return;
+
+  enforce_next_state_update_ = true;  // enforce next state update to trigger without throttling
+
+  // Robot state updates in the scene are only triggered by the state monitor on changes of the state.
+  // Hence, last_state_update_time_ might be much older than current_state_monitor_ (when robot didn't moved for a while).
+  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
+  ros::Time last_robot_update = current_state_monitor_ ? current_state_monitor_->getCurrentStateTime() : ros::Time();
+  while (current_state_monitor_ &&                     // sanity check
+         last_robot_update < t &&                      // wait for recent state update
+         (t - last_robot_motion_time_).toSec() < 1.0)  // but only if robot moved in last second
+  {
+    new_scene_update_condition_.wait_for(lock, boost::chrono::milliseconds(50));
+    last_robot_update = current_state_monitor_->getCurrentStateTime();
+  }
+  // Now, we know that robot state is up-to-date
+
+  // ensure that last update time is more recent than t (or no more update events pending)
+  while (last_update_time_ < t && !callback_queue_.empty())
+  {
+    new_scene_update_condition_.wait_for(lock, boost::chrono::milliseconds(50));
+  }
+}
+
 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
 {
   scene_update_mutex_.lock_shared();
@@ -817,9 +855,9 @@ void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
 
 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead()
 {
-  scene_update_mutex_.unlock_shared();
   if (octomap_monitor_)
     octomap_monitor_->getOcTreePtr()->unlockRead();
+  scene_update_mutex_.unlock_shared();
 }
 
 void planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite()
@@ -831,9 +869,9 @@ void planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite()
 
 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite()
 {
-  scene_update_mutex_.unlock();
   if (octomap_monitor_)
     octomap_monitor_->getOcTreePtr()->unlockWrite();
+  scene_update_mutex_.unlock();
 }
 
 void planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor(const std::string &scene_topic)
@@ -979,7 +1017,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);
 
@@ -1018,24 +1056,23 @@ void planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor()
 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 - wall_last_state_update_;
 
-  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;
+      wall_last_state_update_ = n;
       update = true;
     }
   }
-
   // run the state update with state_pending_mutex_ unlocked
   if (update)
     updateSceneWithCurrentState();
@@ -1048,15 +1085,15 @@ 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 - wall_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();
+        wall_last_state_update_ = ros::WallTime::now();
         update = true;
       }
     }
@@ -1129,8 +1166,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();
       current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
-      last_update_time_ = ros::Time::now();
+      enforce_next_state_update_ = false;
       scene_->getCurrentStateNonConst().update(); // compute all transforms
     }
     triggerSceneUpdateEvent(UPDATE_STATE);

planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h

@@ -619,6 +619,9 @@ class MoveGroup
   /** \brief Given a \e plan, execute it while waiting for completion. Return true on success. */
   MoveItErrorCode execute(const Plan &plan);
 
+  /** \brief Validate that first point of given a \e plan matches current state of robot */
+  bool validatePlan(const Plan &plan);
+
   /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters
       between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the
       waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold

planning_interface/move_group_interface/src/move_group.cpp

@@ -686,6 +686,38 @@ class MoveGroup::MoveGroupImpl
     }
   }
 
+  bool validatePlan(const Plan &plan)
+  {
+    robot_state::RobotStatePtr current_state;
+    if (!getCurrentState(current_state))
+      return false;
+    if (plan.trajectory_.joint_trajectory.points.empty())
+      return true;
+
+    const trajectory_msgs::JointTrajectory &trajectory = plan.trajectory_.joint_trajectory;
+    const std::vector<double> &positions = trajectory.points.front().positions;
+    std::size_t n = trajectory.joint_names.size();
+    if (positions.size() != n)
+      return false;
+
+    for (std::size_t i = 0; i < n; ++i)
+    {
+      const robot_model::JointModel *jm = robot_model_->getJointModel(trajectory.joint_names[i]);
+      if (!jm)
+      {
+        ROS_ERROR_STREAM("Unknown joint in trajectory: " << trajectory.joint_names[i]);
+        return false;
+      }
+      // TODO: check multi-DoF joints
+      if (fabs(current_state->getJointPositions(jm)[0] - positions[i]) < std::numeric_limits<float>::epsilon())
+      {
+        ROS_ERROR("Trajectory start deviates from current robot state");
+        return false;
+      }
+    }
+    return true;
+  }
+
   double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double step, double jump_threshold,
                               moveit_msgs::RobotTrajectory &msg, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
   {
@@ -1175,6 +1207,11 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGrou
   return impl_->execute(plan, true);
 }
 
+bool moveit::planning_interface::MoveGroup::validatePlan(const moveit::planning_interface::MoveGroup::Plan &plan)
+{
+  return impl_->validatePlan(plan);
+}
+
 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan &plan)
 {
   return impl_->plan(plan);

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -139,7 +139,9 @@ void MotionPlanningFrame::computeExecuteButtonClicked()
   if (move_group_ && current_plan_)
   {
     ui_->stop_button->setEnabled(true); // enable stopping
-    bool success = move_group_->execute(*current_plan_);
+    bool success =
+      move_group_->validatePlan(*current_plan_) &&
+      move_group_->execute(*current_plan_);
     onFinishedExecution(success);
   }
 }
@@ -239,6 +241,7 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state,
 
   if (v == "<current>")
   {
+    planning_display_->syncSceneUpdates();
     const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
     if (ps)
       state = ps->getCurrentState();
@@ -402,6 +405,7 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo
 {
   if (move_group_ && planning_display_)
   {
+    planning_display_->syncSceneUpdates();
     robot_state::RobotState state = *planning_display_->getQueryStartState();
     const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
     if (ps)
@@ -416,6 +420,7 @@ void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyCon
 {
   if (move_group_ && planning_display_)
   {
+    planning_display_->syncSceneUpdates();
     robot_state::RobotState state = *planning_display_->getQueryStartState();
     const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
     if (ps)