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)