From 344ed0c2ca2069b78f79959d7289db4ba1968ab8 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 28 Sep 2026 15:07:52 -0600 Subject: [PATCH 1/2] Add GoalStatusCode enum and get_status_code() to client goal handles Co-Authored-By: Claude Opus 5.5 Signed-off-by: David Vadovszki --- .../rclcpp_action/client_goal_handle.hpp | 8 ++++ .../rclcpp_action/client_goal_handle_impl.hpp | 11 +++++ .../generic_client_goal_handle.hpp | 9 +++++ rclcpp_action/include/rclcpp_action/types.hpp | 12 ++++++ .../src/generic_client_goal_handle.cpp | 10 +++++ rclcpp_action/test/test_client.cpp | 37 +++++++++++++++++ rclcpp_action/test/test_generic_client.cpp | 40 +++++++++++++++++++ 7 files changed, 127 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 73987ec887..8d5b9145c4 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/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. + /** + * \return GoalStatusCode::UNKNOWN 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(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 58b1d7f248..28754f40c1 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -103,6 +103,17 @@ ClientGoalHandle::get_status() return status_; } +template +GoalStatusCode +ClientGoalHandle::get_status_code() +{ + const int8_t status = get_status(); + if (status < GoalStatus::STATUS_UNKNOWN || status > GoalStatus::STATUS_ABORTED) { + return GoalStatusCode::UNKNOWN; + } + return static_cast(status); +} + template void ClientGoalHandle::set_status(int8_t status) diff --git a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp index 1d7fa34884..436213e0b5 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp +++ b/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. + /** + * \return GoalStatusCode::UNKNOWN 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 diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 2033891aa9..f383673123 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -34,6 +34,18 @@ using GoalUUID = std::array; 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 diff --git a/rclcpp_action/src/generic_client_goal_handle.cpp b/rclcpp_action/src/generic_client_goal_handle.cpp index b0fad95903..863d663ae2 100644 --- a/rclcpp_action/src/generic_client_goal_handle.cpp +++ b/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) { + return GoalStatusCode::UNKNOWN; + } + return static_cast(status); +} + void GenericClientGoalHandle::set_status(int8_t status) { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index d11866251a..7dbb95dc84 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/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,49 @@ 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(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_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code()); +} + TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -894,6 +929,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) diff --git a/rclcpp_action/test/test_generic_client.cpp b/rclcpp_action/test/test_generic_client.cpp index 10c2bec036..2f8ae1e4f3 100644 --- a/rclcpp_action/test/test_generic_client.cpp +++ b/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(wrapped_result.result); EXPECT_EQ(wrapped_result.code, rclcpp_action::GenericClientGoalHandle::ResultCode::SUCCEEDED); EXPECT_EQ(6ul, result->sequence.size()); @@ -588,16 +590,52 @@ 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_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code()); +} + TEST_F(TestGenericClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) { auto action_generic_client = rclcpp_action::create_generic_client( @@ -858,6 +896,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) From 40b4a7612fded4517b0ab898709f367f1576b8e1 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Thu, 1 Oct 2026 20:25:58 -0600 Subject: [PATCH 2/2] Throw InvalidGoalStatusError from get_status_code() for out-of-range status Co-Authored-By: Claude Opus 5.5 Signed-off-by: David Vadovszki --- .../include/rclcpp_action/client_goal_handle.hpp | 4 ++-- .../include/rclcpp_action/client_goal_handle_impl.hpp | 2 +- rclcpp_action/include/rclcpp_action/exceptions.hpp | 11 +++++++++++ .../rclcpp_action/generic_client_goal_handle.hpp | 4 ++-- rclcpp_action/src/generic_client_goal_handle.cpp | 2 +- rclcpp_action/test/test_client.cpp | 3 ++- rclcpp_action/test/test_generic_client.cpp | 3 ++- 7 files changed, 21 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 8d5b9145c4..36579fb007 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -96,8 +96,8 @@ class ClientGoalHandle /// Get the goal status code as a GoalStatusCode. /** - * \return GoalStatusCode::UNKNOWN if the server sent a status that is not one of the - * action_msgs::msg::GoalStatus values. get_status() still returns the raw value. + * \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(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 28754f40c1..7bed0c3f8a 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -109,7 +109,7 @@ ClientGoalHandle::get_status_code() { const int8_t status = get_status(); if (status < GoalStatus::STATUS_UNKNOWN || status > GoalStatus::STATUS_ABORTED) { - return GoalStatusCode::UNKNOWN; + throw exceptions::InvalidGoalStatusError(status); } return static_cast(status); } diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index a1fcf50bff..20bc224d5a 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__EXCEPTIONS_HPP_ #define RCLCPP_ACTION__EXCEPTIONS_HPP_ +#include #include #include @@ -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 diff --git a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp index 436213e0b5..568e6bc683 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp @@ -88,8 +88,8 @@ class GenericClientGoalHandle /// Get the goal status code as a GoalStatusCode. /** - * \return GoalStatusCode::UNKNOWN if the server sent a status that is not one of the - * action_msgs::msg::GoalStatus values. get_status() still returns the raw value. + * \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 diff --git a/rclcpp_action/src/generic_client_goal_handle.cpp b/rclcpp_action/src/generic_client_goal_handle.cpp index 863d663ae2..00ae1108b5 100644 --- a/rclcpp_action/src/generic_client_goal_handle.cpp +++ b/rclcpp_action/src/generic_client_goal_handle.cpp @@ -94,7 +94,7 @@ GenericClientGoalHandle::get_status_code() { const int8_t status = get_status(); if (status < GoalStatus::STATUS_UNKNOWN || status > GoalStatus::STATUS_ABORTED) { - return GoalStatusCode::UNKNOWN; + throw exceptions::InvalidGoalStatusError(status); } return static_cast(status); } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 7dbb95dc84..c461d1eeed 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -695,7 +695,8 @@ TEST_F(TestClientAgainstServer, get_status_code_out_of_range) client_executor.spin_some(); } ASSERT_EQ(out_of_range_status, goal_handle->get_status()); - EXPECT_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code()); + EXPECT_THROW( + goal_handle->get_status_code(), rclcpp_action::exceptions::InvalidGoalStatusError); } TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) diff --git a/rclcpp_action/test/test_generic_client.cpp b/rclcpp_action/test/test_generic_client.cpp index 2f8ae1e4f3..0bca9acd0b 100644 --- a/rclcpp_action/test/test_generic_client.cpp +++ b/rclcpp_action/test/test_generic_client.cpp @@ -633,7 +633,8 @@ TEST_F(TestGenericClientAgainstServer, get_status_code_out_of_range) client_executor.spin_some(); } ASSERT_EQ(out_of_range_status, goal_handle->get_status()); - EXPECT_EQ(rclcpp_action::GoalStatusCode::UNKNOWN, goal_handle->get_status_code()); + EXPECT_THROW( + goal_handle->get_status_code(), rclcpp_action::exceptions::InvalidGoalStatusError); } TEST_F(TestGenericClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result)