wip debugging

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

Hide file tree

Showing 4 changed files with 40 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
@@ -41,6 +41,7 @@
 #include <moveit/controller_manager/controller_manager.h>
 #include <moveit/macros/class_forward.h>
 #include <memory>
+#include <mutex>
 
 namespace moveit_simple_controller_manager
 {
@@ -116,7 +117,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_) {
+        ROS_WARN_STREAM_NAMED("ActionBasedController", "action finished but DoneCallback not executed, sleeping");
+        //RACE CONDITION done callback is not executed
+        ros::Duration(0.01).sleep();
+      }
+      return ret;
+    }
     return true;
   }
 
@@ -137,6 +146,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
 
 protected:
   ros::NodeHandle nh_;
+  std::mutex state_mutex_;
   std::string getActionName() const
   {
     if (namespace_.empty())
@@ -147,6 +157,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
 
   void finishControllerExecution(const actionlib::SimpleClientGoalState& state)
   {
+    const std::lock_guard<std::mutex> lock(state_mutex_);
     ROS_DEBUG_STREAM_NAMED("ActionBasedController", "Controller " << name_ << " is done with state " << state.toString()
                                                                   << ": " << state.getText());
     if (state == actionlib::SimpleClientGoalState::SUCCEEDED)

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,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       }
     }
 
+    const std::lock_guard<std::mutex> lock(state_mutex_);
+    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 +175,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/...lugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/...lugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp
@@ -62,11 +62,14 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro
 
   control_msgs::FollowJointTrajectoryGoal goal = goal_template_;
   goal.trajectory = trajectory.joint_trajectory;
+
+  const std::lock_guard<std::mutex> lock(state_mutex_);
   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); });
+
   return true;
 }
 

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
@@ -1073,17 +1073,23 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback&
   // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
   // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
   std::size_t i = 0;
+  bool did_break = false;
   for (; i < trajectories_.size(); ++i)
   {
     bool epart = executePart(i);
     if (epart && part_callback)
       part_callback(i);
     if (!epart || execution_complete_)
     {
+      if(!epart)  ROS_ERROR_STREAM_NAMED(LOGNAME, "epart false");
+      if(execution_complete_) ROS_ERROR_STREAM_NAMED(LOGNAME, "execution_complete_ true");
       ++i;
+      did_break = true;
       break;
     }
   }
+
+  ROS_ERROR_STREAM_NAMED(LOGNAME, "Completed trajectory execution with part " << i << " of " << trajectories_.size() << " did break: " << did_break);
 
   // only report that execution finished successfully when the robot actually stopped moving
   if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
@@ -1115,8 +1121,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 +1150,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());
@@ -1289,6 +1302,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
       // if something made the trajectory stop, we stop this thread too
       if (execution_complete_)
       {
+        ROS_WARN_NAMED(LOGNAME, "execution_complete_ during executePart 1305");
         result = false;
         break;
       }
@@ -1297,6 +1311,8 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
         ROS_WARN_STREAM_NAMED(LOGNAME, "Controller handle " << handle->getName() << " reports status "
                                                             << handle->getLastExecutionStatus().asString());
         last_execution_status_ = handle->getLastExecutionStatus();
+        ROS_WARN_STREAM_NAMED(LOGNAME, "Controller handle " << handle->getName() << " reports status2 "
+                                                            << handle->getLastExecutionStatus().asString());
         result = false;
       }
     }