make rcl_lifecycle_com_interface optional in lifecycle nodes (ros2#882)

* make rcl_lifecycle_com_interface optional

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* only disable services, not transition event publisher

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* flag if com interface is enabled

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* use options struct

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* attach allocator to state machine struct

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* validate allocator before using in fini

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* set allocator in default options

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* Update rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h

Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

* remove allocator check in fini

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

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

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.
 /**

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;
  }
  }

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(