fix actionlib race condition

CentralLabFacilities · Aug 27, 2024 · 1697a60 · 1697a60
1 parent 878d291
commit 1697a60
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 7 deletions.
diff --git a/...troller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/...troller_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/...e_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/...e_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
@@ -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());