mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Reset preempt_request after canceling execution
This commit is contained in:
parent
584197ab76
commit
3fac21cbc6
@ -60,6 +60,12 @@ TEST_F(PandaMoveTo, preemptExecution) {
|
||||
}
|
||||
|
||||
EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::PREEMPTED);
|
||||
|
||||
// After preempting motion reset the task and make sure we can successfully plan and execute motion again
|
||||
t.reset();
|
||||
ASSERT_TRUE(t.plan());
|
||||
result = t.execute(*t.solutions().front());
|
||||
EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
@ -316,6 +316,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
||||
while (result_future.wait_for(std::chrono::milliseconds(10)) != std::future_status::ready) {
|
||||
if (pimpl()->preempt_requested_) {
|
||||
auto cancel_future = execute_ac_->async_cancel_goal(goal_handle);
|
||||
this->resetPreemptRequest();
|
||||
if (rclcpp::spin_until_future_complete(execute_solution_node_, cancel_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS) {
|
||||
RCLCPP_ERROR(execute_solution_node_->get_logger(), "Could not preempt execution");
|
||||
|
Loading…
Reference in New Issue
Block a user