From 5e0c838dc8becae9a4898b6486c6115240f34b25 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 6 Jan 2021 13:29:42 -1000 Subject: [PATCH 1/9] make rcl_lifecycle_com_interface optional Signed-off-by: Karsten Knese --- .../include/rcl_lifecycle/rcl_lifecycle.h | 3 ++ rcl_lifecycle/src/rcl_lifecycle.c | 19 +++++----- rcl_lifecycle/test/test_rcl_lifecycle.cpp | 35 ++++++++++++------- 3 files changed, 36 insertions(+), 21 deletions(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h index 3a0aeb638..cd6d3d49f 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -228,6 +228,8 @@ 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] enable_com_interface if `false` the services listed above are not initialized, + * if true all services are available and active * \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 @@ -247,6 +249,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 enable_com_interface, bool default_states, const rcl_allocator_t * allocator); diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index f4dc6f912..d2e6170e9 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -188,6 +188,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 enable_com_interface, bool default_states, const rcl_allocator_t * allocator) { @@ -201,17 +202,19 @@ rcl_lifecycle_state_machine_init( 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; + if (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; + } } if (default_states) { - ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator); + rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator); if (ret != RCL_RET_OK) { // init default state machine might have allocated memory, // so we have to call fini diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp index 5b53c0ab1..bc43df280 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 = @@ -239,72 +238,82 @@ TEST(TestRclLifecycle, state_machine) { // 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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, true, false, &allocator); 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); + &state_machine, &node, pn, cs, gs, gas, gat, gtg, true, 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 + ret = rcl_lifecycle_state_machine_init( + &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, false, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_NE(nullptr, &state_machine.com_interface); + 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); // Everything should be good 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, true, false, &allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // Transition_map is not initialized @@ -414,7 +423,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, true, true, &allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_lifecycle_state_machine_is_initialized(&state_machine); @@ -520,7 +529,7 @@ TEST(TestRclLifecycle, init_fini_maybe_fail) { rcl_lifecycle_state_machine_t sm = rcl_lifecycle_get_zero_initialized_state_machine(); ret = rcl_lifecycle_state_machine_init( - &sm, &node, pn, cs, gs, gas, gat, gtg, true, &allocator); + &sm, &node, pn, cs, gs, gas, gat, gtg, true, true, &allocator); if (RCL_RET_OK == ret) { ret = rcl_lifecycle_state_machine_fini(&sm, &node, &allocator); if (RCL_RET_OK != ret) { From 703a112cccaf32f72f70cf83d48fed70c1668db1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 7 Jan 2021 09:43:25 -1000 Subject: [PATCH 2/9] only disable services, not transition event publisher Signed-off-by: Karsten Knese --- rcl_lifecycle/src/com_interface.c | 156 ++++++++++++++++------ rcl_lifecycle/src/com_interface.h | 30 +++++ rcl_lifecycle/src/rcl_lifecycle.c | 7 + rcl_lifecycle/test/test_rcl_lifecycle.cpp | 6 + 4 files changed, 157 insertions(+), 42 deletions(-) diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index d9c3d0fe0..e58ea7e84 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -68,31 +68,98 @@ 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 + (void) rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + } + + 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); - 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); + 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; - } + if (ret != RCL_RET_OK) { + goto fail; + } - // initialize static message for notification - lifecycle_msgs__msg__TransitionEvent__init(&msg); + // initialize static message for notification + lifecycle_msgs__msg__TransitionEvent__init(&msg); + + return RCL_RET_OK; + +fail: + // error message is already logged on failure + (void) rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + 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) +{ + rcl_ret_t fcn_ret = RCL_RET_OK; + + 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"); + fcn_ret = RCL_RET_ERROR; } + return fcn_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 change state service { rcl_service_options_t service_options = rcl_service_get_default_options(); @@ -155,32 +222,13 @@ 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 + (void) rcl_lifecycle_com_interface_services_fini(com_interface, node_handle); 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 +239,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 +250,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 +261,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 +271,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 +281,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 d2e6170e9..a2a96041c 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -202,6 +202,7 @@ rcl_lifecycle_state_machine_init( allocator, "can't initialize state machine, no allocator given\n", return RCL_RET_INVALID_ARGUMENT); + // enable full com_interface with pub & srvs if (enable_com_interface) { rcl_ret_t ret = rcl_lifecycle_com_interface_init( &state_machine->com_interface, node_handle, @@ -211,6 +212,12 @@ rcl_lifecycle_state_machine_init( 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) { diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp index bc43df280..a9fc399aa 100644 --- a/rcl_lifecycle/test/test_rcl_lifecycle.cpp +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -301,15 +301,21 @@ TEST(TestRclLifecycle, state_machine) { rcutils_reset_error(); // Com interface not enabled + // The transition event publisher is active + // The external transition services are inactive ret = rcl_lifecycle_state_machine_init( &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, false, &allocator); 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); + // Reset the state machine as the previous init call was successful + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // Everything should be good ret = rcl_lifecycle_state_machine_init( From 42a1ca0db314bddcb5ab851355c243e71e5c68ee Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 1 Mar 2021 18:47:16 -0800 Subject: [PATCH 3/9] flag if com interface is enabled Signed-off-by: Karsten Knese --- .../include/rcl_lifecycle/data_types.h | 2 ++ rcl_lifecycle/src/com_interface.c | 27 ++++++++++--------- rcl_lifecycle/src/rcl_lifecycle.c | 18 ++++++++----- 3 files changed, 28 insertions(+), 19 deletions(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index 776298dd2..3d93c77c5 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/data_types.h +++ b/rcl_lifecycle/include/rcl_lifecycle/data_types.h @@ -70,6 +70,8 @@ typedef struct rcl_lifecycle_transition_map_t /// It contains the communication interfac with the ROS world typedef struct rcl_lifecycle_com_interface_t { + /// Flag indicating whether the com interface is enabled or not. + bool enabled; /// Handle to the node used to create the publisher and the services rcl_node_t * node_handle; /// Event used to publish the transitions diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index e58ea7e84..b1704b6d8 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -86,7 +86,9 @@ rcl_lifecycle_com_interface_init( if (RCL_RET_OK != ret) { // cleanup the publisher, which was correctly initialized - (void) rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + 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; @@ -119,7 +121,8 @@ rcl_lifecycle_com_interface_publisher_init( fail: // error message is already logged on failure - (void) rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + ret = rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle); + (void) ret; return RCL_RET_ERROR; } @@ -128,18 +131,15 @@ rcl_lifecycle_com_interface_publisher_fini( rcl_lifecycle_com_interface_t * com_interface, rcl_node_t * node_handle) { - rcl_ret_t fcn_ret = RCL_RET_OK; - 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"); - fcn_ret = RCL_RET_ERROR; } - return fcn_ret; + return ret; } rcl_ret_t @@ -160,10 +160,12 @@ rcl_lifecycle_com_interface_services_init( 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); + 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); @@ -175,7 +177,7 @@ rcl_lifecycle_com_interface_services_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); @@ -187,7 +189,7 @@ rcl_lifecycle_com_interface_services_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); @@ -199,7 +201,7 @@ rcl_lifecycle_com_interface_services_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); @@ -211,7 +213,7 @@ rcl_lifecycle_com_interface_services_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); @@ -223,7 +225,8 @@ rcl_lifecycle_com_interface_services_init( fail: // error messages already logged on failure - (void) rcl_lifecycle_com_interface_services_fini(com_interface, node_handle); + ret = rcl_lifecycle_com_interface_services_fini(com_interface, node_handle); + (void) ret; return RCL_RET_ERROR; } diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index a2a96041c..03bb3040a 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -219,6 +219,7 @@ rcl_lifecycle_state_machine_init( return RCL_RET_ERROR; } } + state_machine->com_interface.enabled = enable_com_interface; if (default_states) { rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator); @@ -276,18 +277,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->com_interface.enabled) { + 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; } From 4eb1167a2522a7d25ec8a08a41f45201fe528e37 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 19 Mar 2021 19:21:47 -0700 Subject: [PATCH 4/9] use options struct Signed-off-by: Karsten Knese --- .../include/rcl_lifecycle/data_types.h | 15 +++++- .../include/rcl_lifecycle/rcl_lifecycle.h | 15 +++--- rcl_lifecycle/src/rcl_lifecycle.c | 32 ++++++++---- rcl_lifecycle/test/test_rcl_lifecycle.cpp | 50 ++++++++++++------- 4 files changed, 73 insertions(+), 39 deletions(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index 3d93c77c5..033203385 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/data_types.h +++ b/rcl_lifecycle/include/rcl_lifecycle/data_types.h @@ -70,8 +70,6 @@ typedef struct rcl_lifecycle_transition_map_t /// It contains the communication interfac with the ROS world typedef struct rcl_lifecycle_com_interface_t { - /// Flag indicating whether the com interface is enabled or not. - bool enabled; /// Handle to the node used to create the publisher and the services rcl_node_t * node_handle; /// Event used to publish the transitions @@ -88,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 { @@ -97,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 cd6d3d49f..56f7f0e95 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,11 +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] enable_com_interface if `false` the services listed above are not initialized, - * if true all services are available and active - * \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_optios 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. @@ -249,9 +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 enable_com_interface, - 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. /** diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 03bb3040a..de43400b1 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() @@ -188,9 +199,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 enable_com_interface, - 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); @@ -199,11 +208,13 @@ 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); + state_machine->options = *state_machine_options; + // enable full com_interface with pub & srvs - if (enable_com_interface) { + 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, @@ -219,14 +230,15 @@ rcl_lifecycle_state_machine_init( return RCL_RET_ERROR; } } - state_machine->com_interface.enabled = enable_com_interface; - if (default_states) { - rcl_ret_t 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, &state_machine->options.allocator); if (ret != RCL_RET_OK) { RCUTILS_SAFE_FWRITE_TO_STDERR( "Freeing state machine failed while handling a previous error. Leaking memory!\n"); @@ -277,7 +289,7 @@ rcl_lifecycle_state_machine_fini( rcl_ret_t rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t * state_machine) { - if (state_machine->com_interface.enabled) { + 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); diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp index a9fc399aa..86bb46054 100644 --- a/rcl_lifecycle/test/test_rcl_lifecycle.cpp +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -236,75 +236,75 @@ 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, true, 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, true, 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, true, 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, true, 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, true, 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, true, 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, true, 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, true, 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, true, 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, false, 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; EXPECT_NE(nullptr, &state_machine.com_interface); EXPECT_NE(nullptr, &state_machine.com_interface.pub_transition_event.impl); @@ -313,13 +313,19 @@ TEST(TestRclLifecycle, state_machine) { 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, &allocator); 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, true, 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 @@ -385,6 +391,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( @@ -429,7 +438,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, 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); @@ -534,8 +543,11 @@ 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, 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); if (RCL_RET_OK != ret) { From 1c460d0c4970178084494a6ee93349ba9c2bdbee Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 19 Mar 2021 21:27:47 -0700 Subject: [PATCH 5/9] attach allocator to state machine struct Signed-off-by: Karsten Knese --- .../include/rcl_lifecycle/rcl_lifecycle.h | 4 +- rcl_lifecycle/src/rcl_lifecycle.c | 11 +--- .../test/test_default_state_machine.cpp | 56 ++++++++++++------- .../test/test_multiple_instances.cpp | 24 +++++--- rcl_lifecycle/test/test_rcl_lifecycle.cpp | 17 ++---- 5 files changed, 61 insertions(+), 51 deletions(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h index 56f7f0e95..54da89da0 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -268,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. @@ -278,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/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index de43400b1..4cccfb178 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -237,8 +237,7 @@ rcl_lifecycle_state_machine_init( 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, &state_machine->options.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"); @@ -257,12 +256,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) { @@ -274,7 +269,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(); diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp index c8de67b33..0f34eb7f7 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() @@ -100,6 +102,7 @@ test_trigger_transition( TEST_F(TestDefaultStateMachine, zero_init) { rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_INVALID_ARGUMENT); + state_machine.options = this->state_machine_options; rcl_reset_error(); const rcl_lifecycle_transition_map_t * transition_map = &state_machine.transition_map; EXPECT_EQ(transition_map->states_size, 0u); @@ -107,7 +110,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 +123,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 +135,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 +201,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 +443,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 +518,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 +636,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 +785,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 +794,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 +819,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 +862,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 86bb46054..3bb1ea554 100644 --- a/rcl_lifecycle/test/test_rcl_lifecycle.cpp +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -317,7 +317,7 @@ TEST(TestRclLifecycle, state_machine) { 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, &allocator); + ret = rcl_lifecycle_state_machine_fini(&state_machine, &node); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // Everything should be good @@ -364,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(); } @@ -488,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; } @@ -549,9 +544,9 @@ TEST(TestRclLifecycle, init_fini_maybe_fail) { ret = rcl_lifecycle_state_machine_init( &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)); } } }); From 9347bdcf0b3f5d8fae202fe8dbe3b1e0d32cb713 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 22 Mar 2021 15:26:18 -0700 Subject: [PATCH 6/9] validate allocator before using in fini Signed-off-by: Karsten Knese --- rcl_lifecycle/src/rcl_lifecycle.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 4cccfb178..89a0d96b5 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -258,6 +258,11 @@ rcl_lifecycle_state_machine_fini( rcl_lifecycle_state_machine_t * state_machine, rcl_node_t * node_handle) { + RCL_CHECK_ALLOCATOR_WITH_MSG( + &state_machine->options.allocator, + "Can't cleanup state machine. No allocator associated with the state machine.\n", + return RCL_RET_ERROR); + rcl_ret_t fcn_ret = RCL_RET_OK; if (rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle) != RCL_RET_OK) { From f47faa76c13e38eb07cf58b563d5213f9d1c430e Mon Sep 17 00:00:00 2001 From: Knese Karsten Date: Mon, 22 Mar 2021 17:10:42 -0700 Subject: [PATCH 7/9] set allocator in default options Signed-off-by: Knese Karsten --- rcl_lifecycle/src/rcl_lifecycle.c | 1 + rcl_lifecycle/test/test_default_state_machine.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 89a0d96b5..d6c2d7ad1 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -186,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; } diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp index 0f34eb7f7..58f61b2e6 100644 --- a/rcl_lifecycle/test/test_default_state_machine.cpp +++ b/rcl_lifecycle/test/test_default_state_machine.cpp @@ -102,7 +102,6 @@ test_trigger_transition( TEST_F(TestDefaultStateMachine, zero_init) { rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_INVALID_ARGUMENT); - state_machine.options = this->state_machine_options; rcl_reset_error(); const rcl_lifecycle_transition_map_t * transition_map = &state_machine.transition_map; EXPECT_EQ(transition_map->states_size, 0u); From c1fec11f2efb0cb7f631afd4bcb110b0e43ed5ce Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 23 Mar 2021 09:09:48 -0700 Subject: [PATCH 8/9] Update rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h Co-authored-by: Chris Lalancette --- rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h index 54da89da0..9ffde3641 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -233,7 +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] state_machine_optios collection of config options for initializing 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. From 5328e6bab2d068a9c4ffa782ebacfec9f24c4061 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 23 Mar 2021 09:10:52 -0700 Subject: [PATCH 9/9] remove allocator check in fini Signed-off-by: Karsten Knese --- rcl_lifecycle/src/rcl_lifecycle.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index d6c2d7ad1..7a3afb1df 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -259,11 +259,6 @@ rcl_lifecycle_state_machine_fini( rcl_lifecycle_state_machine_t * state_machine, rcl_node_t * node_handle) { - RCL_CHECK_ALLOCATOR_WITH_MSG( - &state_machine->options.allocator, - "Can't cleanup state machine. No allocator associated with the state machine.\n", - return RCL_RET_ERROR); - rcl_ret_t fcn_ret = RCL_RET_OK; if (rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle) != RCL_RET_OK) {