diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 7133e8ce14..7872bda8f2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -581,7 +581,8 @@ class Client : public ClientBase if ( goal_status == GoalStatus::STATUS_SUCCEEDED || goal_status == GoalStatus::STATUS_CANCELED || - goal_status == GoalStatus::STATUS_ABORTED) + goal_status == GoalStatus::STATUS_ABORTED || + goal_status == 7) // GoalStatus::STATUS_PREEMPTED) { goal_handles_.erase(goal_id); } diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 42e2844c51..a7b1322d49 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -37,7 +37,8 @@ enum class ResultCode : int8_t UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN, SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED, - ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED + ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED, + PREEMPTED = 7 // action_msgs::msg::GoalStatus::STATUS_PREEMPTED }; @@ -65,7 +66,7 @@ class ClientGoalHandle { /// The unique identifier of the goal GoalUUID goal_id; - /// A status to indicate if the goal was canceled, aborted, or suceeded + /// A status to indicate if the goal was canceled, aborted, preempted, or succeeded ResultCode code; /// User defined fields sent back with an action typename ActionT::Result::SharedPtr result; diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d41b32eb4f..b740fb7e90 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -174,6 +174,26 @@ class ServerGoalHandle : public ServerGoalHandleBase on_terminal_state_(uuid_, response); } + /// Indicate that a goal was preempted and has been aborted. + /** + * Only call this if the goal was executing and has to be preempted. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + preempt(typename ActionT::Result::SharedPtr result_msg) + { + _abort(); + auto response = std::make_shared(); + response->status = 7; // action_msgs::msg::GoalStatus::STATUS_PREEMPTED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + /// Indicate that a goal has succeeded. /** * Only call this if the goal is executing and has reached the desired final state. diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7c873f5ee1..5578db99bf 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -662,6 +662,80 @@ TEST_F(TestServer, publish_status_aborted) EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); } +TEST_F(TestServer, publish_status_preemped) +{ + auto node = std::make_shared("status_preempted", "/rclcpp_action/status_preempted"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->preempt(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + // EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_PREEMPTED, response->status); + EXPECT_EQ(7, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + TEST_F(TestServer, publish_feedback) { auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback");