mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
8375e6ef3e
commit
e121b498f6
@ -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(
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user