From 1697a60312a748549f753b61c8021db6390de830 Mon Sep 17 00:00:00 2001
From: lruegeme <lruegeme@techfak.uni-bielefeld.de>
Date: Tue, 27 Aug 2024 14:09:51 +0200
Subject: [PATCH] fix actionlib race condition

---
 .../action_based_controller_handle.h                 | 10 +++++++++-
 .../gripper_controller_handle.h                      | 12 +++++++-----
 .../src/trajectory_execution_manager.cpp             |  9 ++++++++-
 3 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h
index 798978a9f38..3cc8a33b670 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h
@@ -116,7 +116,15 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
   bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override
   {
     if (controller_action_client_ && !done_)
-      return controller_action_client_->waitForResult(timeout);
+    {
+      auto ret = controller_action_client_->waitForResult(timeout);
+      while (ret && !done_) {
+        // Workatound for actionlib without https://github.com/ros/actionlib/commit/9beedb8307a181d63cdeedab389712b229de0b4f
+        ROS_WARN_STREAM_NAMED("ActionBasedController", "action finished but DoneCallback not executed, sleeping");
+        ros::Duration(0.005).sleep();
+      }
+      return ret;
+    }
     return true;
   }
 
diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
index 6b74476bb4e..04ed7b39730 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -139,12 +139,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       }
     }
 
+    done_ = false;
+    last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
     controller_action_client_->sendGoal(
         goal, [this](const auto& state, const auto& result) { controllerDoneCallback(state, result); },
         [this] { controllerActiveCallback(); }, [this](const auto& feedback) { controllerFeedbackCallback(feedback); });
-
-    done_ = false;
-    last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
     return true;
   }
 
@@ -175,10 +174,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
   void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
                               const control_msgs::GripperCommandResultConstPtr& /* result */)
   {
-    if (state == actionlib::SimpleClientGoalState::ABORTED && allow_failure_)
+    if (state == actionlib::SimpleClientGoalState::ABORTED && allow_failure_) 
+    {
+      ROS_WARN_STREAM_NAMED("GripperController", name_ << " ABORTED but allowing failure");
       finishControllerExecution(actionlib::SimpleClientGoalState::SUCCEEDED);
-    else
+    } else {
       finishControllerExecution(state);
+    }
   }
 
   void controllerActiveCallback()
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 5db9287d05e..327f540eb6e 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
@@ -1115,8 +1115,10 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
   if (ensureActiveControllers(context.controllers_))
   {
     // stop if we are already asked to do so
-    if (execution_complete_)
+    if (execution_complete_) {
+      ROS_WARN_NAMED(LOGNAME, "execution_complete_ during executePart startup");
       return false;
+    }
 
     std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
     {
@@ -1142,7 +1144,12 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
           if (!h)
           {
             active_handles_.clear();
+
+            // TODO create sensible structure instead of juggling mutexes
+            time_index_mutex_.lock();
             current_context_ = -1;
+            time_index_mutex_.unlock();
+            
             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
             ROS_ERROR_NAMED(LOGNAME, "No controller handle for controller '%s'. Aborting.",
                             context.controllers_[i].c_str());