sync planning scene updates after execution

Author: rhaschke

move_group/src/default_capabilities/execute_service_capability.cpp

@@ -78,6 +78,8 @@ bool move_group::MoveGroupExecuteService::executeTrajectoryService(moveit_msgs::
             res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
           else
             res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
+      // wait for all planning scene updates to be processed
+      context_->planning_scene_monitor_->syncUpdates(ros::Time::now());
       ROS_INFO_STREAM("Execution completed: " << es.asString());
     }
     else

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_->syncUpdates(ros::Time::now());
   execution_complete_ = true;
 }