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

use rcl_allocator for rcl_lifecycle & namespaced topics (#142)

Browse source 
* use rcl_allocator for rcl_lifecycle

* correct return value interpretation

* fix unsigned comparison

* use namespace for lifecycle in-built topics

* linters
This commit is contained in:
Karsten Knese 2017-06-16 23:44:06 +02:00 • committed by Dirk Thomas
parent 0f2519944a
commit c37bfec072
13 changed files with 612 additions and 193 deletions
Download patch file Download diff file Expand all files Collapse all files

10
rcl_lifecycle/CMakeLists.txt
View file

@ -83,6 +83,16 @@ if(BUILD_TESTING)
)
target_link_libraries(test_multiple_instances ${PROJECT_NAME})
endif()
ament_add_gtest(test_transition_map
test/test_transition_map.cpp
)
if(TARGET test_transition_map)
target_include_directories(test_transition_map PUBLIC
${rcl_INCLUDE_DIRS}
${rcl_lifecycle_INCLUDE_DIRS}
)
target_link_libraries(test_transition_map ${PROJECT_NAME})
endif()
endif()
ament_export_dependencies(ament_cmake)

3
rcl_lifecycle/include/rcl_lifecycle/data_types.h
View file

@ -44,6 +44,7 @@ typedef struct rcl_lifecycle_state_t
// is "unconfigured_shutdown". However, the user only specifies
// "shutdown". So we register the "unconfigured_shutdown"
// transition with the impuls "shutdown".
// TODO(karsten1987): Introduce new type for valid_transition_keys
rcl_lifecycle_ret_t * valid_transition_keys;
rcl_lifecycle_transition_t * valid_transitions;
unsigned int valid_transition_size;
@ -84,8 +85,6 @@ typedef struct rcl_lifecycle_state_machine_t
rcl_lifecycle_transition_map_t transition_map;
// Communication interface into a ROS world
rcl_lifecycle_com_interface_t com_interface;
// Allocator used.
rcl_allocator_t allocator;
} rcl_lifecycle_state_machine_t;
#if __cplusplus

6
rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h
View file

@ -40,14 +40,16 @@ rcl_lifecycle_state_machine_init(
const rosidl_service_type_support_t * ts_srv_get_state,
const rosidl_service_type_support_t * ts_srv_get_available_states,
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
bool default_states);
bool default_states,
const rcl_allocator_t * allocator);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_state_machine_fini(
rcl_lifecycle_state_machine_t * state_machine,
rcl_node_t * node_handle);
rcl_node_t * node_handle,
const rcl_allocator_t * allocator);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED

34
rcl_lifecycle/include/rcl_lifecycle/transition_map.h
View file

@ -25,17 +25,39 @@ extern "C"
#endif
RCL_LIFECYCLE_PUBLIC
void
rcl_lifecycle_register_state(
rcl_lifecycle_transition_map_t * map,
rcl_lifecycle_state_t state);
RCL_WARN_UNUSED
rcl_lifecycle_transition_map_t
rcl_lifecycle_get_zero_initialized_transition_map();
RCL_LIFECYCLE_PUBLIC
void
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_transition_map_is_initialized(
const rcl_lifecycle_transition_map_t * transition_map);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_transition_map_fini(
rcl_lifecycle_transition_map_t * transition_map,
const rcl_allocator_t * allocator);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_register_state(
rcl_lifecycle_transition_map_t * map,
rcl_lifecycle_state_t state,
const rcl_allocator_t * allocator);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_register_transition(
rcl_lifecycle_transition_map_t * transition_map,
rcl_lifecycle_transition_t transition,
rcl_lifecycle_ret_t key);
rcl_lifecycle_ret_t key,
const rcl_allocator_t * allocator);
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED

46
rcl_lifecycle/src/com_interface.c
View file

@ -23,7 +23,7 @@ extern "C"
#include <string.h>
#include <rcl/error_handling.h>
#include <rcutils/concat.h>
#include <rcutils/format_string.h>
#include <rmw/validate_full_topic_name.h>
#include <rosidl_generator_c/message_type_support_struct.h>
#include <rosidl_generator_c/string_functions.h>
@ -80,34 +80,31 @@ rcl_lifecycle_com_interface_init(
const rosidl_service_type_support_t * ts_srv_change_state,
const rosidl_service_type_support_t * ts_srv_get_state,
const rosidl_service_type_support_t * ts_srv_get_available_states,
const rosidl_service_type_support_t * ts_srv_get_available_transitions)
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
const rcl_allocator_t * allocator)
{
if (!node_handle) {
RCL_SET_ERROR_MSG("node_handle is null", rcl_get_default_allocator());
return RCL_RET_INVALID_ARGUMENT;
}
const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
if (!node_options) {
return RCL_RET_NODE_INVALID;
}
if (!ts_pub_notify) {
RCL_SET_ERROR_MSG("ts_pub_notify is NULL", node_options->allocator);
RCL_SET_ERROR_MSG("ts_pub_notify is NULL", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
if (!ts_srv_change_state) {
RCL_SET_ERROR_MSG("ts_srv_change_state is NULL", node_options->allocator);
RCL_SET_ERROR_MSG("ts_srv_change_state is NULL", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
if (!ts_srv_get_state) {
RCL_SET_ERROR_MSG("ts_srv_get_state is NULL", node_options->allocator);
RCL_SET_ERROR_MSG("ts_srv_get_state is NULL", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
if (!ts_srv_get_available_states) {
RCL_SET_ERROR_MSG("ts_srv_get_available_states is NULL", node_options->allocator);
RCL_SET_ERROR_MSG("ts_srv_get_available_states is NULL", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
if (!ts_srv_get_available_states) {
RCL_SET_ERROR_MSG("ts_srv_get_available_transitions is NULL", node_options->allocator);
RCL_SET_ERROR_MSG("ts_srv_get_available_transitions is NULL", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
@ -117,7 +114,8 @@ rcl_lifecycle_com_interface_init(
// initialize publisher
{
topic_name = rcutils_concat(node_name, pub_transition_event_suffix, "__");
topic_name = rcutils_format_string(*allocator, "/%s/%s",
node_name, pub_transition_event_suffix);
ret = rcl_lifecycle_validate_topic_name(topic_name);
if (ret != RMW_RET_OK) {
goto fail;
@ -127,7 +125,7 @@ rcl_lifecycle_com_interface_init(
rcl_ret_t ret = rcl_publisher_init(
&com_interface->pub_transition_event, node_handle,
ts_pub_notify, topic_name, &publisher_options);
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
if (ret != RCL_RET_OK) {
@ -140,7 +138,8 @@ rcl_lifecycle_com_interface_init(
// initialize change state service
{
topic_name = rcutils_concat(node_name, srv_change_state_suffix, "__");
topic_name = rcutils_format_string(*allocator, "/%s/%s",
node_name, srv_change_state_suffix);
ret = rcl_lifecycle_validate_topic_name(topic_name);
if (ret != RMW_RET_OK) {
goto fail;
@ -150,7 +149,7 @@ rcl_lifecycle_com_interface_init(
rcl_ret_t ret = rcl_service_init(
&com_interface->srv_change_state, node_handle,
ts_srv_change_state, topic_name, &service_options);
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
if (ret != RCL_RET_OK) {
@ -160,7 +159,8 @@ rcl_lifecycle_com_interface_init(
// initialize get state service
{
topic_name = rcutils_concat(node_name, srv_get_state_suffix, "__");
topic_name = rcutils_format_string(*allocator, "/%s/%s",
node_name, srv_get_state_suffix);
ret = rcl_lifecycle_validate_topic_name(topic_name);
if (ret != RMW_RET_OK) {
goto fail;
@ -170,7 +170,7 @@ rcl_lifecycle_com_interface_init(
rcl_ret_t ret = rcl_service_init(
&com_interface->srv_get_state, node_handle,
ts_srv_get_state, topic_name, &service_options);
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
if (ret != RCL_RET_OK) {
@ -180,7 +180,8 @@ rcl_lifecycle_com_interface_init(
// initialize get available states service
{
topic_name = rcutils_concat(node_name, srv_get_available_states_suffix, "__");
topic_name = rcutils_format_string(*allocator, "/%s/%s",
node_name, srv_get_available_states_suffix);
ret = rcl_lifecycle_validate_topic_name(topic_name);
if (ret != RMW_RET_OK) {
goto fail;
@ -190,7 +191,7 @@ rcl_lifecycle_com_interface_init(
rcl_ret_t ret = rcl_service_init(
&com_interface->srv_get_available_states, node_handle,
ts_srv_get_available_states, topic_name, &service_options);
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
if (ret != RCL_RET_OK) {
@ -200,7 +201,8 @@ rcl_lifecycle_com_interface_init(
// initialize get available transitions service
{
topic_name = rcutils_concat(node_name, srv_get_available_transitions_suffix, "__");
topic_name = rcutils_format_string(*allocator, "/%s/%s",
node_name, srv_get_available_transitions_suffix);
ret = rcl_lifecycle_validate_topic_name(topic_name);
if (ret != RMW_RET_OK) {
goto fail;
@ -210,7 +212,7 @@ rcl_lifecycle_com_interface_init(
rcl_ret_t ret = rcl_service_init(
&com_interface->srv_get_available_transitions, node_handle,
ts_srv_get_available_transitions, topic_name, &service_options);
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
if (ret != RCL_RET_OK) {
@ -243,7 +245,7 @@ fail:
}
if (topic_name) {
free(topic_name);
allocator->deallocate(topic_name, allocator->state);
topic_name = NULL;
}

3
rcl_lifecycle/src/com_interface.h
View file

@ -36,7 +36,8 @@ rcl_lifecycle_com_interface_init(
const rosidl_service_type_support_t * ts_srv_change_state,
const rosidl_service_type_support_t * ts_srv_get_state,
const rosidl_service_type_support_t * ts_srv_get_available_states,
const rosidl_service_type_support_t * ts_srv_get_available_transitions);
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
const rcl_allocator_t * allocator);
rcl_ret_t
RCL_WARN_UNUSED

315
rcl_lifecycle/src/default_state_machine.c
View file

@ -20,6 +20,8 @@
#include <lifecycle_msgs/msg/transition.h>
#include "rcl_lifecycle/transition_map.h"
#include "rcl/rcl.h"
#include "rcl/error_handling.h"
#include "default_state_machine.h" // NOLINT
#include "states.h" // NOLINT
@ -150,143 +152,298 @@ const rcl_lifecycle_transition_t rcl_transition_error_error =
// default implementation as despicted on
// design.ros2.org
rcl_ret_t
rcl_lifecycle_init_default_state_machine(rcl_lifecycle_state_machine_t * state_machine)
rcl_lifecycle_init_default_state_machine(
rcl_lifecycle_state_machine_t * state_machine, const rcutils_allocator_t * allocator)
{
// Maybe we can directly store only pointers to states
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_unknown);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_unconfigured);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_inactive);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_active);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_finalized);
// transition states
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_configuring);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_cleaningup);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_shuttingdown);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_activating);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_deactivating);
rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_errorprocessing);
rcutils_ret_t ret;
/*
* we once register all primary states
*/
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_unknown, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_unconfigured, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_inactive, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_active, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_finalized, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
/*
* we once register all transition states
*/
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_configuring, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_cleaningup, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_shuttingdown, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_activating, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_deactivating, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_state(
&state_machine->transition_map, rcl_state_errorprocessing, allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
// add transitions to map
// TRANSITION_CONFIGURE
rcl_lifecycle_register_transition(
/*
* we register all transitions from each primary state
* it registers the transition and the key on which to trigger
*/
// transition configure
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_configure,
lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_configure_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_configure_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_configure_error,
RCL_LIFECYCLE_RET_ERROR);
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
rcl_lifecycle_register_transition(
// transition cleanup
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_cleanup,
lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_cleanup_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_cleanup_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_cleanup_error,
RCL_LIFECYCLE_RET_ERROR);
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
rcl_lifecycle_register_transition(
// transition activate
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_activate,
lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_activate_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_activate_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_activate_error,
RCL_LIFECYCLE_RET_ERROR);
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
rcl_lifecycle_register_transition(
// transition deactivate
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_deactivate,
lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_deactivate_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_deactivate_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_deactivate_error,
RCL_LIFECYCLE_RET_ERROR);
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
rcl_lifecycle_register_transition(
// transition shutdown
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_unconfigured_shutdown,
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_inactive_shutdown,
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_active_shutdown,
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
rcl_lifecycle_register_transition(
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_shutdown_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_shutdown_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_shutdown_error,
RCL_LIFECYCLE_RET_ERROR);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
// error state
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_error_success,
RCL_LIFECYCLE_RET_OK);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_OK,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_error_failure,
RCL_LIFECYCLE_RET_FAILURE);
rcl_lifecycle_register_transition(
RCL_LIFECYCLE_RET_FAILURE,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
ret = rcl_lifecycle_register_transition(
&state_machine->transition_map,
rcl_transition_error_error,
RCL_LIFECYCLE_RET_ERROR);
RCL_LIFECYCLE_RET_ERROR,
allocator);
if (ret != RCL_RET_OK) {
goto fail;
}
state_machine->current_state = &rcl_state_unconfigured;
return RCL_RET_OK;
fail:
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",
rcl_get_default_allocator());
}
return RCL_RET_ERROR;
}
#if __cplusplus

3
rcl_lifecycle/src/default_state_machine.h
View file

@ -29,7 +29,8 @@ extern "C"
RCL_LIFECYCLE_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_lifecycle_init_default_state_machine(rcl_lifecycle_state_machine_t * state_machine);
rcl_lifecycle_init_default_state_machine(
rcl_lifecycle_state_machine_t * state_machine, const rcl_allocator_t * allocator);
#if __cplusplus
}

83
rcl_lifecycle/src/rcl_lifecycle.c
View file

@ -36,15 +36,8 @@ rcl_lifecycle_state_machine_t
rcl_lifecycle_get_zero_initialized_state_machine()
{
rcl_lifecycle_state_machine_t state_machine;
state_machine.transition_map.states = NULL;
state_machine.transition_map.states_size = 0;
state_machine.transition_map.transitions = NULL;
state_machine.transition_map.transitions_size = 0;
state_machine.transition_map = rcl_lifecycle_get_zero_initialized_transition_map();
state_machine.com_interface = rcl_lifecycle_get_zero_initialized_com_interface();
state_machine.allocator.allocate = NULL;
state_machine.allocator.deallocate = NULL;
state_machine.allocator.reallocate = NULL;
state_machine.allocator.state = NULL;
return state_machine;
}
@ -57,56 +50,68 @@ rcl_lifecycle_state_machine_init(
const rosidl_service_type_support_t * ts_srv_get_state,
const rosidl_service_type_support_t * ts_srv_get_available_states,
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
bool default_states)
bool default_states,
const rcl_allocator_t * allocator)
{
if (rcl_lifecycle_com_interface_init(
if (!allocator) {
RCL_SET_ERROR_MSG("can't initialize state machine, no allocator given\n",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
rcl_ret_t ret = rcl_lifecycle_com_interface_init(
&state_machine->com_interface, node_handle,
ts_pub_notify,
ts_srv_change_state, ts_srv_get_state,
ts_srv_get_available_states, ts_srv_get_available_transitions) != RCL_RET_OK)
{
ts_srv_get_available_states, ts_srv_get_available_transitions,
allocator);
if (ret != RCL_RET_OK) {
return RCL_RET_ERROR;
}
if (default_states) {
rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(state_machine);
rcl_ret_t ret =
rcl_lifecycle_init_default_state_machine(state_machine, allocator);
if (ret != RCL_RET_OK) {
// error should already be set
return ret;
// 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;
}
}
}
const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
if (!node_options) {
RCL_SET_ERROR_MSG("node does not have valid options", rcl_get_default_allocator());
return RCL_RET_NODE_INVALID;
}
state_machine->allocator = node_options->allocator;
return RCL_RET_OK;
}
rcl_ret_t
rcl_lifecycle_state_machine_fini(
rcl_lifecycle_state_machine_t * state_machine,
rcl_node_t * node_handle)
rcl_node_t * node_handle,
const rcl_allocator_t * allocator)
{
if (!allocator) {
RCL_SET_ERROR_MSG("can't free state machine, no allocator given\n",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
rcl_ret_t fcn_ret = RCL_RET_OK;
fcn_ret = rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle);
rcl_lifecycle_transition_map_t * transition_map = &state_machine->transition_map;
// for each state free the allocations for their keys/transitions
for (unsigned int i = 0; i < transition_map->states_size; ++i) {
free(transition_map->states[i].valid_transition_keys);
free(transition_map->states[i].valid_transitions);
if (rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle) != RCL_RET_OK) {
RCL_SET_ERROR_MSG(
"could not free lifecycle com interface. Leaking memory!\n", rcl_get_default_allocator());
fcn_ret = RCL_RET_ERROR;
}
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", rcl_get_default_allocator());
fcn_ret = RCL_RET_ERROR;
}
// free the primary states
free(transition_map->states);
transition_map->states = NULL;
// free the tansitions
free(transition_map->transitions);
transition_map->transitions = NULL;
return fcn_ret;
}
@ -122,6 +127,10 @@ rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t *
RCL_SET_ERROR_MSG("change_state service is null", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
if (rcl_lifecycle_transition_map_is_initialized(&state_machine->transition_map) != RCL_RET_OK) {
RCL_SET_ERROR_MSG("transition map is null", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
return RCL_RET_OK;
}
@ -139,7 +148,7 @@ rcl_lifecycle_is_valid_transition(
return &current_state->valid_transitions[i];
}
}
fprintf(stderr, "%s:%u, No callback transition matching %u found for current state %s\n",
fprintf(stderr, "%s:%u, No callback transition matching %d found for current state %s\n",
__FILE__, __LINE__, key, state_machine->current_state->label);
return NULL;
}

143
rcl_lifecycle/src/transition_map.c
View file

@ -22,57 +22,154 @@ extern "C"
#include <string.h>
#include "rcl_lifecycle/transition_map.h"
#include "rcl/error_handling.h"
#include "rcutils/format_string.h"
void
rcl_lifecycle_transition_map_t
rcl_lifecycle_get_zero_initialized_transition_map()
{
static rcl_lifecycle_transition_map_t transition_map;
transition_map.states = NULL;
transition_map.states_size = 0;
transition_map.transitions = NULL;
transition_map.transitions_size = 0;
return transition_map;
}
rcl_ret_t
rcl_lifecycle_transition_map_is_initialized(const rcl_lifecycle_transition_map_t * transition_map)
{
rcl_ret_t is_initialized = RCL_RET_OK;
if (!transition_map->states && !transition_map->transitions) {
is_initialized = RCL_RET_ERROR;
}
return is_initialized;
}
rcl_ret_t
rcl_lifecycle_transition_map_fini(
rcl_lifecycle_transition_map_t * transition_map,
const rcutils_allocator_t * allocator)
{
rcl_ret_t fcn_ret = RCL_RET_OK;
// for each state free the allocations for their keys/transitions
for (unsigned int i = 0; i < transition_map->states_size; ++i) {
allocator->deallocate(transition_map->states[i].valid_transition_keys, allocator->state);
allocator->deallocate(transition_map->states[i].valid_transitions, allocator->state);
}
// free the primary states
allocator->deallocate(transition_map->states, allocator->state);
transition_map->states = NULL;
// free the tansitions
allocator->deallocate(transition_map->transitions, allocator->state);
transition_map->transitions = NULL;
return fcn_ret;
}
rcl_ret_t
rcl_lifecycle_register_state(
rcl_lifecycle_transition_map_t * transition_map,
rcl_lifecycle_state_t state)
rcl_lifecycle_state_t state,
const rcutils_allocator_t * allocator)
{
if (rcl_lifecycle_get_state(transition_map, state.id) != NULL) {
// primary state is already registered
fprintf(stderr, "%s:%u, State %u is already registered\n",
__FILE__, __LINE__, state.id);
return;
char * error_msg = rcutils_format_string(rcutils_get_default_allocator(),
"state %u is already registered\n", state.id);
RCL_SET_ERROR_MSG(error_msg, rcutils_get_default_allocator());
return RCL_RET_ERROR;
}
RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT)
// add new primary state memory
// TODO(karsten1987): Add function for reallocf (see rcl)
++transition_map->states_size;
transition_map->states = realloc(
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));
transition_map->states_size * sizeof(rcl_lifecycle_state_t),
allocator->state);
if (!new_states) {
RCL_SET_ERROR_MSG(
"failed to reallocate memory for new states", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
transition_map->states = new_states;
transition_map->states[transition_map->states_size - 1] = state;
return RCL_RET_OK;
}
void
rcl_ret_t
rcl_lifecycle_register_transition(
rcl_lifecycle_transition_map_t * transition_map,
rcl_lifecycle_transition_t transition,
rcl_lifecycle_ret_t key)
rcl_lifecycle_ret_t key,
const rcutils_allocator_t * allocator)
{
RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
allocator, "invalid allocator", return RCL_RET_ERROR)
rcl_lifecycle_state_t * state = rcl_lifecycle_get_state(transition_map, transition.start->id);
if (!state) {
char * error_msg = rcutils_format_string(rcl_get_default_allocator(),
"state %u is not registered\n", transition.start->id);
RCL_SET_ERROR_MSG(error_msg, rcl_get_default_allocator());
return RCL_RET_ERROR;
}
// we add a new transition, so increase the size
++transition_map->transitions_size;
transition_map->transitions = realloc(
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));
transition_map->transitions_size * sizeof(rcl_lifecycle_transition_t),
allocator->state);
if (!new_transitions) {
RCL_SET_ERROR_MSG(
"failed to reallocate memory for new transitions",
rcl_get_default_allocator());
return RCL_RET_BAD_ALLOC;
}
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;
// connect transition to state key
rcl_lifecycle_state_t * state = rcl_lifecycle_get_state(transition_map, transition.start->id);
++state->valid_transition_size;
state->valid_transitions = realloc(
// 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;
rcl_lifecycle_transition_t * new_valid_transitions = allocator->reallocate(
state->valid_transitions,
state->valid_transition_size * sizeof(rcl_lifecycle_transition_t));
state->valid_transition_keys = realloc(
state->valid_transition_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",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
state->valid_transitions = new_valid_transitions;
rcl_lifecycle_ret_t * new_valid_transition_keys = allocator->reallocate(
state->valid_transition_keys,
state->valid_transition_size * sizeof(rcl_lifecycle_ret_t));
state->valid_transition_size * sizeof(rcl_lifecycle_ret_t),
allocator->state);
if (!new_valid_transitions) {
RCL_SET_ERROR_MSG(
"failed to reallocate memory for new transitions keys on state",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
state->valid_transition_keys = new_valid_transition_keys;
// assign key
state->valid_transition_keys[state->valid_transition_size - 1] = key;
state->valid_transitions[state->valid_transition_size - 1] = transition;
return RCL_RET_OK;
}
rcl_lifecycle_state_t *

48
rcl_lifecycle/test/test_default_state_machine.cpp
View file

@ -33,6 +33,8 @@ class TestDefaultStateMachine : public ::testing::Test
{
protected:
rcl_node_t * node_ptr;
const rcl_allocator_t * allocator;
void SetUp()
{
rcl_ret_t ret;
@ -44,6 +46,8 @@ protected:
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->node_ptr, name, "", &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
const rcl_node_options_t * node_ops = rcl_node_get_options(this->node_ptr);
this->allocator = &node_ops->allocator;
}
void TearDown()
@ -94,13 +98,16 @@ TEST_F(TestDefaultStateMachine, zero_init) {
ASSERT_EQ(transition_map->states, nullptr);
ASSERT_EQ(transition_map->transitions_size, (unsigned int)0);
ASSERT_EQ(transition_map->transitions, nullptr);
auto ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
TEST_F(TestDefaultStateMachine, default_sequence) {
rcl_ret_t ret;
rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(
@ -162,13 +169,16 @@ TEST_F(TestDefaultStateMachine, default_sequence) {
RCL_LIFECYCLE_RET_OK,
lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN,
lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED);
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
rcl_ret_t ret;
rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
{ // supposed to stay unconfigured for all invalid
@ -178,6 +188,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
EXPECT_EQ(
RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(
state_machine.current_state->id,
lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED);
@ -197,6 +208,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == RCL_LIFECYCLE_RET_ERROR) {continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING);
}
@ -217,6 +229,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
fprintf(stderr, "applying key %u\n", *it);
EXPECT_EQ(
RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE);
}
@ -235,6 +248,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == RCL_LIFECYCLE_RET_ERROR) {continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING);
}
@ -253,6 +267,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
{continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE);
}
@ -271,6 +286,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == RCL_LIFECYCLE_RET_ERROR) {continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING);
}
@ -295,6 +311,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == RCL_LIFECYCLE_RET_ERROR) {continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP);
}
@ -319,6 +336,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == RCL_LIFECYCLE_RET_ERROR) {continue;}
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN);
}
@ -333,17 +351,21 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
for (auto it = keys.begin(); it != keys.end(); ++it) {
EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error();
EXPECT_EQ(state_machine.current_state->id,
lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED);
}
}
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
TEST_F(TestDefaultStateMachine, default_in_a_loop) {
rcl_ret_t ret;
rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
for (auto i = 0; i < 5; ++i) {
@ -407,13 +429,16 @@ TEST_F(TestDefaultStateMachine, default_in_a_loop) {
RCL_LIFECYCLE_RET_OK,
lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN,
lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED);
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
TEST_F(TestDefaultStateMachine, default_sequence_failure) {
rcl_ret_t ret;
rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(
@ -520,13 +545,16 @@ TEST_F(TestDefaultStateMachine, default_sequence_failure) {
RCL_LIFECYCLE_RET_FAILURE,
lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN,
lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED);
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
TEST_F(TestDefaultStateMachine, default_sequence_error_resolved) {
rcl_ret_t ret;
rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(
@ -664,6 +692,9 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_resolved) {
RCL_LIFECYCLE_RET_OK,
lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING,
lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED);
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) {
@ -672,7 +703,7 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) {
{
rcl_lifecycle_state_machine_t state_machine =
rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(
@ -692,12 +723,15 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) {
RCL_LIFECYCLE_RET_FAILURE,
lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING,
lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED);
EXPECT_EQ(RCL_RET_OK,
rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator));
}
{
rcl_lifecycle_state_machine_t state_machine =
rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine);
ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(

16
rcl_lifecycle/test/test_multiple_instances.cpp
View file

@ -33,6 +33,7 @@ class TestMultipleInstances : public ::testing::Test
{
protected:
rcl_node_t * node_ptr;
const rcl_allocator_t * allocator;
void SetUp()
{
rcl_ret_t ret;
@ -44,6 +45,8 @@ protected:
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->node_ptr, name, "", &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
const rcl_node_options_t * node_ops = rcl_node_get_options(this->node_ptr);
this->allocator = &node_ops->allocator;
}
void TearDown()
@ -77,17 +80,17 @@ TEST_F(TestMultipleInstances, default_sequence_error_unresolved) {
rcl_lifecycle_state_machine_t state_machine1 =
rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine1);
ret = rcl_lifecycle_init_default_state_machine(&state_machine1, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_lifecycle_state_machine_t state_machine2 =
rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine2);
ret = rcl_lifecycle_init_default_state_machine(&state_machine2, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_lifecycle_state_machine_t state_machine3 =
rcl_lifecycle_get_zero_initialized_state_machine();
ret = rcl_lifecycle_init_default_state_machine(&state_machine3);
ret = rcl_lifecycle_init_default_state_machine(&state_machine3, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
test_trigger_transition(
@ -103,4 +106,11 @@ TEST_F(TestMultipleInstances, default_sequence_error_unresolved) {
lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, state_machine2.current_state->id);
EXPECT_EQ(
lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, state_machine3.current_state->id);
ret = rcl_lifecycle_state_machine_fini(&state_machine1, this->node_ptr, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_lifecycle_state_machine_fini(&state_machine2, this->node_ptr, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_lifecycle_state_machine_fini(&state_machine3, this->node_ptr, this->allocator);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}

75
rcl_lifecycle/test/test_transition_map.cpp Normal file
View file

@ -0,0 +1,75 @@
// Copyright 2016 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 <gtest/gtest.h>
#include "rcl/error_handling.h"
#include "rcl_lifecycle/transition_map.h"
class TestTransitionMap : public ::testing::Test
{
protected:
void SetUp()
{
}
void TearDown()
{
}
};
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));
rcl_allocator_t allocator = rcl_get_default_allocator();
EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator));
}
TEST_F(TestTransitionMap, initialized) {
rcl_lifecycle_transition_map_t transition_map =
rcl_lifecycle_get_zero_initialized_transition_map();
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_lifecycle_state_t state0 = {"my_state", 0, NULL, NULL, 0};
rcl_ret_t ret = rcl_lifecycle_register_state(&transition_map, state0, &allocator);
EXPECT_EQ(RCL_RET_OK, ret);
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_safe();
rcl_lifecycle_state_t state1 = {"my_state", 1, NULL, NULL, 0};
ret = rcl_lifecycle_register_state(&transition_map, state1, &allocator);
const rcl_lifecycle_state_t * start_state =
rcl_lifecycle_get_state(&transition_map, state0.id);
const rcl_lifecycle_state_t * goal_state =
rcl_lifecycle_get_state(&transition_map, state1.id);
EXPECT_EQ(0u, start_state->id);
EXPECT_EQ(1u, goal_state->id);
rcl_lifecycle_transition_t transition01 = {"from0to1", 0, start_state, goal_state};
ret = rcl_lifecycle_register_transition(
&transition_map, transition01, 0, &allocator);
EXPECT_EQ(RCL_RET_OK, ret);
EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator));
}