From 9b1111188194e7d0380c89fc952a5a5e2c960201 Mon Sep 17 00:00:00 2001 From: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Date: Mon, 13 Jan 2025 02:42:42 +0800 Subject: [PATCH] [moveit_ros] fix race condition when stopping trajectory execution (#3198) Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> (cherry picked from commit 6595deea41fedbad9710084d629cb4f4c999a50e) --- .../src/trajectory_execution_manager.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) 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 fd913c7960..00fa65ea17 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 @@ -1271,12 +1271,13 @@ void TrajectoryExecutionManager::clear() { if (execution_complete_) { + std::scoped_lock slock(execution_state_mutex_); for (TrajectoryExecutionContext* trajectory : trajectories_) delete trajectory; trajectories_.clear(); } else - RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed"); + RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!"); } void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback, @@ -1312,7 +1313,13 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& // only report that execution finished successfully when the robot actually stopped moving if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED) - waitForRobotToStop(*trajectories_[i - 1]); + { + std::scoped_lock slock(execution_state_mutex_); + if (!execution_complete_) + { + waitForRobotToStop(*trajectories_[i - 1]); + } + } RCLCPP_INFO(logger_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());