Merged cherry-picks #713, #717, #716, #724 from jade-devel

move_group/src/default_capabilities/execute_service_capability.cpp

@@ -39,13 +39,29 @@
 #include <moveit/move_group/capability_names.h>
 
 move_group::MoveGroupExecuteService::MoveGroupExecuteService():
-  MoveGroupCapability("ExecutePathService")
+  MoveGroupCapability("ExecutePathService"),
+  callback_queue_(),
+  spinner_(1 /* spinner threads */, &callback_queue_)
 {
 }
 
+move_group::MoveGroupExecuteService::~MoveGroupExecuteService()
+{
+  spinner_.stop();
+}
+
 void move_group::MoveGroupExecuteService::initialize()
 {
-  execute_service_ = root_node_handle_.advertiseService(EXECUTE_SERVICE_NAME, &MoveGroupExecuteService::executeTrajectoryService, this);
+  // We need to serve each service request in a thread independent of the main spinner thread.
+  // Otherwise, a synchronous execution request (i.e. waiting for the execution to finish) would block
+  // execution of the main spinner thread.
+  // Hence, we use our own asynchronous spinner listening to our own callback queue.
+  ros::AdvertiseServiceOptions ops;
+  ops.template init<moveit_msgs::ExecuteKnownTrajectory::Request, moveit_msgs::ExecuteKnownTrajectory::Response>
+      (EXECUTE_SERVICE_NAME, boost::bind(&MoveGroupExecuteService::executeTrajectoryService, this, _1, _2));
+  ops.callback_queue = &callback_queue_;
+  execute_service_ = root_node_handle_.advertiseService(ops);
+  spinner_.start();
 }
 
 bool move_group::MoveGroupExecuteService::executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request &req, moveit_msgs::ExecuteKnownTrajectory::Response &res)
@@ -70,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/execute_service_capability.h

@@ -48,6 +48,7 @@ class MoveGroupExecuteService : public MoveGroupCapability
 public:
 
   MoveGroupExecuteService();
+  ~MoveGroupExecuteService();
 
   virtual void initialize();
 
@@ -56,6 +57,8 @@ class MoveGroupExecuteService : public MoveGroupCapability
   bool executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request &req, moveit_msgs::ExecuteKnownTrajectory::Response &res);
 
   ros::ServiceServer execute_service_;
+  ros::CallbackQueue callback_queue_;
+  ros::AsyncSpinner spinner_;
 };
 
 }

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

@@ -7,7 +7,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

@@ -341,6 +341,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();
 
@@ -510,7 +513,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_;
@@ -519,6 +522,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,39 @@ 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(100));
+    last_robot_update = current_state_monitor_->getCurrentStateTime();
+  }
+  // If there was a state monitor connected (and robot moved), the robot state should be up-to-date now
+  // and last_update_time_ = last_robot_motion_time_
+
+  // If last scene update is recent and there are no pending updates, we are done.
+  if (last_update_time_ >= t && callback_queue_.empty())
+    return;
+
+  // Processing pending updates and wait for new incoming updates up to 1s.
+  // This is necessary as re-publishing is throttled.
+  while (!callback_queue_.empty() || (ros::Time::now()-t).toSec() < 1.)
+  {
+    new_scene_update_condition_.wait_for(lock, boost::chrono::seconds(1));
+  }
+}
+
 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
 {
   scene_update_mutex_.lock_shared();
@@ -817,9 +861,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 +875,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 +1023,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 +1062,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 +1091,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 +1172,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

@@ -629,6 +629,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