From 5ebf07688d6896c721488fce7191ac05d4c4a5f3 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 1 Mar 2021 18:47:16 -0800 Subject: [PATCH] flag if com interface is enabled Signed-off-by: Karsten Knese --- .../include/rcl_lifecycle/data_types.h | 2 ++ rcl_lifecycle/src/com_interface.c | 28 +++++++++++-------- rcl_lifecycle/src/rcl_lifecycle.c | 18 +++++++----- 3 files changed, 29 insertions(+), 19 deletions(-) diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index 776298dd20..3d93c77c57 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 e58ea7e849..9355510f3b 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; } @@ -327,6 +330,7 @@ rcl_lifecycle_com_interface_publish_notification( msg.goal_state.id = goal->id; rosidl_runtime_c__String__assign(&msg.goal_state.label, goal->label); + fprintf(stderr, "publishing from rcl_lifecycle\n"); return rcl_publish(&com_interface->pub_transition_event, &msg, NULL); } diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index a2a96041c4..03bb3040ae 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; }