From 1d0bc76f1c39f2ffda73c40cca80c21a6c3fea35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Leroy=20R=C3=BCgemer?= Date: Tue, 20 Aug 2024 15:35:53 +0200 Subject: [PATCH] wip debugging --- .../action_based_controller_handle.h | 13 ++++++++++++- .../gripper_controller_handle.h | 13 ++++++++----- ...llow_joint_trajectory_controller_handle.cpp | 3 +++ .../src/trajectory_execution_manager.cpp | 18 +++++++++++++++++- 4 files changed, 40 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..23b069d45b9 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 @@ -41,6 +41,7 @@ #include #include #include +#include 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 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/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..505498cf7d7 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,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle 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 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 index 5db9287d05e..79761462cb9 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 @@ -1073,6 +1073,7 @@ 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); @@ -1080,10 +1081,15 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& 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 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; } }