Skip to content

Commit e9b588d

Browse files
Karsten1987clalancette
Karsten1987
and
clalancette
authored
make rcl_lifecycle_com_interface optional in lifecycle nodes (#882)
* 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]>
1 parent 0ad1150 commit e9b588d

File tree

8 files changed

+320
-137
lines changed

8 files changed

+320
-137
lines changed

‎rcl_lifecycle/include/rcl_lifecycle/data_types.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,17 @@ typedef struct rcl_lifecycle_com_interface_t
8686
rcl_service_t srv_get_transition_graph;
8787
} rcl_lifecycle_com_interface_t;
8888

89+
/// It contains various options to configure the rcl_lifecycle_state_machine_t instance
90+
typedef struct rcl_lifecycle_state_machine_options_t
91+
{
92+
/// Flag indicating whether the state machine shall be initialized with default states
93+
bool initialize_default_states;
94+
/// Flag indicating whether the com interface shall be used or not
95+
bool enable_com_interface;
96+
/// Allocator used for allocating states and transitions
97+
rcl_allocator_t allocator;
98+
} rcl_lifecycle_state_machine_options_t;
99+
89100
/// It contains the state machine data
90101
typedef struct rcl_lifecycle_state_machine_t
91102
{
@@ -95,6 +106,8 @@ typedef struct rcl_lifecycle_state_machine_t
95106
rcl_lifecycle_transition_map_t transition_map;
96107
/// Communication interface into a ROS world
97108
rcl_lifecycle_com_interface_t com_interface;
109+
/// Options struct with which the state machine was initialized
110+
rcl_lifecycle_state_machine_options_t options;
98111
} rcl_lifecycle_state_machine_t;
99112

100113
#ifdef __cplusplus

‎rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -194,6 +194,11 @@ rcl_lifecycle_transition_fini(
194194
rcl_lifecycle_transition_t * transition,
195195
const rcl_allocator_t * allocator);
196196

