From bbd2718249f5a77b10f6a45087140ca2f7ab4268 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Fri, 8 Nov 2019 05:28:25 +0100 Subject: [PATCH] TrajectoryExecutionManager: fix race condition (#1709) Fix race condition accessing execution_thread_ by adding a new mutex. --- .../trajectory_execution_manager.h | 1 + .../src/trajectory_execution_manager.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 4 deletions(-) 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 ea2f1a0d6d..b07132eef1 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 @@ -321,6 +321,7 @@ class TrajectoryExecutionManager boost::mutex execution_state_mutex_; boost::mutex continuous_execution_mutex_; + boost::mutex execution_thread_mutex_; boost::condition_variable continuous_execution_condition_; 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 1f4a48b46e..602d0da44f 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 @@ -1172,8 +1172,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) ROS_INFO_NAMED(name_, "Stopped trajectory execution."); // wait for the execution thread to finish - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } if (auto_clear) clear(); @@ -1184,8 +1188,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we // join it now { - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } } }