From d4f73e7891eb288b7a9968364c8fe5282c421fc5 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Mon, 18 May 2020 01:07:59 +0530 Subject: [PATCH] Add preempted ResultCode for WrappedResult and preempt API for ServerGoalHandle Signed-off-by: Sarthak Mittal --- .../include/rclcpp_action/client.hpp | 3 +- .../rclcpp_action/client_goal_handle.hpp | 5 +- .../rclcpp_action/server_goal_handle.hpp | 20 +++++ rclcpp_action/test/test_server.cpp | 73 +++++++++++++++++++ 4 files changed, 98 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 7133e8ce14..8470b07226 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 == 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..f34a8abc3e 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 = 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..9bad95a46a 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -662,6 +662,79 @@ 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(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");