197+
/// Return a default initialized state machine options struct.
198+
RCL_LIFECYCLE_PUBLIC
199+
rcl_lifecycle_state_machine_options_t
200+
rcl_lifecycle_get_default_state_machine_options();
201+
197202
/// Return a rcl_lifecycle_state_machine_t struct with members set to `NULL` or 0.
198203
/**
199204
* 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();
228233
* available transitions
229234
* \param[in] ts_srv_get_transition_graph pointer to the service that allows to get transitions from
230235
* the graph
231-
* \param[in] default_states if `true` a new default state machine is initialized, otherwise
232-
* the state_machine pointer is only used to initialize the interfaces
233-
* \param[in] allocator a valid allocator used to initialized the state machine
236+
* \param[in] state_machine_options collection of config options for initializing the state machine
234237
* \return `RCL_RET_OK` if the state machine was initialized successfully, or
235238
* \return `RCL_RET_INVALID_ARGUMENT` if input params is NULL, or
236239
* \return `RCL_RET_ERROR` if an unspecified error occurs.
@@ -247,8 +250,7 @@ rcl_lifecycle_state_machine_init(
247250
const rosidl_service_type_support_t * ts_srv_get_available_states,
248251
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
249252
const rosidl_service_type_support_t * ts_srv_get_transition_graph,
250-
bool default_states,
251-
const rcl_allocator_t * allocator);
253+
const rcl_lifecycle_state_machine_options_t * state_machine_options);
252254

253255
/// Finalize a rcl_lifecycle_state_machine_t.
254256
/**
@@ -266,7 +268,6 @@ rcl_lifecycle_state_machine_init(
266268
*
267269
* \param[inout] state_machine struct to be finalized
268270
* \param[in] node_handle valid (not finalized) handle to the node
269-
* \param[in] allocator a valid allocator used to finalize the state machine
270271
* \return `RCL_RET_OK` if the state was finalized successfully, or
271272
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
272273
* \return `RCL_RET_ERROR` if an unspecified error occurs.
@@ -276,8 +277,7 @@ RCL_WARN_UNUSED
276277
rcl_ret_t
277278
rcl_lifecycle_state_machine_fini(
278279
rcl_lifecycle_state_machine_t * state_machine,
279-
rcl_node_t * node_handle,
280-
const rcl_allocator_t * allocator);
280+
rcl_node_t * node_handle);
281281

282282
/// Check if a state machine is active.
283283
/**

‎rcl_lifecycle/src/com_interface.c‎

Lines changed: 121 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -68,35 +68,104 @@ rcl_lifecycle_com_interface_init(
6868
const rosidl_service_type_support_t * ts_srv_get_available_states,
6969
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
7070
const rosidl_service_type_support_t * ts_srv_get_transition_graph)
71+
{
72+
rcl_ret_t ret = rcl_lifecycle_com_interface_publisher_init(
73+
com_interface, node_handle, ts_pub_notify);
74+
if (ret != RCL_RET_OK) {
75+
return ret;
76+
}
77+
78+
ret = rcl_lifecycle_com_interface_services_init(
79+
com_interface,
80+
node_handle,
81+
ts_srv_change_state,
82+
ts_srv_get_state,
83+
ts_srv_get_available_states,
84+
ts_srv_get_available_transitions,
85+
ts_srv_get_transition_graph);
86+
87+
if (RCL_RET_OK != ret) {
88+
// cleanup the publisher, which was correctly initialized
89+
rcl_ret_t ret_fini = rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle);
90+
// warning is already set, no need to log anything here
91+
(void) ret_fini;
92+
}
93+
94+
return ret;
95+
}
96+
97+
rcl_ret_t
98+
rcl_lifecycle_com_interface_publisher_init(
99+
rcl_lifecycle_com_interface_t * com_interface,
100+
rcl_node_t * node_handle,
101+
const rosidl_message_type_support_t * ts_pub_notify)
71102
{
72103
RCL_CHECK_ARGUMENT_FOR_NULL(com_interface, RCL_RET_INVALID_ARGUMENT);
73104
RCL_CHECK_ARGUMENT_FOR_NULL(node_handle, RCL_RET_INVALID_ARGUMENT);
74105
RCL_CHECK_ARGUMENT_FOR_NULL(ts_pub_notify, RCL_RET_INVALID_ARGUMENT);
106+
107+
// initialize publisher
108+
rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
109+
rcl_ret_t ret = rcl_publisher_init(
110+
&com_interface->pub_transition_event, node_handle,
111+
ts_pub_notify, pub_transition_event_topic, &publisher_options);
112+
113+
if (ret != RCL_RET_OK) {
114+
goto fail;
115+
}
116+
117+
// initialize static message for notification
118+
lifecycle_msgs__msg__TransitionEvent__init(&msg);
119+
120+
return RCL_RET_OK;
121+
122+
fail:
123+
// error message is already logged on failure
124+
ret = rcl_lifecycle_com_interface_publisher_fini(com_interface, node_handle);
125+
(void) ret;
126+
return RCL_RET_ERROR;
127+
}
128+
129+
rcl_ret_t
130+
rcl_lifecycle_com_interface_publisher_fini(
131+
rcl_lifecycle_com_interface_t * com_interface,
132+
rcl_node_t * node_handle)
133+
{
134+
lifecycle_msgs__msg__TransitionEvent__fini(&msg);
135+
136+
rcl_ret_t ret = rcl_publisher_fini(
137+
&com_interface->pub_transition_event, node_handle);
138+
if (ret != RCL_RET_OK) {
139+
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy transition_event publisher");
140+
}
141+
142+
return ret;
143+
}
144+
145+
rcl_ret_t
146+
rcl_lifecycle_com_interface_services_init(
147+
rcl_lifecycle_com_interface_t * com_interface,
148+
rcl_node_t * node_handle,
149+
const rosidl_service_type_support_t * ts_srv_change_state,
150+
const rosidl_service_type_support_t * ts_srv_get_state,
151+
const rosidl_service_type_support_t * ts_srv_get_available_states,
152+
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
153+
const rosidl_service_type_support_t * ts_srv_get_transition_graph)
154+
{
155+
RCL_CHECK_ARGUMENT_FOR_NULL(com_interface, RCL_RET_INVALID_ARGUMENT);
156+
RCL_CHECK_ARGUMENT_FOR_NULL(node_handle, RCL_RET_INVALID_ARGUMENT);
75157
RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_change_state, RCL_RET_INVALID_ARGUMENT);
76158
RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_state, RCL_RET_INVALID_ARGUMENT);
77159
RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_available_states, RCL_RET_INVALID_ARGUMENT);
78160
RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_available_transitions, RCL_RET_INVALID_ARGUMENT);
79161
RCL_CHECK_ARGUMENT_FOR_NULL(ts_srv_get_transition_graph, RCL_RET_INVALID_ARGUMENT);
80162

81-
// initialize publisher
82-
{
83-
rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
84-
rcl_ret_t ret = rcl_publisher_init(
85-
&com_interface->pub_transition_event, node_handle,
86-
ts_pub_notify, pub_transition_event_topic, &publisher_options);
87-
88-
if (ret != RCL_RET_OK) {
89-
goto fail;
90-
}
91-
92-
// initialize static message for notification
93-
lifecycle_msgs__msg__TransitionEvent__init(&msg);
94-
}
163+
rcl_ret_t ret = RCL_RET_OK;
95164

96165
// initialize change state service
97166
{
98167
rcl_service_options_t service_options = rcl_service_get_default_options();
99-
rcl_ret_t ret = rcl_service_init(
168+
ret = rcl_service_init(
100169
&com_interface->srv_change_state, node_handle,
101170
ts_srv_change_state, srv_change_state_service, &service_options);
102171

@@ -108,7 +177,7 @@ rcl_lifecycle_com_interface_init(
108177
// initialize get state service
109178
{
110179
rcl_service_options_t service_options = rcl_service_get_default_options();
111-
rcl_ret_t ret = rcl_service_init(
180+
ret = rcl_service_init(
112181
&com_interface->srv_get_state, node_handle,
113182
ts_srv_get_state, srv_get_state_service, &service_options);
114183

@@ -120,7 +189,7 @@ rcl_lifecycle_com_interface_init(
120189
// initialize get available states service
121190
{
122191
rcl_service_options_t service_options = rcl_service_get_default_options();
123-
rcl_ret_t ret = rcl_service_init(
192+
ret = rcl_service_init(
124193
&com_interface->srv_get_available_states, node_handle,
125194
ts_srv_get_available_states, srv_get_available_states_service, &service_options);
126195

@@ -132,7 +201,7 @@ rcl_lifecycle_com_interface_init(
132201
// initialize get available transitions service
133202
{
134203
rcl_service_options_t service_options = rcl_service_get_default_options();
135-
rcl_ret_t ret = rcl_service_init(
204+
ret = rcl_service_init(
136205
&com_interface->srv_get_available_transitions, node_handle,
137206
ts_srv_get_available_transitions, srv_get_available_transitions_service, &service_options);
138207

@@ -144,7 +213,7 @@ rcl_lifecycle_com_interface_init(
144213
// initialize get transition graph service
145214
{
146215
rcl_service_options_t service_options = rcl_service_get_default_options();
147-
rcl_ret_t ret = rcl_service_init(
216+
ret = rcl_service_init(
148217
&com_interface->srv_get_transition_graph, node_handle,
149218
ts_srv_get_transition_graph, srv_get_transition_graph, &service_options);
150219

@@ -155,32 +224,14 @@ rcl_lifecycle_com_interface_init(
155224
return RCL_RET_OK;
156225

157226
fail:
158-
if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) {
159-
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy transition_event publisher");
160-
}
161-
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_change_state, node_handle)) {
162-
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy change_state service");
163-
}
164-
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_state, node_handle)) {
165-
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_state service");
166-
}
167-
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_states, node_handle)) {
168-
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_available_states service");
169-
}
170-
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_transitions, node_handle)) {
171-
RCUTILS_LOG_ERROR_NAMED(
172-
ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service");
173-
}
174-
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_transition_graph, node_handle)) {
175-
RCUTILS_LOG_ERROR_NAMED(
176-
ROS_PACKAGE_NAME, "Failed to destroy get_transition_graph service");
177-
}
178-
227+
// error messages already logged on failure
228+
ret = rcl_lifecycle_com_interface_services_fini(com_interface, node_handle);
229+
(void) ret;
179230
return RCL_RET_ERROR;
180231
}
181232

182233
rcl_ret_t
183-
rcl_lifecycle_com_interface_fini(
234+
rcl_lifecycle_com_interface_services_fini(
184235
rcl_lifecycle_com_interface_t * com_interface,
185236
rcl_node_t * node_handle)
186237
{
@@ -191,6 +242,8 @@ rcl_lifecycle_com_interface_fini(
191242
rcl_ret_t ret = rcl_service_fini(
192243
&com_interface->srv_get_transition_graph, node_handle);
193244
if (ret != RCL_RET_OK) {
245+
RCUTILS_LOG_ERROR_NAMED(
246+
ROS_PACKAGE_NAME, "Failed to destroy get_transition_graph service");
194247
fcn_ret = RCL_RET_ERROR;
195248
}
196249
}
@@ -200,6 +253,8 @@ rcl_lifecycle_com_interface_fini(
200253
rcl_ret_t ret = rcl_service_fini(
201254
&com_interface->srv_get_available_transitions, node_handle);
202255
if (ret != RCL_RET_OK) {
256+
RCUTILS_LOG_ERROR_NAMED(
257+
ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service");
203258
fcn_ret = RCL_RET_ERROR;
204259
}
205260
}
@@ -209,6 +264,7 @@ rcl_lifecycle_com_interface_fini(
209264
rcl_ret_t ret = rcl_service_fini(
210265
&com_interface->srv_get_available_states, node_handle);
211266
if (ret != RCL_RET_OK) {
267+
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_available_states service");
212268
fcn_ret = RCL_RET_ERROR;
213269
}
214270
}
@@ -218,6 +274,7 @@ rcl_lifecycle_com_interface_fini(
218274
rcl_ret_t ret = rcl_service_fini(
219275
&com_interface->srv_get_state, node_handle);
220276
if (ret != RCL_RET_OK) {
277+
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_state service");
221278
fcn_ret = RCL_RET_ERROR;
222279
}
223280
}
@@ -227,17 +284,35 @@ rcl_lifecycle_com_interface_fini(
227284
rcl_ret_t ret = rcl_service_fini(
228285
&com_interface->srv_change_state, node_handle);
229286
if (ret != RCL_RET_OK) {
287+
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy change_state service");
230288
fcn_ret = RCL_RET_ERROR;
231289
}
232290
}
233291

234-
// destroy the publisher
292+
return fcn_ret;
293+
}
294+
295+
rcl_ret_t
296+
rcl_lifecycle_com_interface_fini(
297+
rcl_lifecycle_com_interface_t * com_interface,
298+
rcl_node_t * node_handle)
299+
{
300+
rcl_ret_t fcn_ret = RCL_RET_OK;
301+
302+
// destroy the services
235303
{
236-
lifecycle_msgs__msg__TransitionEvent__fini(&msg);
304+
rcl_ret_t ret = rcl_lifecycle_com_interface_services_fini(
305+
com_interface, node_handle);
306+
if (RCL_RET_OK != ret) {
307+
fcn_ret = RCL_RET_ERROR;
308+
}
309+
}
237310

238-
rcl_ret_t ret = rcl_publisher_fini(
239-
&com_interface->pub_transition_event, node_handle);
240-
if (ret != RCL_RET_OK) {
311+
// destroy the event publisher
312+
{
313+
rcl_ret_t ret = rcl_lifecycle_com_interface_publisher_fini(
314+
com_interface, node_handle);
315+
if (RCL_RET_OK != ret) {
241316
fcn_ret = RCL_RET_ERROR;
242317
}
243318
}

‎rcl_lifecycle/src/com_interface.h‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,36 @@ rcl_lifecycle_com_interface_init(
3939
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
4040
const rosidl_service_type_support_t * ts_srv_get_transition_graph);
4141

42+
rcl_ret_t
43+
RCL_WARN_UNUSED
44+
rcl_lifecycle_com_interface_publisher_init(
45+
rcl_lifecycle_com_interface_t * com_interface,
46+
rcl_node_t * node_handle,
47+
const rosidl_message_type_support_t * ts_pub_notify);
48+
49+
rcl_ret_t
50+
RCL_WARN_UNUSED
51+
rcl_lifecycle_com_interface_publisher_fini(
52+
rcl_lifecycle_com_interface_t * com_interface,
53+
rcl_node_t * node_handle);
54+
55+
rcl_ret_t
56+
RCL_WARN_UNUSED
57+
rcl_lifecycle_com_interface_services_init(
58+
rcl_lifecycle_com_interface_t * com_interface,
59+
rcl_node_t * node_handle,
60+
const rosidl_service_type_support_t * ts_srv_change_state,
61+
const rosidl_service_type_support_t * ts_srv_get_state,
62+
const rosidl_service_type_support_t * ts_srv_get_available_states,
63+
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
64+
const rosidl_service_type_support_t * ts_srv_get_transition_graph);
65+
66+
rcl_ret_t
67+
RCL_WARN_UNUSED
68+
rcl_lifecycle_com_interface_services_fini(
69+
rcl_lifecycle_com_interface_t * com_interface,
70+
rcl_node_t * node_handle);
71+
4272
rcl_ret_t
4373
RCL_WARN_UNUSED
4474
rcl_lifecycle_com_interface_fini(

0 commit comments

Comments
 (0)