Add GoalStatusCode enum and get_status_code() to client goal handles

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -94,6 +94,14 @@ class ClientGoalHandle
   int8_t
   get_status();
 
+  /// Get the goal status code as a GoalStatusCode.
+  /**
+   * \throws exceptions::InvalidGoalStatusError If the server sent a status that is not one of
+   *   the action_msgs::msg::GoalStatus values. get_status() still returns the raw value.
+   */
+  GoalStatusCode
+  get_status_code();
+
   /// Check if an action client has subscribed to feedback for the goal.
   bool
   is_feedback_aware();

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -103,6 +103,17 @@ ClientGoalHandle<ActionT>::get_status()
   return status_;
 }
 
+template<typename ActionT>
+GoalStatusCode
+ClientGoalHandle<ActionT>::get_status_code()
+{
+  const int8_t status = get_status();
+  if (status < GoalStatus::STATUS_UNKNOWN || status > GoalStatus::STATUS_ABORTED) {
+    throw exceptions::InvalidGoalStatusError(status);
+  }
+  return static_cast<GoalStatusCode>(status);
+}
+
 template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::set_status(int8_t status)

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_ACTION__EXCEPTIONS_HPP_
 #define RCLCPP_ACTION__EXCEPTIONS_HPP_
 
+#include <cstdint>
 #include <stdexcept>
 #include <string>
 
@@ -41,6 +42,16 @@ class UnawareGoalHandleError : public std::runtime_error
   }
 };
 
+class InvalidGoalStatusError : public std::runtime_error
+{
+public:
+  explicit InvalidGoalStatusError(int8_t status)
+  : std::runtime_error(
+      "Goal status " + std::to_string(status) + " is not an action_msgs/GoalStatus value.")
+  {
+  }
+};
+
 }  // namespace exceptions
 
 }  // namespace rclcpp_action

rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp

@@ -86,6 +86,15 @@ class GenericClientGoalHandle
   int8_t
   get_status();
 
+  /// Get the goal status code as a GoalStatusCode.
+  /**
+   * \throws exceptions::InvalidGoalStatusError If the server sent a status that is not one of
+   *   the action_msgs::msg::GoalStatus values. get_status() still returns the raw value.
+   */
+  RCLCPP_ACTION_PUBLIC
+  GoalStatusCode
+  get_status_code();
+
   /// Check if an action client has subscribed to feedback for the goal.
   RCLCPP_ACTION_PUBLIC
   bool

rclcpp_action/include/rclcpp_action/types.hpp

@@ -34,6 +34,18 @@ using GoalUUID = std::array<uint8_t, UUID_SIZE>;
 using GoalStatus = action_msgs::msg::GoalStatus;
 using GoalInfo = action_msgs::msg::GoalInfo;
 
+/// Every status a goal can have, as defined in action_msgs::msg::GoalStatus.
+enum class GoalStatusCode : int8_t
+{
+  UNKNOWN = GoalStatus::STATUS_UNKNOWN,
+  ACCEPTED = GoalStatus::STATUS_ACCEPTED,
+  EXECUTING = GoalStatus::STATUS_EXECUTING,
+  CANCELING = GoalStatus::STATUS_CANCELING,
+  SUCCEEDED = GoalStatus::STATUS_SUCCEEDED,
+  CANCELED = GoalStatus::STATUS_CANCELED,
+  ABORTED = GoalStatus::STATUS_ABORTED
+};
+
 /// Convert a goal id to a human readable RFC-4122 compliant string.
 RCLCPP_ACTION_PUBLIC
 std::string

rclcpp_action/src/generic_client_goal_handle.cpp

@@ -89,6 +89,16 @@ GenericClientGoalHandle::get_status()
   return status_;
 }
 
+GoalStatusCode
+GenericClientGoalHandle::get_status_code()
+{
+  const int8_t status = get_status();
+  if (status < GoalStatus::STATUS_UNKNOWN || status > GoalStatus::STATUS_ABORTED) {
+    throw exceptions::InvalidGoalStatusError(status);
+  }
+  return static_cast<GoalStatusCode>(status);
+}
+
 void
 GenericClientGoalHandle::set_status(int8_t status)
 {

rclcpp_action/test/test_client.cpp

@@ -629,12 +629,14 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result)
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::ACCEPTED, goal_handle->get_status_code());
   EXPECT_FALSE(goal_handle->is_feedback_aware());
   EXPECT_FALSE(goal_handle->is_result_aware());
   auto future_result = action_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
   dual_spin_until_future_complete(future_result);
   auto wrapped_result = future_result.get();
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::SUCCEEDED, goal_handle->get_status_code());
   ASSERT_EQ(6ul, wrapped_result.result->sequence.size());
   EXPECT_EQ(0, wrapped_result.result->sequence[0]);
   EXPECT_EQ(1, wrapped_result.result->sequence[1]);
