diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index d4b5498..5843831 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/data_types.h +++ b/rcl_lifecycle/include/rcl_lifecycle/data_types.h @@ -51,8 +51,8 @@ typedef struct rcl_lifecycle_transition_t { const char * label; unsigned int id; - const rcl_lifecycle_state_t * start; - const rcl_lifecycle_state_t * goal; + rcl_lifecycle_state_t * start; + rcl_lifecycle_state_t * goal; } rcl_lifecycle_transition_t; typedef struct rcl_lifecycle_transition_map_t diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h index 34f6fcb..e032d50 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -25,6 +25,55 @@ extern "C" #include "rcl_lifecycle/data_types.h" #include "rcl_lifecycle/visibility_control.h" +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_state_t +rcl_lifecycle_get_zero_initialized_state(); + +RCL_LIFECYCLE_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lifecycle_state_init( + rcl_lifecycle_state_t * state, + unsigned int id, + const char * label, + const rcl_allocator_t * allocator); + +RCL_LIFECYCLE_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lifecycle_state_fini( + rcl_lifecycle_state_t * state, + const rcl_allocator_t * allocator); + +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_transition_t +rcl_lifecycle_get_zero_initialized_transition(); + +/// Initialize transition with existing states +/** + * Note: the transition pointer will take ownership + * of the start and goal state. When calling + * rcl_lifecycle_transition_fini(), the two states + * will be freed. + */ +RCL_LIFECYCLE_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lifecycle_transition_init( + rcl_lifecycle_transition_t * transition, + unsigned int id, + const char * label, + rcl_lifecycle_state_t * start, + rcl_lifecycle_state_t * goal, + const rcl_allocator_t * allocator); + +RCL_LIFECYCLE_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lifecycle_transition_fini( + rcl_lifecycle_transition_t * transition, + const rcl_allocator_t * allocator); + RCL_LIFECYCLE_PUBLIC rcl_lifecycle_state_machine_t rcl_lifecycle_get_zero_initialized_state_machine(); diff --git a/rcl_lifecycle/src/default_state_machine.c b/rcl_lifecycle/src/default_state_machine.c index c426bd5..3933309 100644 --- a/rcl_lifecycle/src/default_state_machine.c +++ b/rcl_lifecycle/src/default_state_machine.c @@ -37,38 +37,38 @@ rcl_lifecycle_transition_t * empty_transition = NULL; unsigned int empty_transition_size = 0; // Primary States -const rcl_lifecycle_state_t rcl_state_unknown = { +rcl_lifecycle_state_t rcl_state_unknown = { "unknown", lifecycle_msgs__msg__State__PRIMARY_STATE_UNKNOWN, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_unconfigured = { +rcl_lifecycle_state_t rcl_state_unconfigured = { "unconfigured", lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_inactive = { +rcl_lifecycle_state_t rcl_state_inactive = { "inactive", lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_active = { +rcl_lifecycle_state_t rcl_state_active = { "active", lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_finalized = { +rcl_lifecycle_state_t rcl_state_finalized = { "finalized", lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED, NULL, NULL, 0 }; // Transition States -const rcl_lifecycle_state_t rcl_state_configuring = { +rcl_lifecycle_state_t rcl_state_configuring = { "configuring", lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_cleaningup = { +rcl_lifecycle_state_t rcl_state_cleaningup = { "cleaningup", lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_shuttingdown = { +rcl_lifecycle_state_t rcl_state_shuttingdown = { "shuttingdown", lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_activating = { +rcl_lifecycle_state_t rcl_state_activating = { "activating", lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_deactivating = { +rcl_lifecycle_state_t rcl_state_deactivating = { "deactivating", lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, NULL, NULL, 0 }; -const rcl_lifecycle_state_t rcl_state_errorprocessing = { +rcl_lifecycle_state_t rcl_state_errorprocessing = { "errorprocessing", lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, NULL, NULL, 0 }; diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 9a8f66d..74eb5c9 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -25,6 +25,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" #include "rcl_lifecycle/rcl_lifecycle.h" #include "rcl_lifecycle/transition_map.h" @@ -33,6 +34,144 @@ extern "C" #include "default_state_machine.h" // NOLINT #include "states.h" // NOLINT +rcl_lifecycle_state_t +rcl_lifecycle_get_zero_initialized_state() +{ + rcl_lifecycle_state_t state; + state.id = 0; + state.label = NULL; + return state; +} + +rcl_ret_t +rcl_lifecycle_state_init( + rcl_lifecycle_state_t * state, + unsigned int id, + const char * label, + const rcl_allocator_t * allocator) +{ + if (!allocator) { + RCL_SET_ERROR_MSG("can't initialize state, no allocator given\n", + rcl_get_default_allocator()); + return RCL_RET_ERROR; + } + if (!state) { + RCL_SET_ERROR_MSG("state pointer is null\n", *allocator); + return RCL_RET_ERROR; + } + + state->id = id; + state->label = rcutils_strndup(label, strlen(label), *allocator); + if (!state->label) { + RCL_SET_ERROR_MSG("failed to duplicate label for rcl_lifecycle_state_t\n", *allocator); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lifecycle_state_fini( + rcl_lifecycle_state_t * state, + const rcl_allocator_t * allocator) +{ + if (!allocator) { + RCL_SET_ERROR_MSG("can't free state, no allocator given\n", + rcl_get_default_allocator()); + return RCL_RET_ERROR; + } + // it is already NULL + if (!state) { + return RCL_RET_OK; + } + + if (state->label) { + allocator->deallocate((char *)state->label, allocator->state); + state->label = NULL; + } + allocator->deallocate(state, allocator->state); + state = NULL; + + return RCL_RET_OK; +} + +rcl_lifecycle_transition_t +rcl_lifecycle_get_zero_initialized_transition() +{ + rcl_lifecycle_transition_t transition; + transition.id = 0; + transition.label = NULL; + transition.start = NULL; + transition.goal = NULL; + return transition; +} + +rcl_ret_t +rcl_lifecycle_transition_init( + rcl_lifecycle_transition_t * transition, + unsigned int id, + const char * label, + rcl_lifecycle_state_t * start, + rcl_lifecycle_state_t * goal, + const rcl_allocator_t * allocator) +{ + if (!allocator) { + RCL_SET_ERROR_MSG("can't initialize transition, no allocator given\n", + rcl_get_default_allocator()); + return RCL_RET_ERROR; + } + + if (!transition) { + RCL_SET_ERROR_MSG("transition pointer is null\n", *allocator); + return RCL_RET_OK; + } + + transition->start = start; + transition->goal = goal; + + transition->id = id; + transition->label = rcutils_strndup(label, strlen(label), *allocator); + if (!transition->label) { + RCL_SET_ERROR_MSG("failed to duplicate label for rcl_lifecycle_transition_t\n", *allocator); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lifecycle_transition_fini( + rcl_lifecycle_transition_t * transition, + const rcl_allocator_t * allocator) +{ + if (!allocator) { + RCL_SET_ERROR_MSG("can't initialize transition, no allocator given\n", + rcl_get_default_allocator()); + return RCL_RET_ERROR; + } + // it is already NULL + if (!transition) { + return RCL_RET_OK; + } + + rcl_ret_t ret = RCL_RET_OK; + + if (rcl_lifecycle_state_fini(transition->start, allocator) != RCL_RET_OK) { + ret = RCL_RET_ERROR; + } + if (rcl_lifecycle_state_fini(transition->goal, allocator) != RCL_RET_OK) { + ret = RCL_RET_ERROR; + } + + allocator->deallocate((char *)transition->label, allocator->state); + transition->label = NULL; + + allocator->deallocate(transition, allocator->state); + transition = NULL; + + return ret; +} + // get zero initialized state machine here rcl_lifecycle_state_machine_t rcl_lifecycle_get_zero_initialized_state_machine() diff --git a/rcl_lifecycle/src/states.h b/rcl_lifecycle/src/states.h index c0b6250..6de3173 100644 --- a/rcl_lifecycle/src/states.h +++ b/rcl_lifecycle/src/states.h @@ -25,18 +25,18 @@ extern "C" // primary states based on // design.ros2.org/articles/node_lifecycle.html -extern const rcl_lifecycle_state_t rcl_state_unknown; -extern const rcl_lifecycle_state_t rcl_state_unconfigured; -extern const rcl_lifecycle_state_t rcl_state_inactive; -extern const rcl_lifecycle_state_t rcl_state_active; -extern const rcl_lifecycle_state_t rcl_state_finalized; +extern rcl_lifecycle_state_t rcl_state_unknown; +extern rcl_lifecycle_state_t rcl_state_unconfigured; +extern rcl_lifecycle_state_t rcl_state_inactive; +extern rcl_lifecycle_state_t rcl_state_active; +extern rcl_lifecycle_state_t rcl_state_finalized; -extern const rcl_lifecycle_state_t rcl_state_configuring; -extern const rcl_lifecycle_state_t rcl_state_cleaningup; -extern const rcl_lifecycle_state_t rcl_state_shuttingdown; -extern const rcl_lifecycle_state_t rcl_state_activating; -extern const rcl_lifecycle_state_t rcl_state_deactivating; -extern const rcl_lifecycle_state_t rcl_state_errorprocessing; +extern rcl_lifecycle_state_t rcl_state_configuring; +extern rcl_lifecycle_state_t rcl_state_cleaningup; +extern rcl_lifecycle_state_t rcl_state_shuttingdown; +extern rcl_lifecycle_state_t rcl_state_activating; +extern rcl_lifecycle_state_t rcl_state_deactivating; +extern rcl_lifecycle_state_t rcl_state_errorprocessing; extern const rcl_lifecycle_transition_t rcl_transition_configure; extern const rcl_lifecycle_transition_t rcl_transition_cleanup; diff --git a/rcl_lifecycle/test/test_transition_map.cpp b/rcl_lifecycle/test/test_transition_map.cpp index bb7efe5..77d69af 100644 --- a/rcl_lifecycle/test/test_transition_map.cpp +++ b/rcl_lifecycle/test/test_transition_map.cpp @@ -61,14 +61,15 @@ TEST_F(TestTransitionMap, initialized) { 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_state_t * start_state = rcl_lifecycle_get_state(&transition_map, state0.id); - const rcl_lifecycle_state_t * goal_state = + 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}; + 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);