From 6671baa22338dec04ca405b60176b0159ed7999c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 5 Jan 2017 08:28:52 +0100 Subject: [PATCH] fix race conditions when updating PlanningScene (#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: https://github.com/ros/ros_comm/pull/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 (#291)" This reverts commit e3ef9a64fe003a8a607f16fd9df112bc7a7d98bf. * 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 --- .../src/pick_place_action_capability.cpp | 4 + .../move_action_capability.cpp | 2 + .../plan_service_capability.cpp | 2 + moveit_ros/planning/CMakeLists.txt | 2 +- .../current_state_monitor.h | 11 ++- .../planning_scene_monitor.h | 13 ++- .../src/current_state_monitor.cpp | 20 +++++ .../src/planning_scene_monitor.cpp | 85 ++++++++++++++++--- .../trajectory_execution_manager.h | 1 + .../src/trajectory_execution_manager.cpp | 66 +++++++++++++- .../move_group_interface/src/move_group.cpp | 8 +- .../planning_interface/test/CMakeLists.txt | 1 + .../test/robot_state_update.py | 48 +++++++++++ .../test/robot_state_update.test | 5 ++ .../motion_planning_frame.h | 2 + .../src/motion_planning_frame_planning.cpp | 25 ++++-- .../planning_scene_display.h | 5 ++ .../src/planning_scene_display.cpp | 7 ++ 18 files changed, 279 insertions(+), 28 deletions(-) create mode 100755 moveit_ros/planning_interface/test/robot_state_update.py create mode 100644 moveit_ros/planning_interface/test/robot_state_update.test diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index a9d3bc679f..4a7b6b5485 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/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; diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index dd5531b952..21f103f551 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/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; diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index b01ace3211..746070b07d 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/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; diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 4829da0711..9b72f4ae72 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/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 diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index b4a743c773..f56d397d1e 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -45,6 +45,7 @@ #include #include #include +#include namespace planning_scene_monitor { @@ -135,8 +136,13 @@ class CurrentStateMonitor * @return Returns the map from joint names to joint state values*/ std::map 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 update_callbacks_; }; diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index dc03846a77..1d66c4f395 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/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 > 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& 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_; diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 136259267a..75cbf07a6b 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/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(); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 82589d2280..2893f5295f 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/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 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,7 +1165,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 - last_robot_state_update_wall_time_; { // lock for access to dt_state_update_ and state_update_pending_ @@ -1116,14 +1173,19 @@ 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(); + 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 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); diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index c6730f0203..8fcae3b559 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/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(); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 41139e2fb9..cb5bca6bca 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -932,7 +932,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); robot_state::RobotStatePtr current_state; - if (!csm_->waitForCompleteState(1.0) || !(current_state = csm_->getCurrentState())) + if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState())) { ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within " "1s"); @@ -1228,15 +1228,22 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& // execute each trajectory, one after the other (executePart() is blocking) or until one fails. // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success) - for (std::size_t i = 0; i < trajectories_.size(); ++i) + std::size_t i = 0; + for (; i < trajectories_.size(); ++i) { bool epart = executePart(i); if (epart && part_callback) part_callback(i); if (!epart || execution_complete_) + { + ++i; break; + } } + // only report that execution finished when the robot stopped moving + waitForRobotToStop(*trajectories_[i - 1]); + ROS_DEBUG_NAMED("traj_execution", "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str()); @@ -1458,6 +1465,61 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } } +bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time) +{ + if (allowed_start_tolerance_ == 0) // skip validation on this magic number + return true; + + ros::WallTime start = ros::WallTime::now(); + double time_remaining = wait_time; + + robot_state::RobotStatePtr prev_state, cur_state; + prev_state = csm_->getCurrentState(); + prev_state->enforceBounds(); + + // assume robot stopped when 3 consecutive checks yield the same robot state + unsigned int no_motion_count = 0; // count iterations with no motion + while (time_remaining > 0. && no_motion_count < 3) + { + if (!csm_->waitForCurrentState(ros::Time::now(), time_remaining) || !(cur_state = csm_->getCurrentState())) + { + ROS_WARN_NAMED("traj_execution", "Failed to receive current joint state"); + return false; + } + cur_state->enforceBounds(); + time_remaining = wait_time - (ros::WallTime::now() - start).toSec(); // remaining wait_time + + // check for motion in effected joints of execution context + bool moved = false; + for (const auto &trajectory : context.trajectory_parts_) + { + const std::vector &joint_names = trajectory.joint_trajectory.joint_names; + const std::size_t n = joint_names.size(); + + for (std::size_t i = 0; i < n && !moved; ++i) + { + const robot_model::JointModel *jm = cur_state->getJointModel(joint_names[i]); + if (!jm) + continue; // joint vanished from robot state (shouldn't happen), but we don't care + + if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_) + { + moved = true; + no_motion_count = 0; + break; + } + } + } + + if (!moved) + ++no_motion_count; + + std::swap(prev_state, cur_state); + } + + return time_remaining > 0; +} + std::pair TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex() const { boost::mutex::scoped_lock slock(time_index_mutex_); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group.cpp index 503fff7298..ae92d15be9 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group.cpp @@ -588,9 +588,11 @@ class MoveGroup::MoveGroupImpl if (!current_state_monitor_->isActive()) current_state_monitor_->startStateMonitor(); - if (!current_state_monitor_->waitForCompleteState(opt_.group_name_, wait_seconds)) - ROS_WARN_NAMED("move_group_interface", "Joint values for monitored state are requested but the full state is not " - "known"); + if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + return false; + } current_state = current_state_monitor_->getCurrentState(); return true; diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index dc12a412a9..523f384b2b 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -7,6 +7,7 @@ if (CATKIN_ENABLE_TESTING) #target_link_libraries(test_cleanup moveit_move_group_interface) add_rostest(python_move_group.test) + add_rostest(robot_state_update.test) endif() install(PROGRAMS python_move_group.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py new file mode 100755 index 0000000000..de98da22af --- /dev/null +++ b/moveit_ros/planning_interface/test/robot_state_update.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +import unittest +import numpy as np +import rospy +import rostest +import os + +from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface + + +class RobotStateUpdateTest(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + @classmethod + def setUpClass(self): + self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description") + + @classmethod + def tearDown(self): + pass + + def plan(self, target): + self.group.set_joint_value_target(target) + return self.group.compute_plan() + + def test(self): + current = np.asarray(self.group.get_current_joint_values()) + for i in range(30): + target = current + np.random.uniform(-0.5, 0.5, size = current.shape) + # if plan was successfully executed, current state should be reported at target + if self.group.execute(self.plan(target)): + actual = np.asarray(self.group.get_current_joint_values()) + self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) + # otherwise current state should be still the same + else: + actual = np.asarray(self.group.get_current_joint_values()) + self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0)) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_robot_state_update' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest) + + # suppress cleanup segfault in ROS < Kinetic + os._exit(0) diff --git a/moveit_ros/planning_interface/test/robot_state_update.test b/moveit_ros/planning_interface/test/robot_state_update.test new file mode 100644 index 0000000000..e282f0ab22 --- /dev/null +++ b/moveit_ros/planning_interface/test/robot_state_update.test @@ -0,0 +1,5 @@ + + + + diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index b69b0ae8d5..3bdb94ba4e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -226,6 +226,8 @@ private Q_SLOTS: void configureWorkspace(); void updateQueryStateHelper(robot_state::RobotState& state, const std::string& v); void fillStateSelectionOptions(); + void useStartStateButtonExec(); + void useGoalStateButtonExec(); // Scene objects tab void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape, diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index d4f685bfdf..15c4625e31 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -175,13 +175,17 @@ void MotionPlanningFrame::onFinishedExecution(bool success) // update query start state to current if neccessary if (ui_->start_state_selection->currentText() == "") - { - ros::Duration(1).sleep(); useStartStateButtonClicked(); - } } void MotionPlanningFrame::useStartStateButtonClicked() +{ + // use background job: fetching the current state might take up to a second + planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::useStartStateButtonExec, this), // + "update start state"); +} + +void MotionPlanningFrame::useStartStateButtonExec() { robot_state::RobotState start = *planning_display_->getQueryStartState(); updateQueryStateHelper(start, ui_->start_state_selection->currentText().toStdString()); @@ -189,6 +193,13 @@ void MotionPlanningFrame::useStartStateButtonClicked() } void MotionPlanningFrame::useGoalStateButtonClicked() +{ + // use background job: fetching the current state might take up to a second + planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::useGoalStateButtonExec, this), // + "update goal state"); +} + +void MotionPlanningFrame::useGoalStateButtonExec() { robot_state::RobotState goal = *planning_display_->getQueryGoalState(); updateQueryStateHelper(goal, ui_->goal_state_selection->currentText().toStdString()); @@ -407,11 +418,11 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo { if (move_group_ && planning_display_) { - robot_state::RobotState state = *planning_display_->getQueryStartState(); + planning_display_->waitForCurrentRobotState(); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - state = ps->getCurrentState(); + robot_state::RobotState state = ps->getCurrentState(); planning_display_->setQueryStartState(state); } } @@ -421,11 +432,11 @@ void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyCon { if (move_group_ && planning_display_) { - robot_state::RobotState state = *planning_display_->getQueryStartState(); + planning_display_->waitForCurrentRobotState(); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - state = ps->getCurrentState(); + robot_state::RobotState state = ps->getCurrentState(); planning_display_->setQueryGoalState(state); } } diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 1922c92d3e..04f79b66c5 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -104,7 +104,12 @@ class PlanningSceneDisplay : public rviz::Display const std::string getMoveGroupNS() const; const robot_model::RobotModelConstPtr& getRobotModel() const; + + /// wait for robot state more recent than t + bool waitForCurrentRobotState(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/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 41edf2032e..efddb6b7d0 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -282,6 +282,13 @@ const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() con } } +bool PlanningSceneDisplay::waitForCurrentRobotState(const ros::Time &t) +{ + if (planning_scene_monitor_) + return planning_scene_monitor_->waitForCurrentRobotState(t); + return false; +} + planning_scene_monitor::LockedPlanningSceneRO PlanningSceneDisplay::getPlanningSceneRO() const { return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);