@@ -653,16 +655,50 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate)
   auto goal_handle = future_goal_handle.get();
   ASSERT_NE(nullptr, goal_handle);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::ACCEPTED, goal_handle->get_status_code());
   auto future_result = action_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
 
   action_client.reset();  // Ensure goal handle is invalidated once client goes out of scope
 
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code());
   using rclcpp_action::exceptions::UnawareGoalHandleError;
   EXPECT_THROW(future_result.get(), UnawareGoalHandleError);
 }
 
+TEST_F(TestClientAgainstServer, get_status_code_out_of_range)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  ASSERT_NE(nullptr, goal_handle);
+
+  // A server that does not follow the action protocol can send any int8_t as a status.
+  const int8_t out_of_range_status = 42;
+  ActionStatusMessage status_message;
+  rclcpp_action::GoalStatus goal_status;
+  goal_status.goal_info.goal_id.uuid = goal_handle->get_goal_id();
+  goal_status.status = out_of_range_status;
+  status_message.status_list.push_back(goal_status);
+  status_publisher->publish(status_message);
+
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+  while (goal_handle->get_status() != out_of_range_status &&
+    std::chrono::steady_clock::now() < deadline)
+  {
+    client_executor.spin_some();
+  }
+  ASSERT_EQ(out_of_range_status, goal_handle->get_status());
+  EXPECT_THROW(
+    goal_handle->get_status_code(), rclcpp_action::exceptions::InvalidGoalStatusError);
+}
+
 TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
@@ -894,6 +930,8 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals)
   EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::CANCELED, goal_handle0->get_status_code());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::CANCELED, goal_handle1->get_status_code());
 }
 
 TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback)

rclcpp_action/test/test_generic_client.cpp

@@ -559,12 +559,14 @@ TEST_F(TestGenericClientAgainstServer, async_send_goal_no_callbacks_wait_for_res
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::ACCEPTED, goal_handle->get_status_code());
   EXPECT_FALSE(goal_handle->is_feedback_aware());
   EXPECT_FALSE(goal_handle->is_result_aware());
   auto future_result = action_generic_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
   dual_spin_until_future_complete(future_result);
   auto wrapped_result = future_result.get();
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::SUCCEEDED, goal_handle->get_status_code());
   const ActionResult * result = static_cast<const ActionResult *>(wrapped_result.result);
   EXPECT_EQ(wrapped_result.code, rclcpp_action::GenericClientGoalHandle::ResultCode::SUCCEEDED);
   EXPECT_EQ(6ul, result->sequence.size());
@@ -588,16 +590,53 @@ TEST_F(TestGenericClientAgainstServer, async_send_goal_no_callbacks_then_invalid
   auto goal_handle = future_goal_handle.get();
   ASSERT_NE(nullptr, goal_handle);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::ACCEPTED, goal_handle->get_status_code());
   auto future_result = action_generic_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
 
   action_generic_client.reset();  // Ensure goal handle is invalidated once client goes out of scope
 
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code());
   using rclcpp_action::exceptions::UnawareGoalHandleError;
   EXPECT_THROW(future_result.get(), UnawareGoalHandleError);
 }
 
+TEST_F(TestGenericClientAgainstServer, get_status_code_out_of_range)
+{
+  auto action_generic_client = rclcpp_action::create_generic_client(
+    client_node,
+    action_name,
+    "test_msgs/action/Fibonacci");
+  ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(goal));
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  ASSERT_NE(nullptr, goal_handle);
+
+  // A server that does not follow the action protocol can send any int8_t as a status.
+  const int8_t out_of_range_status = 42;
+  ActionStatusMessage status_message;
+  rclcpp_action::GoalStatus goal_status;
+  goal_status.goal_info.goal_id.uuid = goal_handle->get_goal_id();
+  goal_status.status = out_of_range_status;
+  status_message.status_list.push_back(goal_status);
+  status_publisher->publish(status_message);
+
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+  while (goal_handle->get_status() != out_of_range_status &&
+    std::chrono::steady_clock::now() < deadline)
+  {
+    client_executor.spin_some();
+  }
+  ASSERT_EQ(out_of_range_status, goal_handle->get_status());
+  EXPECT_THROW(
+    goal_handle->get_status_code(), rclcpp_action::exceptions::InvalidGoalStatusError);
+}
+
 TEST_F(TestGenericClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result)
 {
   auto action_generic_client = rclcpp_action::create_generic_client(
@@ -858,6 +897,8 @@ TEST_F(TestGenericClientAgainstServer, async_cancel_all_goals)
   EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::CANCELED, goal_handle0->get_status_code());
+  EXPECT_EQ(rclcpp_action::GoalStatusCode::CANCELED, goal_handle1->get_status_code());
 }
 
 TEST_F(TestGenericClientAgainstServer, async_cancel_all_goals_with_callback)