ExecuteTaskSolutionCapability: Reject new goals when busy (#496)

- Rename goalCallback() -> execCallback()
- Run execCallback asynchronously and use future to know status of execution
This commit is contained in:
JafarAbdi 2022-06-20 15:08:22 +00:00 committed by Robert Haschke
parent 1e058598c7
commit c11a34a935
2 changed files with 11 additions and 4 deletions

View File

@ -95,10 +95,13 @@ void ExecuteTaskSolutionCapability::initialize() {
ActionServerType::CancelCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
ActionServerType::AcceptedCallback(
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
[this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
last_goal_future_ =
std::async(std::launch::async, &ExecuteTaskSolutionCapability::execCallback, this, goal_handle);
}));
}
void ExecuteTaskSolutionCapability::goalCallback(
void ExecuteTaskSolutionCapability::execCallback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();

View File

@ -63,18 +63,22 @@ private:
bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
void execCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
rclcpp_action::CancelResponse
preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
/** Always accept the goal */
/** Only accept the goal if we aren't executing one */
rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/,
const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const {
if (last_goal_future_.valid() &&
last_goal_future_.wait_for(std::chrono::seconds::zero()) != std::future_status::ready)
return rclcpp_action::GoalResponse::REJECT;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
ActionServerType::SharedPtr as_;
std::future<void> last_goal_future_;
};
} // namespace move_group