From ed6d02958faa5d8fc20cb9a8b2297227332ffca3 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 7 Jan 2021 09:43:25 -1000 Subject: [PATCH] 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(