diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index adc38c7b96..f9488fc1d5 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -369,7 +369,14 @@ class Server : public ServerBase, public std::enable_shared_from_this_cancel_goal(); + try { + goal_handle->_cancel_goal(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); + return CancelResponse::REJECT; + } } } return resp; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index af94d67cbf..1a0b074ba3 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1234,14 +1234,10 @@ class TestDeadlockServer : public TestServer this->TryLockFor(lock, std::chrono::milliseconds(1000)); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [this](std::shared_ptr handle) { + [this](std::shared_ptr) { // instead of making a deadlock, check if it can acquire the lock in a second std::unique_lock lock(server_mutex_, std::defer_lock); this->TryLockFor(lock, std::chrono::milliseconds(1000)); - // TODO(KavenYau): this check may become obsolete with https://github.com/ros2/rclcpp/issues/1599 - if (!handle->is_active()) { - return rclcpp_action::CancelResponse::REJECT; - } return rclcpp_action::CancelResponse::ACCEPT; }, [this](std::shared_ptr handle) { @@ -1316,6 +1312,9 @@ TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled) send_goal_request(node_, uuid1_); std::thread t(&TestDeadlockServer::GoalSucceeded, this); rclcpp::sleep_for(std::chrono::milliseconds(50)); - send_cancel_request(node_, uuid1_); + auto response_ptr = send_cancel_request(node_, uuid1_); + + // current goal handle is not cancelable, so it returns ERROR_REJECTED + EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code); t.join(); }