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
|
// configure the action server
|
||||||
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
||||||
context_->moveit_cpp_->getNode(), "execute_task_solution",
|
context_->moveit_cpp_->getNode(), "execute_task_solution",
|
||||||
ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
|
[this](const rclcpp_action::GoalUUID& /*uuid*/,
|
||||||
std::placeholders::_1, std::placeholders::_2)),
|
const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) {
|
||||||
ActionServerType::CancelCallback(
|
// Reject new goal if another goal is currently processed
|
||||||
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
|
if (last_goal_future_.valid() &&
|
||||||
ActionServerType::AcceptedCallback(
|
last_goal_future_.wait_for(std::chrono::seconds::zero()) != std::future_status::ready) {
|
||||||
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
|
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(
|
void ExecuteTaskSolutionCapability::goalCallback(
|
||||||
|
|||||||
@ -68,13 +68,8 @@ private:
|
|||||||
rclcpp_action::CancelResponse
|
rclcpp_action::CancelResponse
|
||||||
preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
|
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_;
|
ActionServerType::SharedPtr as_;
|
||||||
|
std::future<void> last_goal_future_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace move_group
|
} // namespace move_group
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user