Skip to content

Commit

Permalink
only disable services, not transition event publisher
Browse files Browse the repository at this point in the history 
Signed-off-by: Karsten Knese <[email protected]>
  • Loading branch information
@Karsten1987
Karsten1987 committed Mar 2, 2021
1 parent a40a4e5 commit ed6d029
Show file tree
Hide file tree
Showing 4 changed files with 157 additions and 42 deletions.
156 changes: 114 additions & 42 deletions rcl_lifecycle/src/com_interface.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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)
{
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
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
7 changes: 7 additions & 0 deletions rcl_lifecycle/src/rcl_lifecycle.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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) {
Expand Down
6 changes: 6 additions & 0 deletions rcl_lifecycle/test/test_rcl_lifecycle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down

0 comments on commit ed6d029

Please sign in to comment.