Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ros2]: Fixed preempting of execution #363

Open
wants to merge 3 commits into
base: ros2
Choose a base branch
from
Open

[ros2]: Fixed preempting of execution #363

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9cf3973
Fixed preempting of execution: Action server starts thread when accep…
wyattrees Jun 23, 2021
64690d0
Use callback groups to be able to call cancellation while executing
JafarAbdi Aug 6, 2021
fcdce1e
Revert impl
JafarAbdi Aug 6, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion capabilities/src/execute_task_solution_capability.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,8 @@ namespace move_group {
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}

void ExecuteTaskSolutionCapability::initialize() {
action_callback_group_ =
context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
// configure the action server
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
context_->moveit_cpp_->getNode(), "execute_task_solution",
Expand All @@ -95,7 +97,8 @@ void ExecuteTaskSolutionCapability::initialize() {
ActionServerType::CancelCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
ActionServerType::AcceptedCallback(
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe the std::bind isn't needed here since 1 argument is expected

rcl_action_server_get_default_options(), action_callback_group_);
}

void ExecuteTaskSolutionCapability::goalCallback(
Expand Down
1 change: 1 addition & 0 deletions capabilities/src/execute_task_solution_capability.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
}

ActionServerType::SharedPtr as_;
rclcpp::CallbackGroup::SharedPtr action_callback_group_;
};

} // namespace move_group
14 changes: 11 additions & 3 deletions core/src/task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -301,9 +301,17 @@ moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) {
}

auto result_future = ac->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "Get result call failed");
return error_code;
while (result_future.wait_for(std::chrono::milliseconds(10)) != std::future_status::ready) {
if (pimpl()->preempt_requested_) {
auto cancel_future = ac->async_cancel_goal(goal_handle);
if (rclcpp::spin_until_future_complete(node, cancel_future) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "Could not preempt execution");
return error_code;
} else {
error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED;
return error_code;
}
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is that really the best we can do for this kind of feature? 10ms waits are quite bad style :(

}

auto result = result_future.get();
Expand Down