From 3ee41cda156b3da2f5b5202c0abe57433a782e0b 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)

# Conflicts:
#	moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
---
 .../src/trajectory_execution_manager.cpp            | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

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 32e4e27b4d..ecf2907c1f 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
@@ -1316,6 +1316,7 @@ void TrajectoryExecutionManager::clear()
 {
   if (execution_complete_)
   {
+    std::scoped_lock slock(execution_state_mutex_);
     for (TrajectoryExecutionContext* trajectory : trajectories_)
       delete trajectory;
     trajectories_.clear();
@@ -1329,7 +1330,11 @@ void TrajectoryExecutionManager::clear()
     }
   }
   else
+<<<<<<< HEAD
     RCLCPP_ERROR(LOGGER, "Cannot push a new trajectory while another is being executed");
+=======
+    RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!");
+>>>>>>> 6595deea4 ([moveit_ros] fix race condition when stopping trajectory execution (#3198))
 }
 
 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
@@ -1365,7 +1370,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());