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