niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

[rcl_action] Implement goal handle (#320)

Browse source 
* Add action goal handle implementation and unit tests
* Add check to goal state machine transition function for index out of bounds
This commit is contained in:
Jacob Perron 2018-11-06 10:28:06 -08:00 • committed by GitHub
parent 1120b2f6a4
commit b2578bbbda
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 498 additions and 59 deletions
Download patch file Download diff file Expand all files Collapse all files

13
rcl_action/CMakeLists.txt
View file

@ -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
)

78
rcl_action/include/rcl_action/goal_handle.h
View file

@ -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 <rcl/rcl.h>
* #include <rcl_action/rcl_action.h>
* #include <rosidl_generator_c/action_type_support_struct.h>
* #include <example_interfaces/action/fibonacci.h>
*
* // ... 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().
*
* <hr>
* 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.
*
* <hr>
* 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
}

151
rcl_action/src/rcl_action/goal_handle.c Normal file
View file

@ -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

7
rcl_action/src/rcl_action/goal_state_machine.c
View file

@ -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;

308
rcl_action/test/rcl_action/test_goal_handle.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#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<uint8_t>(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<rcl_action_goal_event_t, rcl_action_goal_state_t>;
using StateTransitionSequence = std::vector<EventStatePair>;
const std::vector<std::string> event_strs = {
"EXECUTE", "CANCEL", "SET_SUCCEEDED", "SET_ABORTED", "SET_CANCELED"};
class TestGoalHandleStateTransitionSequence
: public ::testing::TestWithParam<StateTransitionSequence>
{
public:
static std::string print_sequence_param_name(
const testing::TestParamInfo<StateTransitionSequence> & 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);