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

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
}

89
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(
&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)
{
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,
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;
}

157
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_register_state(
rcl_lifecycle_transition_map_t * transition_map,
rcl_lifecycle_state_t state)
rcl_lifecycle_transition_map_t
rcl_lifecycle_get_zero_initialized_transition_map()
{
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;
}
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;
// add new primary state memory
// TODO(karsten1987): Add function for reallocf (see rcl)
++transition_map->states_size;
transition_map->states = realloc(
transition_map->states,
transition_map->states_size * sizeof(rcl_lifecycle_state_t));
transition_map->states[transition_map->states_size - 1] = state;
return transition_map;
}
void
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,
const rcutils_allocator_t * allocator)
{
if (rcl_lifecycle_get_state(transition_map, state.id) != NULL) {
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
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),
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;
}
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 *