diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index 776298dd2..033203385 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/data_types.h +++ b/rcl_lifecycle/include/rcl_lifecycle/data_types.h @@ -86,6 +86,17 @@ typedef struct rcl_lifecycle_com_interface_t rcl_service_t srv_get_transition_graph; } rcl_lifecycle_com_interface_t; +/// It contains various options to configure the rcl_lifecycle_state_machine_t instance +typedef struct rcl_lifecycle_state_machine_options_t +{ + /// Flag indicating whether the state machine shall be initialized with default states + bool initialize_default_states; + /// Flag indicating whether the com interface shall be used or not + bool enable_com_interface; + /// Allocator used for allocating states and transitions + rcl_allocator_t allocator; +} rcl_lifecycle_state_machine_options_t; + /// It contains the state machine data typedef struct rcl_lifecycle_state_machine_t { @@ -95,6 +106,8 @@ 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; + /// Options struct with which the state machine was initialized + rcl_lifecycle_state_machine_options_t options; } rcl_lifecycle_state_machine_t; #ifdef __cplusplus diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h index 3a0aeb638..9ffde3641 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -194,6 +194,11 @@ rcl_lifecycle_transition_fini( rcl_lifecycle_transition_t * transition, const rcl_allocator_t * allocator); +/// Return a default initialized state machine options struct. +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_state_machine_options_t +rcl_lifecycle_get_default_state_machine_options(); + /// Return a rcl_lifecycle_state_machine_t struct with members set to `NULL` or 0. /** * Should be called to get a null rcl_lifecycle_state_machine_t before passing to @@ -228,9 +233,7 @@ rcl_lifecycle_get_zero_initialized_state_machine(); * available transitions * \param[in] ts_srv_get_transition_graph pointer to the service that allows to get transitions from * the graph - * \param[in] default_states if `true` a new default state machine is initialized, otherwise - * the state_machine pointer is only used to initialize the interfaces - * \param[in] allocator a valid allocator used to initialized the state machine + * \param[in] state_machine_options collection of config options for initializing the state machine * \return `RCL_RET_OK` if the state machine was initialized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if input params is NULL, or * \return `RCL_RET_ERROR` if an unspecified error occurs. @@ -247,8 +250,7 @@ rcl_lifecycle_state_machine_init( 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_transition_graph, - bool default_states, - const rcl_allocator_t * allocator); + const rcl_lifecycle_state_machine_options_t * state_machine_options); /// Finalize a rcl_lifecycle_state_machine_t. /** @@ -266,7 +268,6 @@ rcl_lifecycle_state_machine_init( * * \param[inout] state_machine struct to be finalized * \param[in] node_handle valid (not finalized) handle to the node - * \param[in] allocator a valid allocator used to finalize the state machine * \return `RCL_RET_OK` if the state was finalized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. @@ -276,8 +277,7 @@ RCL_WARN_UNUSED rcl_ret_t rcl_lifecycle_state_machine_fini( rcl_lifecycle_state_machine_t * state_machine, - rcl_node_t * node_handle, - const rcl_allocator_t * allocator); + rcl_node_t * node_handle); /// Check if a state machine is active. /** diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index d9c3d0fe0..b1704b6d8 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -68,35 +68,104 @@ rcl_lifecycle_com_interface_init( 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_transition_graph) +{ + rcl_ret_t ret = rcl_lifecycle_com_interface_publisher_init( + com_interface, node_handle, ts_pub_notify); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_lifecycle_com_interface_services_init( + com_interface, + node_handle, + ts_srv_change_state, + ts_srv_get_state, + ts_srv_get_available_states, + ts_srv_get_available_transitions, + ts_srv_get_transition_graph); + + if (RCL_RET_OK != ret) { + // cleanup the publisher, which was correctly initialized + rcl_ret_t ret_fini = rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + // warning is already set, no need to log anything here + (void) ret_fini; + } + + return ret; +} + +rcl_ret_t +rcl_lifecycle_com_interface_publisher_init( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle, + const rosidl_message_type_support_t * ts_pub_notify) { RCL_CHECK_ARGUMENT_FOR_NULL(com_interface, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_handle, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_pub_notify, RCL_RET_INVALID_ARGUMENT); + + // initialize publisher + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init( + &com_interface->pub_transition_event, node_handle, + ts_pub_notify, pub_transition_event_topic, &publisher_options); + + if (ret != RCL_RET_OK) { + goto fail; + } + + // initialize static message for notification + lifecycle_msgs__msg__TransitionEvent__init(&msg); + + return RCL_RET_OK; + +fail: + // error message is already logged on failure + ret = rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + (void) ret; + return RCL_RET_ERROR; +} + +rcl_ret_t +rcl_lifecycle_com_interface_publisher_fini( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle) +{ + lifecycle_msgs__msg__TransitionEvent__fini(&msg); + + rcl_ret_t ret = rcl_publisher_fini( + &com_interface->pub_transition_event, node_handle); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy transition_event publisher"); + } + + return ret; +} + +rcl_ret_t +rcl_lifecycle_com_interface_services_init( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle, + 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_transition_graph) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(com_interface, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_handle, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_change_state, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_state, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_available_states, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_available_transitions, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_transition_graph, RCL_RET_INVALID_ARGUMENT); - // initialize publisher - { - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - rcl_ret_t ret = rcl_publisher_init( - &com_interface->pub_transition_event, node_handle, - ts_pub_notify, pub_transition_event_topic, &publisher_options); - - if (ret != RCL_RET_OK) { - goto fail; - } - - // initialize static message for notification - lifecycle_msgs__msg__TransitionEvent__init(&msg); - } + rcl_ret_t ret = RCL_RET_OK; // initialize change state service { rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init( + ret = rcl_service_init( &com_interface->srv_change_state, node_handle, ts_srv_change_state, srv_change_state_service, &service_options); @@ -108,7 +177,7 @@ rcl_lifecycle_com_interface_init( // initialize get state service { rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init( + ret = rcl_service_init( &com_interface->srv_get_state, node_handle, ts_srv_get_state, srv_get_state_service, &service_options); @@ -120,7 +189,7 @@ rcl_lifecycle_com_interface_init( // initialize get available states service { rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init( + ret = rcl_service_init( &com_interface->srv_get_available_states, node_handle, ts_srv_get_available_states, srv_get_available_states_service, &service_options); @@ -132,7 +201,7 @@ rcl_lifecycle_com_interface_init( // initialize get available transitions service { rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init( + ret = rcl_service_init( &com_interface->srv_get_available_transitions, node_handle, ts_srv_get_available_transitions, srv_get_available_transitions_service, &service_options); @@ -144,7 +213,7 @@ rcl_lifecycle_com_interface_init( // initialize get transition graph service { rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init( + ret = rcl_service_init( &com_interface->srv_get_transition_graph, node_handle, ts_srv_get_transition_graph, srv_get_transition_graph, &service_options); @@ -155,32 +224,14 @@ rcl_lifecycle_com_interface_init( return RCL_RET_OK; fail: - if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy transition_event publisher"); - } - if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_change_state, node_handle)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy change_state service"); - } - if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_state, node_handle)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_state service"); - } - if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_states, node_handle)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_available_states service"); - } - if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_transitions, node_handle)) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service"); - } - if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_transition_graph, node_handle)) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Failed to destroy get_transition_graph service"); - } - + // error messages already logged on failure + ret = rcl_lifecycle_com_interface_services_fini(com_interface, node_handle); + (void) ret; return RCL_RET_ERROR; } rcl_ret_t -rcl_lifecycle_com_interface_fini( +rcl_lifecycle_com_interface_services_fini( rcl_lifecycle_com_interface_t * com_interface, rcl_node_t * node_handle) { @@ -191,6 +242,8 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t ret = rcl_service_fini( &com_interface->srv_get_transition_graph, node_handle); if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to destroy get_transition_graph service"); fcn_ret = RCL_RET_ERROR; } } @@ -200,6 +253,8 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t ret = rcl_service_fini( &com_interface->srv_get_available_transitions, node_handle); if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service"); fcn_ret = RCL_RET_ERROR; } } @@ -209,6 +264,7 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t ret = rcl_service_fini( &com_interface->srv_get_available_states, node_handle); if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_available_states service"); fcn_ret = RCL_RET_ERROR; } } @@ -218,6 +274,7 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t ret = rcl_service_fini( &com_interface->srv_get_state, node_handle); if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_state service"); fcn_ret = RCL_RET_ERROR; } } @@ -227,17 +284,35 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t ret = rcl_service_fini( &com_interface->srv_change_state, node_handle); if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy change_state service"); fcn_ret = RCL_RET_ERROR; } } - // destroy the publisher + return fcn_ret; +} + +rcl_ret_t +rcl_lifecycle_com_interface_fini( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle) +{ + rcl_ret_t fcn_ret = RCL_RET_OK; + + // destroy the services { - lifecycle_msgs__msg__TransitionEvent__fini(&msg); + rcl_ret_t ret = rcl_lifecycle_com_interface_services_fini( + com_interface, node_handle); + if (RCL_RET_OK != ret) { + fcn_ret = RCL_RET_ERROR; + } + } - rcl_ret_t ret = rcl_publisher_fini( - &com_interface->pub_transition_event, node_handle); - if (ret != RCL_RET_OK) { + // destroy the event publisher + { + rcl_ret_t ret = rcl_lifecycle_com_interface_publisher_fini( + com_interface, node_handle); + if (RCL_RET_OK != ret) { fcn_ret = RCL_RET_ERROR; } } diff --git a/rcl_lifecycle/src/com_interface.h b/rcl_lifecycle/src/com_interface.h index d59bb752a..2fa0c146a 100644 --- a/rcl_lifecycle/src/com_interface.h +++ b/rcl_lifecycle/src/com_interface.h @@ -39,6 +39,36 @@ rcl_lifecycle_com_interface_init( const rosidl_service_type_support_t * ts_srv_get_available_transitions, const rosidl_service_type_support_t * ts_srv_get_transition_graph); +rcl_ret_t +RCL_WARN_UNUSED +rcl_lifecycle_com_interface_publisher_init( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle, + const rosidl_message_type_support_t * ts_pub_notify); + +rcl_ret_t +RCL_WARN_UNUSED +rcl_lifecycle_com_interface_publisher_fini( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle); + +rcl_ret_t +RCL_WARN_UNUSED +rcl_lifecycle_com_interface_services_init( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle, + 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_transition_graph); + +rcl_ret_t +RCL_WARN_UNUSED +rcl_lifecycle_com_interface_services_fini( + rcl_lifecycle_com_interface_t * com_interface, + rcl_node_t * node_handle); + rcl_ret_t RCL_WARN_UNUSED rcl_lifecycle_com_interface_fini( diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index f4dc6f912..7a3afb1df 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -167,6 +167,17 @@ rcl_lifecycle_transition_fini( return ret; } +rcl_lifecycle_state_machine_options_t +rcl_lifecycle_get_default_state_machine_options() +{ + rcl_lifecycle_state_machine_options_t options; + options.enable_com_interface = true; + options.initialize_default_states = true; + options.allocator = rcl_get_default_allocator(); + + return options; +} + // get zero initialized state machine here rcl_lifecycle_state_machine_t rcl_lifecycle_get_zero_initialized_state_machine() @@ -175,6 +186,7 @@ rcl_lifecycle_get_zero_initialized_state_machine() state_machine.current_state = NULL; state_machine.transition_map = rcl_lifecycle_get_zero_initialized_transition_map(); state_machine.com_interface = rcl_lifecycle_get_zero_initialized_com_interface(); + state_machine.options = rcl_lifecycle_get_default_state_machine_options(); return state_machine; } @@ -188,8 +200,7 @@ rcl_lifecycle_state_machine_init( 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_transition_graph, - bool default_states, - const rcl_allocator_t * allocator) + const rcl_lifecycle_state_machine_options_t * state_machine_options) { RCL_CHECK_FOR_NULL_WITH_MSG( state_machine, "State machine is null\n", return RCL_RET_INVALID_ARGUMENT); @@ -198,24 +209,36 @@ rcl_lifecycle_state_machine_init( node_handle, "Node handle is null\n", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ALLOCATOR_WITH_MSG( - allocator, "can't initialize state machine, no allocator given\n", + &state_machine_options->allocator, "can't initialize state machine, no allocator given\n", return RCL_RET_INVALID_ARGUMENT); - 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, ts_srv_get_transition_graph); - if (ret != RCL_RET_OK) { - return RCL_RET_ERROR; + state_machine->options = *state_machine_options; + + // enable full com_interface with pub & srvs + if (state_machine->options.enable_com_interface) { + 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, ts_srv_get_transition_graph); + if (ret != RCL_RET_OK) { + return RCL_RET_ERROR; + } + } else { + rcl_ret_t ret = rcl_lifecycle_com_interface_publisher_init( + &state_machine->com_interface, node_handle, ts_pub_notify); + if (ret != RCL_RET_OK) { + return RCL_RET_ERROR; + } } - if (default_states) { - ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator); + if (state_machine->options.initialize_default_states) { + rcl_ret_t ret = rcl_lifecycle_init_default_state_machine( + state_machine, &state_machine->options.allocator); if (ret != RCL_RET_OK) { // init default state machine might have allocated memory, // so we have to call fini - ret = rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator); + ret = rcl_lifecycle_state_machine_fini(state_machine, node_handle); if (ret != RCL_RET_OK) { RCUTILS_SAFE_FWRITE_TO_STDERR( "Freeing state machine failed while handling a previous error. Leaking memory!\n"); @@ -234,12 +257,8 @@ rcl_lifecycle_state_machine_init( rcl_ret_t rcl_lifecycle_state_machine_fini( rcl_lifecycle_state_machine_t * state_machine, - rcl_node_t * node_handle, - const rcl_allocator_t * allocator) + rcl_node_t * node_handle) { - RCL_CHECK_ALLOCATOR_WITH_MSG( - allocator, "can't free state machine, no allocator given\n", return RCL_RET_INVALID_ARGUMENT); - rcl_ret_t fcn_ret = RCL_RET_OK; if (rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle) != RCL_RET_OK) { @@ -251,7 +270,7 @@ rcl_lifecycle_state_machine_fini( } if (rcl_lifecycle_transition_map_fini( - &state_machine->transition_map, allocator) != RCL_RET_OK) + &state_machine->transition_map, &state_machine->options.allocator) != RCL_RET_OK) { rcl_error_string_t error_string = rcl_get_error_string(); rcutils_reset_error(); @@ -266,18 +285,21 @@ rcl_lifecycle_state_machine_fini( rcl_ret_t rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t * state_machine) { - RCL_CHECK_FOR_NULL_WITH_MSG( - state_machine->com_interface.srv_get_state.impl, "get_state service is null\n", - return RCL_RET_INVALID_ARGUMENT); - - RCL_CHECK_FOR_NULL_WITH_MSG( - state_machine->com_interface.srv_change_state.impl, "change_state service is null\n", - return RCL_RET_INVALID_ARGUMENT); + if (state_machine->options.enable_com_interface) { + RCL_CHECK_FOR_NULL_WITH_MSG( + state_machine->com_interface.srv_get_state.impl, "get_state service is null\n", + return RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_FOR_NULL_WITH_MSG( + state_machine->com_interface.srv_change_state.impl, "change_state service is null\n", + return RCL_RET_INVALID_ARGUMENT); + } if (rcl_lifecycle_transition_map_is_initialized(&state_machine->transition_map) != RCL_RET_OK) { RCL_SET_ERROR_MSG("transition map is null"); return RCL_RET_INVALID_ARGUMENT; } + return RCL_RET_OK; } diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp index c8de67b33..58f61b2e6 100644 --- a/rcl_lifecycle/test/test_default_state_machine.cpp +++ b/rcl_lifecycle/test/test_default_state_machine.cpp @@ -39,6 +39,7 @@ class TestDefaultStateMachine : public ::testing::Test rcl_context_t * context_ptr; rcl_node_t * node_ptr; const rcl_allocator_t * allocator; + rcl_lifecycle_state_machine_options_t state_machine_options; void SetUp() { rcl_ret_t ret; @@ -62,7 +63,8 @@ class TestDefaultStateMachine : public ::testing::Test ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; const rcl_node_options_t * node_ops = rcl_node_get_options(this->node_ptr); - this->allocator = &node_ops->allocator; + state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.allocator = node_ops->allocator; } void TearDown() @@ -107,7 +109,7 @@ TEST_F(TestDefaultStateMachine, zero_init) { EXPECT_EQ(transition_map->transitions_size, 0u); EXPECT_EQ(transition_map->transitions, nullptr); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator); + auto ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -120,10 +122,11 @@ TEST_F(TestDefaultStateMachine, default_init) { EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); - ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.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); + ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -131,7 +134,8 @@ 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -196,14 +200,15 @@ TEST_F(TestDefaultStateMachine, default_sequence) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; std::vector transition_ids = @@ -437,14 +442,15 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; for (auto i = 0; i < 5; ++i) { @@ -511,14 +517,15 @@ TEST_F(TestDefaultStateMachine, default_in_a_loop) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -628,14 +635,15 @@ TEST_F(TestDefaultStateMachine, default_sequence_failure) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine(&state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -776,7 +784,7 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_resolved) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) { @@ -785,7 +793,9 @@ 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, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine( + &state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -808,13 +818,15 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } { rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); - ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator); + state_machine.options = this->state_machine_options; + ret = rcl_lifecycle_init_default_state_machine( + &state_machine, &state_machine.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -849,21 +861,22 @@ TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr)); } } TEST_F(TestDefaultStateMachine, init_fini_maybe_fail) { rcl_lifecycle_state_machine_t sm = rcl_lifecycle_get_zero_initialized_state_machine(); + sm.options = this->state_machine_options; RCUTILS_FAULT_INJECTION_TEST( { - rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(&sm, this->allocator); + rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(&sm, &sm.options.allocator); if (RCL_RET_OK == ret) { - ret = rcl_lifecycle_state_machine_fini(&sm, this->node_ptr, this->allocator); + ret = rcl_lifecycle_state_machine_fini(&sm, this->node_ptr); if (RCL_RET_OK != ret) { EXPECT_EQ( RCL_RET_OK, - rcl_lifecycle_state_machine_fini(&sm, this->node_ptr, this->allocator)); + rcl_lifecycle_state_machine_fini(&sm, this->node_ptr)); } } }); diff --git a/rcl_lifecycle/test/test_multiple_instances.cpp b/rcl_lifecycle/test/test_multiple_instances.cpp index 29e42f20c..74f1291c1 100644 --- a/rcl_lifecycle/test/test_multiple_instances.cpp +++ b/rcl_lifecycle/test/test_multiple_instances.cpp @@ -35,7 +35,8 @@ class TestMultipleInstances : public ::testing::Test public: rcl_context_t * context_ptr; rcl_node_t * node_ptr; - const rcl_allocator_t * allocator; + rcl_lifecycle_state_machine_options_t state_machine_options; + void SetUp() { rcl_ret_t ret; @@ -59,7 +60,8 @@ class TestMultipleInstances : public ::testing::Test ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; const rcl_node_options_t * node_ops = rcl_node_get_options(this->node_ptr); - this->allocator = &node_ops->allocator; + state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.allocator = node_ops->allocator; } void TearDown() @@ -96,17 +98,23 @@ 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, this->allocator); + state_machine1.options = this->state_machine_options; + ret = + rcl_lifecycle_init_default_state_machine(&state_machine1, &state_machine1.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_lifecycle_state_machine_t state_machine2 = rcl_lifecycle_get_zero_initialized_state_machine(); - ret = rcl_lifecycle_init_default_state_machine(&state_machine2, this->allocator); + state_machine2.options = this->state_machine_options; + ret = + rcl_lifecycle_init_default_state_machine(&state_machine2, &state_machine2.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_lifecycle_state_machine_t state_machine3 = rcl_lifecycle_get_zero_initialized_state_machine(); - ret = rcl_lifecycle_init_default_state_machine(&state_machine3, this->allocator); + state_machine3.options = this->state_machine_options; + ret = + rcl_lifecycle_init_default_state_machine(&state_machine3, &state_machine3.options.allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_trigger_transition( @@ -123,10 +131,10 @@ TEST_F(TestMultipleInstances, default_sequence_error_unresolved) { 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); + ret = rcl_lifecycle_state_machine_fini(&state_machine1, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_lifecycle_state_machine_fini(&state_machine2, this->node_ptr, this->allocator); + ret = rcl_lifecycle_state_machine_fini(&state_machine2, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_lifecycle_state_machine_fini(&state_machine3, this->node_ptr, this->allocator); + ret = rcl_lifecycle_state_machine_fini(&state_machine3, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp index 5b53c0ab1..3bb1ea554 100644 --- a/rcl_lifecycle/test/test_rcl_lifecycle.cpp +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -223,7 +223,6 @@ TEST(TestRclLifecycle, state_machine) { EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)) << 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 = @@ -237,74 +236,96 @@ TEST(TestRclLifecycle, state_machine) { const rosidl_service_type_support_t * gtg = ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions); + rcl_lifecycle_state_machine_options_t state_machine_options = + rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.initialize_default_states = false; + // Check various arguments are null ret = rcl_lifecycle_state_machine_init( - nullptr, &node, pn, cs, gs, gas, gat, gtg, false, &allocator); + nullptr, &node, pn, cs, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, nullptr, pn, cs, gs, gas, gat, gtg, false, &allocator); + &state_machine, nullptr, pn, cs, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, nullptr, cs, gs, gas, gat, gtg, false, &allocator); + &state_machine, &node, nullptr, cs, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, nullptr, gs, gas, gat, gtg, false, &allocator); + &state_machine, &node, pn, nullptr, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, nullptr, gas, gat, gtg, false, &allocator); + &state_machine, &node, pn, cs, nullptr, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, gs, nullptr, gat, gtg, false, &allocator); + &state_machine, &node, pn, cs, gs, nullptr, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, gs, gas, nullptr, gtg, false, &allocator); + &state_machine, &node, pn, cs, gs, gas, nullptr, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, gs, gas, gat, nullptr, false, &allocator); + &state_machine, &node, pn, cs, gs, gas, gat, nullptr, &state_machine_options); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); rcutils_reset_error(); - ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, nullptr); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); - rcutils_reset_error(); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_lifecycle_state_machine_is_initialized(&state_machine)); - rcutils_reset_error(); + // Com interface not enabled + // The transition event publisher is active + // The external transition services are inactive + state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = false; + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, gtg, &state_machine_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_NE(nullptr, &state_machine.com_interface); + EXPECT_NE(nullptr, &state_machine.com_interface.pub_transition_event.impl); + EXPECT_EQ(nullptr, state_machine.com_interface.srv_change_state.impl); + EXPECT_EQ(nullptr, state_machine.com_interface.srv_get_state.impl); + EXPECT_EQ(nullptr, state_machine.com_interface.srv_get_available_states.impl); + EXPECT_EQ(nullptr, state_machine.com_interface.srv_get_available_transitions.impl); + EXPECT_EQ(nullptr, state_machine.com_interface.srv_get_transition_graph.impl); + EXPECT_EQ( + RCL_RET_OK, + rcl_lifecycle_state_machine_is_initialized(&state_machine)) << rcl_get_error_string().str; + // Reset the state machine as the previous init call was successful + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // Everything should be good + state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.initialize_default_states = false; + ret = rcl_lifecycle_state_machine_init( - &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, &allocator); + &state_machine, &node, pn, cs, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // Transition_map is not initialized @@ -343,18 +364,13 @@ TEST(TestRclLifecycle, state_machine) { state_machine.com_interface.srv_change_state.impl = reinterpret_cast(temp_function); - // allocator is nullptr - ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, nullptr); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); - rcutils_reset_error(); - - ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator); + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node); EXPECT_EQ(RCL_RET_OK, ret) << 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); + ret = rcl_lifecycle_state_machine_fini(&state_machine, nullptr); EXPECT_EQ(RCL_RET_ERROR, ret); rcutils_reset_error(); } @@ -370,6 +386,9 @@ TEST(TestRclLifecycle, state_transitions) { rcl_node_t node = rcl_get_zero_initialized_node(); rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_lifecycle_state_machine_options_t state_machine_options = + rcl_lifecycle_get_default_state_machine_options(); + rcl_context_t context = rcl_get_zero_initialized_context(); rcl_node_options_t options = rcl_node_get_default_options(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -414,7 +433,7 @@ TEST(TestRclLifecycle, state_transitions) { 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); + &state_machine, &node, pn, cs, gs, gas, gat, gtg, &state_machine_options); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_lifecycle_state_machine_is_initialized(&state_machine); @@ -464,7 +483,7 @@ TEST(TestRclLifecycle, state_transitions) { rcl_print_state_machine(&state_machine); EXPECT_FALSE(rcutils_error_is_set()); - ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator); + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -519,12 +538,15 @@ TEST(TestRclLifecycle, init_fini_maybe_fail) { // Init segfaults if this is not zero initialized rcl_lifecycle_state_machine_t sm = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_state_machine_options_t state_machine_options = + rcl_lifecycle_get_default_state_machine_options(); + ret = rcl_lifecycle_state_machine_init( - &sm, &node, pn, cs, gs, gas, gat, gtg, true, &allocator); + &sm, &node, pn, cs, gs, gas, gat, gtg, &state_machine_options); if (RCL_RET_OK == ret) { - ret = rcl_lifecycle_state_machine_fini(&sm, &node, &allocator); + ret = rcl_lifecycle_state_machine_fini(&sm, &node); if (RCL_RET_OK != ret) { - EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_state_machine_fini(&sm, &node, &allocator)); + EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_state_machine_fini(&sm, &node)); } } });