Reset preempt_request after canceling execution

This commit is contained in:
marqrazz 2025-05-30 15:19:53 -06:00
parent 584197ab76
commit 3fac21cbc6
2 changed files with 7 additions and 0 deletions

View File

@ -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) {

View File

@ -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");