Add preempted ResultCode for WrappedResult and preempt API for Server…

…GoalHandle

Signed-off-by: Sarthak Mittal <[email protected]>

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);
       }

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;

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<typename ActionT::Impl::GetResultService::Response>();
+    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.

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<rclcpp::Node>("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<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(
+    node, "fibonacci",
+    handle_goal,
+    handle_cancel,
+    handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "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::Impl::GetResultService>(
+    "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<Fibonacci::Impl::GetResultService::Request>();
+  request->goal_id.uuid = uuid;
+  auto future = result_client->async_send_request(request);
+
+  // Send a result
+  auto result = std::make_shared<Fibonacci::Result>();
+  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<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback");