[moveit_ros] fix race condition when stopping trajectory execution

moveit · Jan 6, 2025 · b182a37 · b182a37
1 parent a61d973
commit b182a37
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 5 deletions.
diff --git a/...tion_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/...tion_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp
@@ -284,7 +284,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
                      bool auto_clear);
   bool executePart(std::size_t part_index);
-  bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0);
+  bool waitForRobotToStop(const std::vector<std::vector<std::string>>& joint_names_per_traj, double wait_time = 1.0);
 
   void stopExecutionInternal();
 
@@ -311,6 +311,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
 
   std::mutex execution_state_mutex_;
   std::mutex execution_thread_mutex_;
+  std::mutex execution_trajectories_mutex_;
 
   // this condition is used to notify the completion of execution for given trajectories
   std::condition_variable execution_complete_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
@@ -1271,9 +1271,11 @@ void TrajectoryExecutionManager::clear()
 {
   if (execution_complete_)
   {
+    execution_trajectories_mutex_.lock();
     for (TrajectoryExecutionContext* trajectory : trajectories_)
       delete trajectory;
     trajectories_.clear();
+    execution_trajectories_mutex_.unlock();
   }
   else
     RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");
@@ -1312,7 +1314,23 @@ 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]);
+  {
+    execution_trajectories_mutex_.lock();
+    if (!execution_complete_ && trajectories_.size() > 0)
+    {
+      const auto& context = *trajectories_[i - 1];
+      std::vector<std::vector<std::string>> joint_names_per_traj;
+      for (const auto& trajectory : context.trajectory_parts_)
+      {
+        joint_names_per_traj.push_back(trajectory.joint_trajectory.joint_names);
+      }
+      execution_trajectories_mutex_.unlock();
+      waitForRobotToStop(joint_names_per_traj);
+    }
+    else
+      execution_trajectories_mutex_.unlock();
+  }
+
 
   RCLCPP_INFO(logger_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
 
@@ -1558,7 +1576,8 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
   }
 }
 
-bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time)
+bool TrajectoryExecutionManager::waitForRobotToStop(const std::vector<std::vector<std::string>>& joint_names_per_traj,
+                                                    double wait_time)
 {
   // skip waiting for convergence?
   if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_)
@@ -1589,9 +1608,8 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon
 
     // check for motion in effected joints of execution context
     bool moved = false;
-    for (const auto& trajectory : context.trajectory_parts_)
+    for (const auto& joint_names : joint_names_per_traj)
     {
-      const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
       const std::size_t n = joint_names.size();
 
       for (std::size_t i = 0; i < n && !moved; ++i)