diff --git a/rcl_lifecycle/CMakeLists.txt b/rcl_lifecycle/CMakeLists.txt index 4cb2ec5..ce6e6ea 100644 --- a/rcl_lifecycle/CMakeLists.txt +++ b/rcl_lifecycle/CMakeLists.txt @@ -89,6 +89,16 @@ if(BUILD_TESTING) ) target_link_libraries(test_multiple_instances ${PROJECT_NAME}) endif() + ament_add_gtest(test_rcl_lifecycle + test/test_rcl_lifecycle.cpp + ) + if(TARGET test_rcl_lifecycle) + ament_target_dependencies(test_rcl_lifecycle + "rcl" + "osrf_testing_tools_cpp" + ) + target_link_libraries(test_rcl_lifecycle ${PROJECT_NAME}) + endif() ament_add_gtest(test_transition_map test/test_transition_map.cpp ) diff --git a/rcl_lifecycle/src/default_state_machine.c b/rcl_lifecycle/src/default_state_machine.c index 20272b5..17eeb82 100644 --- a/rcl_lifecycle/src/default_state_machine.c +++ b/rcl_lifecycle/src/default_state_machine.c @@ -650,13 +650,15 @@ _register_transitions( return ret; } -// default implementation as despicted on +// default implementation as depicted on // design.ros2.org rcl_ret_t rcl_lifecycle_init_default_state_machine( rcl_lifecycle_state_machine_t * state_machine, const rcutils_allocator_t * allocator) { rcl_ret_t ret = RCL_RET_ERROR; + // Used for concatenating error messages in the fail: block. + const char * fail_error_message = ""; // *************************** // register all primary states @@ -691,8 +693,26 @@ rcl_lifecycle_init_default_state_machine( return ret; fail: + // If rcl_lifecycle_transition_map_fini() fails, it will clobber the error string here. + // Concatenate the error strings if that happens + if (rcl_error_is_set()) { + fail_error_message = rcl_get_error_string().str; + } + if (rcl_lifecycle_transition_map_fini(&state_machine->transition_map, allocator) != RCL_RET_OK) { - RCL_SET_ERROR_MSG("could not free lifecycle transition map. Leaking memory!\n"); + const char * fini_error = ""; + if (rcl_error_is_set()) { + fini_error = rcl_get_error_string().str; + rcl_reset_error(); + } + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Freeing transition map failed while handling a previous error. Leaking memory!" + "\nOriginal error:\n\t%s\nError encountered in rcl_lifecycle_transition_map_fini():\n\t%s\n", + fail_error_message, fini_error); + } + + if (!rcl_error_is_set()) { + RCL_SET_ERROR_MSG("Unspecified error in default_state_machine _register_transitions()"); } return RCL_RET_ERROR; } diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index de1b29c..ce6c076 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -40,6 +40,8 @@ rcl_lifecycle_get_zero_initialized_state() rcl_lifecycle_state_t state; state.id = 0; state.label = NULL; + state.valid_transitions = NULL; + state.valid_transition_size = 0; return state; } @@ -58,6 +60,10 @@ rcl_lifecycle_state_init( RCL_SET_ERROR_MSG("state pointer is null\n"); return RCL_RET_ERROR; } + if (!label) { + RCL_SET_ERROR_MSG("State label is null\n"); + return RCL_RET_ERROR; + } state->id = id; state->label = rcutils_strndup(label, strlen(label), *allocator); @@ -118,7 +124,22 @@ rcl_lifecycle_transition_init( if (!transition) { RCL_SET_ERROR_MSG("transition pointer is null\n"); - return RCL_RET_OK; + return RCL_RET_ERROR; + } + + if (!label) { + RCL_SET_ERROR_MSG("label pointer is null\n"); + return RCL_RET_ERROR; + } + + if (!start) { + RCL_SET_ERROR_MSG("start state pointer is null\n"); + return RCL_RET_ERROR; + } + + if (!goal) { + RCL_SET_ERROR_MSG("goal state pointer is null\n"); + return RCL_RET_ERROR; } transition->start = start; @@ -140,7 +161,7 @@ rcl_lifecycle_transition_fini( const rcl_allocator_t * allocator) { if (!allocator) { - RCL_SET_ERROR_MSG("can't initialize transition, no allocator given\n"); + RCL_SET_ERROR_MSG("can't finalize transition, no allocator given\n"); return RCL_RET_ERROR; } // it is already NULL @@ -192,6 +213,14 @@ rcl_lifecycle_state_machine_init( bool default_states, const rcl_allocator_t * allocator) { + if (!state_machine) { + RCL_SET_ERROR_MSG("State machine is null\n"); + return RCL_RET_ERROR; + } + if (!node_handle) { + RCL_SET_ERROR_MSG("Node handle is null\n"); + return RCL_RET_ERROR; + } if (!allocator) { RCL_SET_ERROR_MSG("can't initialize state machine, no allocator given\n"); return RCL_RET_ERROR; @@ -207,15 +236,12 @@ rcl_lifecycle_state_machine_init( } if (default_states) { - rcl_ret_t ret = - rcl_lifecycle_init_default_state_machine(state_machine, allocator); + ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator); if (ret != RCL_RET_OK) { // init default state machine might have allocated memory, // so we have to call fini - if (rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator) != RCL_RET_OK) { - // error already set - return RCL_RET_ERROR; - } + ret = rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator); + return RCL_RET_ERROR; } } diff --git a/rcl_lifecycle/src/transition_map.c b/rcl_lifecycle/src/transition_map.c index c8b467a..aa5ac51 100644 --- a/rcl_lifecycle/src/transition_map.c +++ b/rcl_lifecycle/src/transition_map.c @@ -53,6 +53,11 @@ rcl_lifecycle_transition_map_fini( rcl_lifecycle_transition_map_t * transition_map, const rcutils_allocator_t * allocator) { + if (!allocator) { + RCL_SET_ERROR_MSG("can't free transition map, no allocator given\n"); + return RCL_RET_ERROR; + } + rcl_ret_t fcn_ret = RCL_RET_OK; // free valid transitions for all states @@ -87,15 +92,16 @@ rcl_lifecycle_register_state( allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT) // add new primary state memory - transition_map->states_size += 1; + unsigned int new_states_size = transition_map->states_size + 1; rcl_lifecycle_state_t * new_states = allocator->reallocate( transition_map->states, - transition_map->states_size * sizeof(rcl_lifecycle_state_t), + new_states_size * sizeof(rcl_lifecycle_state_t), allocator->state); if (!new_states) { RCL_SET_ERROR_MSG("failed to reallocate memory for new states"); return RCL_RET_ERROR; } + transition_map->states_size = new_states_size; transition_map->states = new_states; transition_map->states[transition_map->states_size - 1] = state; @@ -117,16 +123,22 @@ rcl_lifecycle_register_transition( return RCL_RET_ERROR; } - // we add a new transition, so increase the size - transition_map->transitions_size += 1; + rcl_lifecycle_state_t * goal = rcl_lifecycle_get_state(transition_map, transition.goal->id); + if (!goal) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("state %u is not registered\n", transition.goal->id); + return RCL_RET_ERROR; + } + // Attempt to add new transition, don't update map if it fails + unsigned int new_transitions_size = transition_map->transitions_size + 1; rcl_lifecycle_transition_t * new_transitions = allocator->reallocate( transition_map->transitions, - transition_map->transitions_size * sizeof(rcl_lifecycle_transition_t), + new_transitions_size * sizeof(rcl_lifecycle_transition_t), allocator->state); if (!new_transitions) { RCL_SET_ERROR_MSG("failed to reallocate memory for new transitions"); return RCL_RET_BAD_ALLOC; } + transition_map->transitions_size = new_transitions_size; transition_map->transitions = new_transitions; // finally set the new transition to the end of the array transition_map->transitions[transition_map->transitions_size - 1] = transition; @@ -134,15 +146,16 @@ rcl_lifecycle_register_transition( // we have to copy the transitons here once more to the actual state // as we can't assign only the pointer. This pointer gets invalidated whenever // we add a new transition and re-shuffle/re-allocate new memory for it. - state->valid_transition_size += 1; + unsigned int new_valid_transitions_size = state->valid_transition_size + 1; rcl_lifecycle_transition_t * new_valid_transitions = allocator->reallocate( state->valid_transitions, - state->valid_transition_size * sizeof(rcl_lifecycle_transition_t), + new_valid_transitions_size * sizeof(rcl_lifecycle_transition_t), allocator->state); if (!new_valid_transitions) { RCL_SET_ERROR_MSG("failed to reallocate memory for new transitions on state"); return RCL_RET_ERROR; } + state->valid_transition_size = new_valid_transitions_size; state->valid_transitions = new_valid_transitions; state->valid_transitions[state->valid_transition_size - 1] = transition; diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp index d5827ac..afd66c3 100644 --- a/rcl_lifecycle/test/test_default_state_machine.cpp +++ b/rcl_lifecycle/test/test_default_state_machine.cpp @@ -113,7 +113,13 @@ TEST_F(TestDefaultStateMachine, zero_init) { TEST_F(TestDefaultStateMachine, default_init) { rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); - auto ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator); + // Because this init method is so complex, the succession of failures caused by a null + // allocator will result in several error messages overwriting themselves. + auto ret = rcl_lifecycle_init_default_state_machine(&state_machine, nullptr); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcutils_reset_error(); + + ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator); diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp new file mode 100644 index 0000000..e429cd8 --- /dev/null +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -0,0 +1,427 @@ +// Copyright 2020 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. + +// testing default transition sequence. +// This test requires that the transitions are set +// as depicted in design.ros2.org + +#include + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "lifecycle_msgs/msg/transition_event.h" +#include "lifecycle_msgs/srv/change_state.h" +#include "lifecycle_msgs/srv/get_available_states.h" +#include "lifecycle_msgs/srv/get_available_transitions.h" +#include "lifecycle_msgs/srv/get_state.h" + +static void * bad_malloc(size_t, void *) +{ + return nullptr; +} + +static void * bad_realloc(void *, size_t, void *) +{ + return nullptr; +} + +TEST(TestRclLifecycle, lifecycle_state) { + rcl_lifecycle_state_t state = rcl_lifecycle_get_zero_initialized_state(); + EXPECT_EQ(state.id, 0u); + EXPECT_EQ(state.label, nullptr); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + unsigned int expected_id = 42; + const char expected_label[] = "label"; + rcl_ret_t ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_init(&state, expected_id, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_init(nullptr, expected_id, &expected_label[0], &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + rcl_allocator_t bad_allocator = rcl_get_default_allocator(); + bad_allocator.allocate = bad_malloc; + bad_allocator.reallocate = bad_realloc; + ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], &bad_allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], &allocator); + EXPECT_EQ(state.id, expected_id); + EXPECT_STREQ(state.label, &expected_label[0]); + + ret = rcl_lifecycle_state_fini(&state, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + // Already finalized + ret = rcl_lifecycle_state_fini(nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_lifecycle_state_fini(&state, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} + +TEST(TestRclLifecycle, lifecycle_transition) { + rcl_lifecycle_transition_t transition = rcl_lifecycle_get_zero_initialized_transition(); + EXPECT_EQ(transition.id, 0u); + EXPECT_EQ(transition.label, nullptr); + EXPECT_EQ(transition.start, nullptr); + EXPECT_EQ(transition.goal, nullptr); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // These need to be allocated on heap so rcl_lifecycle_transition_fini doesn't free a stack + // allocated variable + rcl_lifecycle_state_t * start = reinterpret_cast( + allocator.allocate(sizeof(rcl_lifecycle_state_t), allocator.state)); + EXPECT_NE(start, nullptr); + rcl_lifecycle_state_t * end = reinterpret_cast( + allocator.allocate(sizeof(rcl_lifecycle_state_t), allocator.state)); + EXPECT_NE(end, nullptr); + const char start_label[] = "start"; + const char end_label[] = "end"; + *start = rcl_lifecycle_get_zero_initialized_state(); + *end = rcl_lifecycle_get_zero_initialized_state(); + + rcl_ret_t ret = rcl_lifecycle_state_init(start, 0u, &start_label[0], &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_lifecycle_state_init(end, 1u, &end_label[0], &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + unsigned int expected_id = 42; + const char expected_label[] = "label"; + + ret = rcl_lifecycle_transition_init( + nullptr, expected_id, nullptr, nullptr, nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + &transition, expected_id, nullptr, nullptr, nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + nullptr, expected_id, nullptr, nullptr, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + &transition, expected_id, nullptr, nullptr, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + &transition, expected_id, &expected_label[0], nullptr, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + &transition, expected_id, &expected_label[0], start, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + rcl_allocator_t bad_allocator = rcl_get_default_allocator(); + bad_allocator.allocate = bad_malloc; + bad_allocator.reallocate = bad_realloc; + ret = rcl_lifecycle_transition_init( + &transition, expected_id, &expected_label[0], start, end, &bad_allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_init( + &transition, expected_id, &expected_label[0], start, end, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(transition.id, expected_id); + EXPECT_STREQ(transition.label, &expected_label[0]); + + ret = rcl_lifecycle_transition_fini(nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_transition_fini(&transition, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + // Already finalized + ret = rcl_lifecycle_transition_fini(nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_lifecycle_transition_fini(&transition, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} + +TEST(TestRclLifecycle, state_machine) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + EXPECT_EQ(state_machine.current_state, nullptr); + EXPECT_EQ(state_machine.transition_map.states, nullptr); + EXPECT_EQ(state_machine.transition_map.transitions, nullptr); + EXPECT_EQ(state_machine.transition_map.states_size, 0u); + EXPECT_EQ(state_machine.transition_map.transitions_size, 0u); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_node_options_t options = rcl_node_get_default_options(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + ret = rcl_node_init(&node, "node", "namespace", &context, &options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + const rosidl_message_type_support_t * pn = + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent); + const rosidl_service_type_support_t * cs = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState); + const rosidl_service_type_support_t * gs = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState); + const rosidl_service_type_support_t * gas = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates); + const rosidl_service_type_support_t * gat = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions); + const rosidl_service_type_support_t * gtg = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions); + + // Check various arguments are null + ret = rcl_lifecycle_state_machine_init( + nullptr, &node, pn, cs, gs, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, nullptr, pn, cs, gs, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, nullptr, cs, gs, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, nullptr, gs, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, nullptr, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, nullptr, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, nullptr, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, nullptr, false, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + + // Everything should be good + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // Transition_map is not initialized + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + + void * temp_function = state_machine.com_interface.srv_change_state.impl; + state_machine.com_interface.srv_change_state.impl = nullptr; + // get_state service is valid, but not change_state service + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + rcutils_reset_error(); + state_machine.com_interface.srv_change_state.impl = + reinterpret_cast(temp_function); + + // Allocate some memory and initialize states and transitions so is_initialized will pass + state_machine.transition_map.states_size = 1u; + state_machine.transition_map.states = reinterpret_cast( + allocator.allocate( + state_machine.transition_map.states_size * sizeof(rcl_lifecycle_state_t), + allocator.state)); + ASSERT_NE(state_machine.transition_map.states, nullptr); + state_machine.transition_map.states[0] = rcl_lifecycle_get_zero_initialized_state(); + + state_machine.transition_map.transitions_size = 1u; + state_machine.transition_map.transitions = + reinterpret_cast(allocator.allocate( + state_machine.transition_map.transitions_size * sizeof(rcl_lifecycle_transition_t), + allocator.state)); + ASSERT_NE(state_machine.transition_map.transitions, nullptr); + state_machine.transition_map.transitions[0] = rcl_lifecycle_get_zero_initialized_transition(); + + EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_OK); + + // allocator is nullptr + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, nullptr); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + + // Node is null + ret = rcl_lifecycle_state_machine_fini(&state_machine, nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_ERROR); + std::cout << "state_machine: " << __LINE__ << std::endl; +} + +TEST(TestRclLifecycle, state_transitions) { + rcl_lifecycle_state_machine_t state_machine = + rcl_lifecycle_get_zero_initialized_state_machine(); + EXPECT_EQ(state_machine.current_state, nullptr); + EXPECT_EQ(state_machine.transition_map.states, nullptr); + EXPECT_EQ(state_machine.transition_map.transitions, nullptr); + EXPECT_EQ(state_machine.transition_map.states_size, 0u); + EXPECT_EQ(state_machine.transition_map.transitions_size, 0u); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_node_options_t options = rcl_node_get_default_options(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + ret = rcl_node_init(&node, "node", "namespace", &context, &options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + const rosidl_message_type_support_t * pn = + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent); + const rosidl_service_type_support_t * cs = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState); + const rosidl_service_type_support_t * gs = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState); + const rosidl_service_type_support_t * gas = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates); + const rosidl_service_type_support_t * gat = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions); + const rosidl_service_type_support_t * gtg = + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions); + + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, gtg, true, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_lifecycle_state_machine_is_initialized(&state_machine); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + const rcl_lifecycle_transition_t * transition = rcl_lifecycle_get_transition_by_id(nullptr, 0); + EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str; + rcutils_reset_error(); + + transition = rcl_lifecycle_get_transition_by_id( + state_machine.current_state, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE); + EXPECT_EQ(transition->id, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE); + + // Update this test with a new invalid number if 42 ever becomes a valid state id + transition = rcl_lifecycle_get_transition_by_id(state_machine.current_state, 42); + EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str; + rcutils_reset_error(); + + transition = rcl_lifecycle_get_transition_by_label(state_machine.current_state, "configure"); + EXPECT_STREQ(transition->label, "configure"); + + transition = rcl_lifecycle_get_transition_by_label(state_machine.current_state, "NOT A LABEL"); + EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_lifecycle_trigger_transition_by_id(nullptr, 0, false); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_trigger_transition_by_id( + &state_machine, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, false); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_lifecycle_trigger_transition_by_label(nullptr, "transition_success", true); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + ret = rcl_lifecycle_trigger_transition_by_label(&state_machine, "transition_success", true); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // If using the public interface to register transitions, this case should already be checked. + state_machine.current_state->valid_transitions[0].goal = nullptr; + ret = rcl_lifecycle_trigger_transition_by_label(&state_machine, "transition_success", true); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcutils_reset_error(); + + rcl_print_state_machine(&state_machine); + EXPECT_FALSE(rcutils_error_is_set()); + + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} diff --git a/rcl_lifecycle/test/test_transition_map.cpp b/rcl_lifecycle/test/test_transition_map.cpp index c37f1e9..9904603 100644 --- a/rcl_lifecycle/test/test_transition_map.cpp +++ b/rcl_lifecycle/test/test_transition_map.cpp @@ -35,11 +35,22 @@ protected: } }; +static void * bad_malloc(size_t, void *) +{ + return nullptr; +} + +static void * bad_realloc(void *, size_t, void *) +{ + return nullptr; +} + TEST_F(TestTransitionMap, zero_initialized) { rcl_lifecycle_transition_map_t transition_map = rcl_lifecycle_get_zero_initialized_transition_map(); EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_transition_map_is_initialized(&transition_map)); + rcutils_reset_error(); rcl_allocator_t allocator = rcl_get_default_allocator(); EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator)); @@ -57,11 +68,25 @@ TEST_F(TestTransitionMap, initialized) { EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_is_initialized(&transition_map)); ret = rcl_lifecycle_register_state(&transition_map, state0, &allocator); - EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_ERROR, ret); + rcutils_reset_error(); rcl_lifecycle_state_t state1 = {"my_state_1", 1, NULL, 0}; ret = rcl_lifecycle_register_state(&transition_map, state1, &allocator); + rcl_lifecycle_state_t unregistered = {"my_state_2", 2, NULL, 0}; + + rcl_allocator_t bad_allocator = rcl_get_default_allocator(); + bad_allocator.allocate = bad_malloc; + bad_allocator.reallocate = bad_realloc; + rcl_lifecycle_state_t * original_ptr = transition_map.states; + size_t original_size = transition_map.states_size; + ret = rcl_lifecycle_register_state(&transition_map, unregistered, &bad_allocator); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcutils_reset_error(); + EXPECT_EQ(transition_map.states, original_ptr); + EXPECT_EQ(original_size, transition_map.states_size); + rcl_lifecycle_state_t * start_state = rcl_lifecycle_get_state(&transition_map, state0.id); rcl_lifecycle_state_t * goal_state = @@ -71,25 +96,70 @@ TEST_F(TestTransitionMap, initialized) { rcl_lifecycle_transition_t transition01 = {"from0to1", 0, start_state, goal_state}; + original_size = transition_map.transitions_size; ret = rcl_lifecycle_register_transition( &transition_map, transition01, &allocator); EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(1u, transition_map.transitions_size); rcl_lifecycle_transition_t transition10 = {"from1to0", 1, goal_state, start_state}; + original_size = transition_map.transitions_size; ret = rcl_lifecycle_register_transition( &transition_map, transition10, &allocator); EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(2u, transition_map.transitions_size); + + rcl_lifecycle_transition_t * old_transitions_ptr = transition_map.transitions; + original_size = transition_map.transitions_size; + rcl_lifecycle_transition_t transition_bad1 = {"from0tobad", 2, + start_state, &unregistered}; + ret = rcl_lifecycle_register_transition( + &transition_map, transition_bad1, &allocator); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcutils_reset_error(); + EXPECT_EQ(old_transitions_ptr, transition_map.transitions); + EXPECT_EQ(original_size, transition_map.transitions_size); + + old_transitions_ptr = transition_map.transitions; + original_size = transition_map.transitions_size; + rcl_lifecycle_transition_t transition_bad2 = {"frombadto1", 3, + &unregistered, goal_state}; + ret = rcl_lifecycle_register_transition( + &transition_map, transition_bad2, &allocator); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcutils_reset_error(); + EXPECT_EQ(old_transitions_ptr, transition_map.transitions); + EXPECT_EQ(original_size, transition_map.transitions_size); + + old_transitions_ptr = transition_map.transitions; + original_size = transition_map.transitions_size; + rcl_lifecycle_transition_t transition00 = {"from0to0", 4, + start_state, start_state}; + ret = rcl_lifecycle_register_transition( + &transition_map, transition00, &bad_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcutils_reset_error(); + EXPECT_EQ(old_transitions_ptr, transition_map.transitions); + EXPECT_EQ(original_size, transition_map.transitions_size); const rcl_lifecycle_transition_t * trans = rcl_lifecycle_get_transition_by_id(start_state, 0); EXPECT_EQ(0u, trans->id); + trans = rcl_lifecycle_get_transitions(&transition_map, 0); + EXPECT_EQ(0u, trans->id); trans = rcl_lifecycle_get_transition_by_label(start_state, "from0to1"); EXPECT_EQ(0u, trans->id); trans = rcl_lifecycle_get_transition_by_id(goal_state, 1); EXPECT_EQ(1u, trans->id); + trans = rcl_lifecycle_get_transitions(&transition_map, 1); + EXPECT_EQ(1u, trans->id); trans = rcl_lifecycle_get_transition_by_label(goal_state, "from1to0"); EXPECT_EQ(1u, trans->id); + // Check nonexistent transition + trans = rcl_lifecycle_get_transitions(&transition_map, 2); + EXPECT_EQ(nullptr, trans); + rcutils_reset_error(); EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator)); }