Skip to content

Commit

Permalink
make rcl_lifecycle_com_interface optional in lifecycle nodes (#882)
Browse files Browse the repository at this point in the history 
* make rcl_lifecycle_com_interface optional

Signed-off-by: Karsten Knese <[email protected]>

* only disable services, not transition event publisher

Signed-off-by: Karsten Knese <[email protected]>

* flag if com interface is enabled

Signed-off-by: Karsten Knese <[email protected]>

* use options struct

Signed-off-by: Karsten Knese <[email protected]>

* attach allocator to state machine struct

Signed-off-by: Karsten Knese <[email protected]>

* validate allocator before using in fini

Signed-off-by: Karsten Knese <[email protected]>

* set allocator in default options

Signed-off-by: Knese Karsten <[email protected]>

* Update rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h

Co-authored-by: Chris Lalancette <[email protected]>

* remove allocator check in fini

Signed-off-by: Karsten Knese <[email protected]>

Co-authored-by: Chris Lalancette <[email protected]>
  • Loading branch information
@Karsten1987 @clalancette
Karsten1987 and clalancette authored Mar 23, 2021
1 parent 0ad1150 commit e9b588d
Show file tree
Hide file tree
Showing 8 changed files with 320 additions and 137 deletions.
13 changes: 13 additions & 0 deletions rcl_lifecycle/include/rcl_lifecycle/data_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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
Expand Down
16 changes: 8 additions & 8 deletions rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand All @@ -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.
/**
Expand All @@ -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.
Expand All @@ -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.
/**
Expand Down
167 changes: 121 additions & 46 deletions rcl_lifecycle/src/com_interface.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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)
{
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand Down
30 changes: 30 additions & 0 deletions rcl_lifecycle/src/com_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
Loading

0 comments on commit e9b588d

Please sign in to comment.