Run goalcallback asynchronously (#496)

* Run goalCallback in a seperate asynchronously

* Clang-tidy fixes

* Replace std::bind with lambdas

* Add comment

* Format

---------

Co-authored-by: JafarAbdi <cafer.abdi@gmail.com>
This commit is contained in:
Sebastian Jahr 2023-10-27 10:31:44 +02:00 committed by GitHub
parent 8375e6ef3e
commit e121b498f6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 17 additions and 12 deletions

View File

@ -90,12 +90,22 @@ void ExecuteTaskSolutionCapability::initialize() {
// configure the action server
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
context_->moveit_cpp_->getNode(), "execute_task_solution",
ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
std::placeholders::_1, std::placeholders::_2)),
ActionServerType::CancelCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
ActionServerType::AcceptedCallback(
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
[this](const rclcpp_action::GoalUUID& /*uuid*/,
const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) {
// Reject new goal if another goal is currently processed
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;
},
[this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
return preemptCallback(goal_handle);
},
[this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
last_goal_future_ =
std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle);
});
}
void ExecuteTaskSolutionCapability::goalCallback(

View File

@ -68,13 +68,8 @@ private:
rclcpp_action::CancelResponse
preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
/** Always accept the goal */
rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/,
const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
ActionServerType::SharedPtr as_;
std::future<void> last_goal_future_;
};
} // namespace move_group