From b2578bbbda6d354ec332f7cd79d55d6ce211c595 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 6 Nov 2018 10:28:06 -0800 Subject: [PATCH] [rcl_action] Implement goal handle (#320) * Add action goal handle implementation and unit tests * Add check to goal state machine transition function for index out of bounds --- rcl_action/CMakeLists.txt | 13 + rcl_action/include/rcl_action/goal_handle.h | 78 ++--- rcl_action/src/rcl_action/goal_handle.c | 151 +++++++++ .../src/rcl_action/goal_state_machine.c | 7 + .../test/rcl_action/test_goal_handle.cpp | 308 ++++++++++++++++++ 5 files changed, 498 insertions(+), 59 deletions(-) create mode 100644 rcl_action/src/rcl_action/goal_handle.c create mode 100644 rcl_action/test/rcl_action/test_goal_handle.cpp diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index b92f12d..83c9931 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -32,6 +32,7 @@ add_executable(test_compile_headers ) set(rcl_action_sources + src/${PROJECT_NAME}/goal_handle.c src/${PROJECT_NAME}/goal_state_machine.c src/${PROJECT_NAME}/names.c src/${PROJECT_NAME}/types.c @@ -71,6 +72,18 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_find_gtest() # Gtests + ament_add_gtest(test_goal_handle + test/rcl_action/test_goal_handle.cpp + ) + if(TARGET test_goal_handle) + target_include_directories(test_goal_handle PUBLIC + include + ${rcl_INCLUDE_DIRS} + ) + target_link_libraries(test_goal_handle + ${PROJECT_NAME} + ) + endif() ament_add_gtest(test_goal_state_machine test/rcl_action/test_goal_state_machine.cpp ) diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h index 2ef5cc4..f5032d1 100644 --- a/rcl_action/include/rcl_action/goal_handle.h +++ b/rcl_action/include/rcl_action/goal_handle.h @@ -23,9 +23,8 @@ extern "C" #include "rcl_action/goal_state_machine.h" #include "rcl_action/types.h" #include "rcl_action/visibility_control.h" +#include "rcl/allocator.h" -// Forward declare -typedef struct rcl_action_server_t rcl_action_server_t; /// Internal rcl action goal implementation struct. struct rcl_action_goal_handle_impl_t; @@ -55,33 +54,10 @@ rcl_action_get_zero_initialized_goal_handle(void); * Goal information can be accessed with rcl_action_goal_handle_get_message() and * rcl_action_goal_handle_get_info(). * - * The given rcl_action_server_t must be valid and the resulting rcl_action_goal_handle_t is - * only valid as long as the given rcl_action_server_t remains valid. - * - * Expected usage: - * - * ```c - * #include - * #include - * #include - * #include - * - * // ... initialize node - * const rosidl_action_type_support_t * ts = - * ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci); - * rcl_action_server_t action_server = rcl_action_get_zero_initialized_server(); - * rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - * ret = rcl_action_server_init(&action_server, &node, ts, "fibonacci", &action_server_ops); - * // ... error handling - * rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle(); - * ret = rcl_action_goal_handle_init(&goal_handle, &action_server); - * // ... error handling, and on shutdown do finalization: - * ret = rcl_action_goal_handle_fini(&goal_handle); - * // ... error handling for rcl_goal_handle_fini() - * ret = rcl_action_server_fini(&action_server, &node); - * // ... error handling for rcl_action_server_fini() - * // ... finalize and error handling for node - * ``` + * Goal handles are typically initialized and finalized by action servers. + * I.e. The allocator should be provided by the action server. + * Goal handles are created with rcl_action_accept_new_goal() and destroyed with + * rcl_action_clear_expired_goals() or rcl_action_server_fini(). * *
* Attribute | Adherence @@ -93,20 +69,21 @@ rcl_action_get_zero_initialized_goal_handle(void); * * \param[out] goal_handle preallocated, zero-initialized, goal handle structure * to be initialized - * \param[in] action_server valid rcl action server - * \param[in] type_support type support object for the action's type + * \param[in] goal_info information about the goal to be copied to the goal handle + * \param[in] allocator a valid allocator used to initialized the goal handle * \return `RCL_RET_OK` if goal_handle was initialized successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if the allocator is invalid, or * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or - * \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or - * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. + * \return `RCL_RET_ALREADY_INIT` if the goal handle has already been initialized, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_action_goal_handle_init( rcl_action_goal_handle_t * goal_handle, - const rcl_action_server_t * action_server); + rcl_action_goal_info_t * goal_info, + rcl_allocator_t allocator); /// Finalize a rcl_action_goal_handle_t. /** @@ -128,11 +105,9 @@ rcl_action_goal_handle_init( * Uses Atomics | No * Lock-Free | Yes * - * \param[in] goal_handle struct to be deinitialized - * \param[in] action_server used to create the goal handle + * \param[inout] goal_handle struct to be deinitialized * \return `RCL_RET_OK` if the goal handle was deinitialized successfully, or * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED @@ -156,7 +131,6 @@ rcl_action_goal_handle_fini(rcl_action_goal_handle_t * goal_handle); * \return `RCL_RET_OK` if the goal state was updated successfully, or * \return `RCL_RET_ACTION_GOAL_EVENT_INVALID` if the goal event is invalid, or * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED @@ -181,7 +155,7 @@ rcl_action_update_goal_state( * \param[out] goal_info a preallocated struct where the goal info is copied * \return `RCL_RET_OK` if the goal ID was accessed successfully, or * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. + * \return `RCL_RET_INVALID_ARGUMENT` if the goal_info argument is invalid */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED @@ -206,7 +180,7 @@ rcl_action_goal_handle_get_info( * \param[out] status a preallocated struct where the goal status is copied * \return `RCL_RET_OK` if the goal ID was accessed successfully, or * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. + * \return `RCL_RET_INVALID_ARGUMENT` if the status argument is invalid */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED @@ -219,10 +193,6 @@ rcl_action_goal_handle_get_status( /** * This is a non-blocking call. * - * The allocator needs to either be a valid allocator or `NULL`, in which case - * the default allocator will be used. - * The allocator is used when allocation is needed for an error message. - * *
* Attribute | Adherence * ------------------ | ------------- @@ -232,26 +202,19 @@ rcl_action_goal_handle_get_status( * Lock-Free | Yes * * \param[in] goal_handle struct containing the goal and metadata - * \param[in] error_msg_allocator a valid allocator or `NULL` * \return `true` if a goal is in one of the following states: ACCEPTED, EXECUTING, or CANCELING, or * \return `false` otherwise, also - * \return `false` if the goal handle pointer is invalid or the allocator is invalid + * \return `false` if the goal handle pointer is invalid */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED bool -rcl_action_goal_handle_is_active( - const rcl_action_goal_handle_t * goal_handle, - rcl_allocator_t * error_msg_allocator); +rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle); /// Check if a rcl_action_goal_handle_t is valid. /** * This is a non-blocking call. * - * The allocator needs to either be a valid allocator or `NULL`, in which case - * the default allocator will be used. - * The allocator is used when allocation is needed for an error message. - * * A goal handle is invalid if: * - the implementation is `NULL` (rcl_action_goal_handle_init() not called or failed) * - rcl_shutdown() has been called since the goal handle has been initialized @@ -266,16 +229,13 @@ rcl_action_goal_handle_is_active( * Lock-Free | Yes * * \param[in] goal_handle struct to evaluate as valid or not - * \param[in] error_msg_allocator a valid allocator or `NULL` * \return `true` if the goal handle is valid, `false` otherwise, also - * \return `false` if the allocator is invalid + * \return `false` if the goal handle pointer is null */ RCL_ACTION_PUBLIC RCL_WARN_UNUSED bool -rcl_action_goal_handle_is_valid( - const rcl_action_goal_handle_t * goal_handle, - rcl_allocator_t * error_msg_allocator); +rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle); #ifdef __cplusplus } diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c new file mode 100644 index 0000000..dce06dc --- /dev/null +++ b/rcl_action/src/rcl_action/goal_handle.c @@ -0,0 +1,151 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl_action/goal_handle.h" + +#include "rcl/rcl.h" +#include "rcl/error_handling.h" + +typedef struct rcl_action_goal_handle_impl_t +{ + rcl_action_goal_info_t info; + rcl_action_goal_state_t state; + rcl_allocator_t allocator; +} rcl_action_goal_handle_impl_t; + +rcl_action_goal_handle_t +rcl_action_get_zero_initialized_goal_handle(void) +{ + static rcl_action_goal_handle_t null_handle = {0}; + return null_handle; +} + +rcl_ret_t +rcl_action_goal_handle_init( + rcl_action_goal_handle_t * goal_handle, + rcl_action_goal_info_t * goal_info, + rcl_allocator_t allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + // Ensure the goal handle is zero initialized + if (goal_handle->impl) { + RCL_SET_ERROR_MSG("goal_handle already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + // Allocate space for the goal handle impl + goal_handle->impl = (rcl_action_goal_handle_impl_t *)allocator.allocate( + sizeof(rcl_action_goal_handle_impl_t), allocator.state); + if (!goal_handle->impl) { + RCL_SET_ERROR_MSG("goal_handle memory allocation failed"); + return RCL_RET_BAD_ALLOC; + } + // Copy goal info (assuming it is trivially copyable) + goal_handle->impl->info = *goal_info; + // Initialize state to ACCEPTED + goal_handle->impl->state = GOAL_STATE_ACCEPTED; + // Copy the allocator + goal_handle->impl->allocator = allocator; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_goal_handle_fini(rcl_action_goal_handle_t * goal_handle) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_ACTION_GOAL_HANDLE_INVALID); + if (goal_handle->impl) { + goal_handle->impl->allocator.deallocate(goal_handle->impl, goal_handle->impl->allocator.state); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_update_goal_state( + rcl_action_goal_handle_t * goal_handle, + const rcl_action_goal_event_t goal_event) +{ + if (!rcl_action_goal_handle_is_valid(goal_handle)) { + return RCL_RET_ACTION_GOAL_HANDLE_INVALID; // error message is set + } + rcl_action_goal_state_t new_state = rcl_action_transition_goal_state( + goal_handle->impl->state, goal_event); + if (GOAL_STATE_UNKNOWN == new_state) { + return RCL_RET_ACTION_GOAL_EVENT_INVALID; + } + goal_handle->impl->state = new_state; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_goal_handle_get_info( + const rcl_action_goal_handle_t * goal_handle, + rcl_action_goal_info_t * goal_info) +{ + if (!rcl_action_goal_handle_is_valid(goal_handle)) { + return RCL_RET_ACTION_GOAL_HANDLE_INVALID; // error message is set + } + RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, RCL_RET_INVALID_ARGUMENT); + // Assumption: goal info is trivially copyable + *goal_info = goal_handle->impl->info; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_goal_handle_get_status( + const rcl_action_goal_handle_t * goal_handle, + rcl_action_goal_state_t * status) +{ + if (!rcl_action_goal_handle_is_valid(goal_handle)) { + return RCL_RET_ACTION_GOAL_HANDLE_INVALID; // error message is set + } + RCL_CHECK_ARGUMENT_FOR_NULL(status, RCL_RET_INVALID_ARGUMENT); + *status = goal_handle->impl->state; + return RCL_RET_OK; +} + +bool +rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle) +{ + if (!rcl_action_goal_handle_is_valid(goal_handle)) { + return false; // error message is set + } + switch (goal_handle->impl->state) { + case GOAL_STATE_ACCEPTED: + case GOAL_STATE_EXECUTING: + case GOAL_STATE_CANCELING: + return true; + default: + return false; + } +} + +bool +rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(goal_handle, "goal handle pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + goal_handle->impl, "goal handle implementation is invalid", return false); + return true; +} + +#ifdef __cplusplus +} +#endif diff --git a/rcl_action/src/rcl_action/goal_state_machine.c b/rcl_action/src/rcl_action/goal_state_machine.c index 7e1ea59..cfb38fd 100644 --- a/rcl_action/src/rcl_action/goal_state_machine.c +++ b/rcl_action/src/rcl_action/goal_state_machine.c @@ -98,6 +98,13 @@ rcl_action_transition_goal_state( const rcl_action_goal_state_t state, const rcl_action_goal_event_t event) { + // event < 0 is always false since it is an unsigned enum + if (state < 0 || + state >= GOAL_STATE_NUM_STATES || + event >= GOAL_EVENT_NUM_EVENTS) + { + return GOAL_STATE_UNKNOWN; + } rcl_action_goal_event_handler handler = _goal_state_transition_map[state][event]; if (NULL == handler) { return GOAL_STATE_UNKNOWN; diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp new file mode 100644 index 0000000..6700eaa --- /dev/null +++ b/rcl_action/test/rcl_action/test_goal_handle.cpp @@ -0,0 +1,308 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include +#include +#include +#include + +#include "rcl_action/goal_handle.h" +#include "rcl_action/types.h" + +#include "rcl/error_handling.h" + +TEST(TestGoalHandle, test_goal_handle_init_fini) +{ + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + + // Initialize with a null goal handle + rcl_ret_t ret = rcl_action_goal_handle_init(nullptr, &goal_info, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Initialize with a null goal info + rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle(); + EXPECT_EQ(goal_handle.impl, nullptr); + ret = rcl_action_goal_handle_init(&goal_handle, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Initialize with an invalid allocator + rcl_allocator_t invalid_allocator = (rcl_allocator_t)rcutils_get_zero_initialized_allocator(); + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, invalid_allocator); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Initialize with valid goal handle and allocator + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_NE(goal_handle.impl, nullptr); + + // Try to initialize again + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_ALREADY_INIT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Finalize with null goal handle + ret = rcl_action_goal_handle_fini(nullptr); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Finalize with valid goal handle + ret = rcl_action_goal_handle_fini(&goal_handle); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} + +TEST(TestGoalHandle, test_goal_handle_is_valid) +{ + // Check null goal handle + bool is_valid = rcl_action_goal_handle_is_valid(nullptr); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check uninitialized goal handle + rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle(); + is_valid = rcl_action_goal_handle_is_valid(&goal_handle); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check valid goal handle + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rcl_ret_t ret = rcl_action_goal_handle_init( + &goal_handle, &goal_info, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + is_valid = rcl_action_goal_handle_is_valid(&goal_handle); + EXPECT_TRUE(is_valid) << rcl_get_error_string().str; + + // Finalize + ret = rcl_action_goal_handle_fini(&goal_handle); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} + +TEST(TestGoalHandle, test_goal_handle_get_info) +{ + // Initialize a goal info message to test + rcl_action_goal_info_t goal_info_input = rcl_action_get_zero_initialized_goal_info(); + for (int i = 0; i < 16; ++i) { + goal_info_input.uuid[i] = static_cast(i); + } + goal_info_input.stamp.sec = 123; + goal_info_input.stamp.nanosec = 456u; + + // Check with null goal handle + rcl_action_goal_info_t goal_info_output = rcl_action_get_zero_initialized_goal_info(); + rcl_ret_t ret = rcl_action_goal_handle_get_info(nullptr, &goal_info_output); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with invalid goal handle + rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle(); + ret = rcl_action_goal_handle_get_info(&goal_handle, &goal_info_output); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with null goal info + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info_input, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_goal_handle_get_info(&goal_handle, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with valid arguments + ret = rcl_action_goal_handle_get_info(&goal_handle, &goal_info_output); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + for (int i = 0; i < 16; ++i) { + EXPECT_EQ(goal_info_input.uuid[i], goal_info_output.uuid[i]); + } + EXPECT_EQ(goal_info_input.stamp.sec, goal_info_output.stamp.sec); + EXPECT_EQ(goal_info_input.stamp.nanosec, goal_info_output.stamp.nanosec); + + // Finalize + ret = rcl_action_goal_handle_fini(&goal_handle); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} + +TEST(TestGoalHandle, test_goal_handle_update_state_invalid) +{ + // Check with null argument + rcl_ret_t ret = rcl_action_update_goal_state(nullptr, GOAL_EVENT_EXECUTE); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with invalid goal handle + rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle(); + ret = rcl_action_update_goal_state(&goal_handle, GOAL_EVENT_NUM_EVENTS); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with invalid goal event + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_update_goal_state(&goal_handle, GOAL_EVENT_NUM_EVENTS); + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); +} + +using EventStatePair = std::pair; +using StateTransitionSequence = std::vector; +const std::vector event_strs = { + "EXECUTE", "CANCEL", "SET_SUCCEEDED", "SET_ABORTED", "SET_CANCELED"}; + +class TestGoalHandleStateTransitionSequence + : public ::testing::TestWithParam +{ +public: + static std::string print_sequence_param_name( + const testing::TestParamInfo & info) + { + std::stringstream result; + for (const EventStatePair & event_state : info.param) { + result << "_" << event_strs[event_state.first]; + } + return result.str(); + } + +protected: + rcl_action_goal_handle_t goal_handle; + StateTransitionSequence test_sequence; + + void expect_state_eq(const rcl_action_goal_state_t expected_state) + { + rcl_action_goal_state_t state; + rcl_ret_t ret = rcl_action_goal_handle_get_status(&this->goal_handle, &state); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(state, expected_state); + } + + void SetUp() + { + // Initialize goal info + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + + // Initialize goal handle + this->goal_handle = rcl_action_get_zero_initialized_goal_handle(); + rcl_ret_t ret = rcl_action_goal_handle_init( + &this->goal_handle, &goal_info, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Get test sequence + this->test_sequence = GetParam(); + } + + void TearDown() + { + rcl_ret_t ret = rcl_action_goal_handle_fini(&this->goal_handle); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions) +{ + // Goal handle starts in state ACCEPTED + expect_state_eq(GOAL_STATE_ACCEPTED); + + // Walk through state transitions + rcl_ret_t ret; + for (const EventStatePair & event_state : this->test_sequence) { + ret = rcl_action_update_goal_state(&this->goal_handle, event_state.first); + const rcl_action_goal_state_t & expected_state = event_state.second; + if (GOAL_STATE_UNKNOWN == expected_state) { + EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID); + continue; + } + EXPECT_EQ(ret, RCL_RET_OK); + expect_state_eq(expected_state); + } +} + +// Test sequence parameters +// Note, each sequence starts in the ACCEPTED state +const StateTransitionSequence valid_state_transition_sequences[] = { + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_CANCELED, GOAL_STATE_CANCELED}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED}, + }, + { + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_CANCELED, GOAL_STATE_CANCELED}, + }, + { + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED}, + }, + // This is an odd case, but valid nonetheless + { + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED}, + }, +}; + +INSTANTIATE_TEST_CASE_P( + TestValidGoalHandleStateTransitions, + TestGoalHandleStateTransitionSequence, + ::testing::ValuesIn(valid_state_transition_sequences), + TestGoalHandleStateTransitionSequence::print_sequence_param_name); + +const StateTransitionSequence invalid_state_transition_sequences[] = { + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_EXECUTE, GOAL_STATE_UNKNOWN}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING}, + {GOAL_EVENT_CANCEL, GOAL_STATE_UNKNOWN}, + }, + { + {GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING}, + {GOAL_EVENT_EXECUTE, GOAL_STATE_UNKNOWN}, + }, + { + {GOAL_EVENT_SET_CANCELED, GOAL_STATE_UNKNOWN}, + }, + { + {GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_UNKNOWN}, + }, + { + {GOAL_EVENT_SET_ABORTED, GOAL_STATE_UNKNOWN}, + }, +}; + +INSTANTIATE_TEST_CASE_P( + TestInvalidGoalHandleStateTransitions, + TestGoalHandleStateTransitionSequence, + ::testing::ValuesIn(invalid_state_transition_sequences), + TestGoalHandleStateTransitionSequence::print_sequence_param_name);