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

…3198) Co-authored-by: Sebastian Castro <[email protected]> (cherry picked from commit 6595dee) # Conflicts: # moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
moveit · Jan 12, 2025 · 3ee41cd · 3ee41cd
1 parent 7a425d9
commit 3ee41cd
Showing 1 changed file with 12 additions and 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
@@ -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());