[moveit_ros] fix race condition when stopping trajectory execution (b…

…ackport #3198) (#3239)

(cherry picked from commit 6595dee)

Co-authored-by: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com>

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,7 @@ void TrajectoryExecutionManager::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,
@@ -1365,7 +1366,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());