From 76c70412503ce3bad4598ec792c6601787e3fa84 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 15 Jul 2016 08:19:56 +0200 Subject: [PATCH] cherry-pick #716 from indigo-devel fixes raise conditions in updating of PlanningScene state - unlock mutexes in reverse order of locking - PlanningSceneMonitor::syncSceneUpdates(ros::Time): sync all scene updates upto given time - syncSceneUpdates() after execution of trajectories & before fetching current robot state - sync scene update before starting planning - validate trajectory before execution - final fixup #724 --- .../execute_service_capability.cpp | 14 ++-- .../move_action_capability.cpp | 4 +- .../plan_service_capability.cpp | 2 +- planning/CMakeLists.txt | 2 +- .../plan_execution/src/plan_execution.cpp | 2 + .../planning_scene_monitor.h | 10 ++- .../src/planning_scene_monitor.cpp | 70 +++++++++++++++---- .../moveit/move_group_interface/move_group.h | 3 + .../move_group_interface/src/move_group.cpp | 37 ++++++++++ .../src/motion_planning_frame_planning.cpp | 7 +- .../planning_scene_display.h | 5 ++ .../src/planning_scene_display.cpp | 6 ++ 12 files changed, 136 insertions(+), 26 deletions(-) diff --git a/move_group/src/default_capabilities/execute_service_capability.cpp b/move_group/src/default_capabilities/execute_service_capability.cpp index 9fcd76fa15..3b02399f80 100644 --- a/move_group/src/default_capabilities/execute_service_capability.cpp +++ b/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 diff --git a/move_group/src/default_capabilities/move_action_capability.cpp b/move_group/src/default_capabilities/move_action_capability.cpp index f4fdf03245..c47ee5f377 100644 --- a/move_group/src/default_capabilities/move_action_capability.cpp +++ b/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 ¤t_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(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 diff --git a/move_group/src/default_capabilities/plan_service_capability.cpp b/move_group/src/default_capabilities/plan_service_capability.cpp index c29193e009..5d425ab938 100644 --- a/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/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_); diff --git a/planning/CMakeLists.txt b/planning/CMakeLists.txt index a330911cca..f5fbe8d05b 100644 --- a/planning/CMakeLists.txt +++ b/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 diff --git a/planning/plan_execution/src/plan_execution.cpp b/planning/plan_execution/src/plan_execution.cpp index 5f3d251076..80eb0a648b 100644 --- a/planning/plan_execution/src/plan_execution.cpp +++ b/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; } diff --git a/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index b138e75035..ca36e87ce3 100644 --- a/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/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 spinner_; + ros::Time last_robot_motion_time_; /// Last time the robot has moved + bool enforce_next_state_update_; }; typedef boost::shared_ptr PlanningSceneMonitorPtr; diff --git a/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index aea119c033..7fc173b8f2 100644 --- a/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/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 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,7 +1091,7 @@ 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_ @@ -1056,7 +1099,7 @@ void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback(cons 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 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); diff --git a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h index 498b3557b0..c80790d1f5 100644 --- a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h +++ b/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 diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp index bb0200f701..a6c0e0a58d 100644 --- a/planning_interface/move_group_interface/src/move_group.cpp +++ b/planning_interface/move_group_interface/src/move_group.cpp @@ -720,6 +720,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 &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::epsilon()) + { + ROS_ERROR("Trajectory start deviates from current robot state"); + return false; + } + } + return true; + } + double computeCartesianPath(const std::vector &waypoints, double step, double jump_threshold, moveit_msgs::RobotTrajectory &msg, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code) { @@ -1230,6 +1262,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); diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 5d6fb4ea1d..12f90c22a0 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/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 == "") { + 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) diff --git a/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 2b459087e8..c0ecc36cb4 100644 --- a/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -106,7 +106,12 @@ class PlanningSceneDisplay : public rviz::Display const std::string getMoveGroupNS() const; const robot_model::RobotModelConstPtr& getRobotModel() const; + + /// sync all planning scene updates up to time t + void syncSceneUpdates(const ros::Time &t = ros::Time::now()); + /// get read-only access to planning scene planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const; + /// get write access to planning scene planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW(); const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor(); diff --git a/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 5cf86b962e..796ab329d3 100644 --- a/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -297,6 +297,12 @@ const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() con } } +void PlanningSceneDisplay::syncSceneUpdates(const ros::Time &t) +{ + if (planning_scene_monitor_) + planning_scene_monitor_->syncSceneUpdates(t); +} + planning_scene_monitor::LockedPlanningSceneRO PlanningSceneDisplay::getPlanningSceneRO() const { return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);