Skip to content

Commit 733c3e4

Browse files
SteveMacenskimergify[bot]
SteveMacenski
authored and
mergify[bot]
committed
Autostarting lifecycle nodes and example launch file demo (#430)
* autostarting lifecycle nodes and example Signed-off-by: Steve Macenski <[email protected]> * fix linting Signed-off-by: Steve Macenski <[email protected]> * fix linting Signed-off-by: Steve Macenski <[email protected]> * removing an unused variable Signed-off-by: Steve Macenski <[email protected]> * initializing a member client as none like sub Signed-off-by: Steve Macenski <[email protected]> * completing auto-start feature for composition nodes Signed-off-by: Steve Macenski <[email protected]> * linting Signed-off-by: Steve Macenski <[email protected]> * final linting fix Signed-off-by: Steve Macenski <[email protected]> * Resolving issue with StateTransition messages Signed-off-by: Steve Macenski <[email protected]> * removing Node's autostart Signed-off-by: Steve Macenski <[email protected]> * Fixing lifecyclenode type Signed-off-by: Steve Macenski <[email protected]> * update docstring Signed-off-by: Steve Macenski <[email protected]> * adding in composable lifecycle node Signed-off-by: Steve Macenski <[email protected]> * Adding docstring Signed-off-by: Steve Macenski <[email protected]> * require autostart non-None Signed-off-by: Steve Macenski <[email protected]> * remove unused imports Signed-off-by: Steve Macenski <[email protected]> * review comments continued Signed-off-by: Steve Macenski <[email protected]> * is lifecycle node and autostart to lifecycle-only classes Signed-off-by: Steve Macenski <[email protected]> * final bits Signed-off-by: Steve Macenski <[email protected]> * whoops, remove the Avatar-inspired printout Signed-off-by: Steve Macenski <[email protected]> * finish linting Signed-off-by: Steve Macenski <[email protected]> * review fix Signed-off-by: Steve Macenski <[email protected]> * remove old import Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> (cherry picked from commit 3569f0d)
1 parent 62c56e5 commit 733c3e4

File tree

11 files changed

+454
-88
lines changed

11 files changed

+454
-88
lines changed

‎launch_ros/examples/lifecycle_autostart_pub_sub_launch.py‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
# Copyright 2024 Open Navigation LLC
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
"""Launch a lifecycle talker and a lifecycle listener."""
16+
17+
import os
18+
import sys
19+
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa
20+
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa
21+
22+
import launch # noqa: E402
23+
import launch.actions # noqa: E402
24+
import launch.events # noqa: E402
25+
26+
import launch_ros.actions # noqa: E402
27+
import launch_ros.events # noqa: E402
28+
import launch_ros.events.lifecycle # noqa: E402
29+
30+
31+
def main(argv=sys.argv[1:]):
32+
ld = launch.LaunchDescription()
33+
talker_node = launch_ros.actions.LifecycleNode(
34+
name='talker',
35+
namespace='',
36+
package='lifecycle',
37+
executable='lifecycle_talker',
38+
output='screen',
39+
autostart=True)
40+
listener_node = launch_ros.actions.LifecycleNode(
41+
name='listener',
42+
namespace='',
43+
package='lifecycle',
44+
executable='lifecycle_listener',
45+
output='screen',
46+
autostart=True)
47+
ld.add_action(talker_node)
48+
ld.add_action(listener_node)
49+
ls = launch.LaunchService(argv=argv)
50+
ls.include_launch_description(ld)
51+
return ls.run()
52+
53+
54+
if __name__ == '__main__':
55+
main()

‎launch_ros/examples/lifecycle_component_autostart_pub_sub_launch.py‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
# Copyright 2024 Open Navigation LLC
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
"""Launch a lifecycle talker and a lifecycle listener."""
16+
17+
import sys
18+
19+
import launch # noqa: E402
20+
21+
from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes
22+
from launch_ros.descriptions import ComposableLifecycleNode
23+
24+
25+
def main(argv=sys.argv[1:]):
26+
ld = launch.LaunchDescription()
27+
28+
# Can autostart from the container
29+
container = ComposableNodeContainer(
30+
name='lifecycle_component_container',
31+
namespace='',
32+
package='rclcpp_components',
33+
executable='component_container',
34+
composable_node_descriptions=[
35+
ComposableLifecycleNode(
36+
package='composition',
37+
plugin='composition::Listener',
38+
name='listener',
39+
autostart=True),
40+
]
41+
)
42+
43+
# ... and also from an external loader
44+
loader = LoadComposableNodes(
45+
target_container='lifecycle_component_container',
46+
composable_node_descriptions=[
47+
ComposableLifecycleNode(
48+
package='composition',
49+
plugin='composition::Talker',
50+
name='talker',
51+
autostart=True),
52+
],
53+
)
54+
55+
ld.add_action(container)
56+
ld.add_action(loader)
57+
ls = launch.LaunchService(argv=argv)
58+
ls.include_launch_description(ld)
59+
return ls.run()
60+
61+
62+
if __name__ == '__main__':
63+
main()

‎launch_ros/launch_ros/actions/lifecycle_node.py‎

Lines changed: 45 additions & 84 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,6 @@
1414

1515
"""Module for the LifecycleNode action."""
1616

17-
import functools
18-
import threading
19-
from typing import cast
2017
from typing import List
2118
from typing import Optional
2219

@@ -28,11 +25,10 @@
2825
import lifecycle_msgs.msg
2926
import lifecycle_msgs.srv
3027

28+
from .lifecycle_transition import LifecycleTransition
3129
from .node import Node
32-
from ..events.lifecycle import ChangeState
33-
from ..events.lifecycle import StateTransition
3430

35-
from ..ros_adapters import get_ros_node
31+
from ..utilities import LifecycleEventManager
3632

3733

3834
class LifecycleNode(Node):
@@ -43,6 +39,7 @@ def __init__(
4339
*,
4440
name: SomeSubstitutionsType,
4541
namespace: SomeSubstitutionsType,
42+
autostart: bool = False,
4643
**kwargs
4744
) -> None:
4845
"""
@@ -71,70 +68,25 @@ def __init__(
7168
:param name: The name of the lifecycle node.
7269
Although it defaults to None it is a required parameter and the default will be removed
7370
in a future release.
71+
:param namespace: The ROS namespace for this Node
72+
:param autostart: Whether or not to automatically transition to the 'active' state.
7473
"""
7574
super().__init__(name=name, namespace=namespace, **kwargs)
7675
self.__logger = launch.logging.get_logger(__name__)
77-
self.__rclpy_subscription = None
78-
self.__current_state = \
79-
ChangeState.valid_states[lifecycle_msgs.msg.State.PRIMARY_STATE_UNKNOWN]
80-
81-
def _on_transition_event(self, context, msg):
82-
try:
83-
event = StateTransition(action=self, msg=msg)
84-
self.__current_state = ChangeState.valid_states[msg.goal_state.id]
85-
context.asyncio_loop.call_soon_threadsafe(lambda: context.emit_event_sync(event))
86-
except Exception as exc:
87-
self.__logger.error(
88-
"Exception in handling of 'lifecycle.msg.TransitionEvent': {}".format(exc))
89-
90-
def _call_change_state(self, request, context: launch.LaunchContext):
91-
while not self.__rclpy_change_state_client.wait_for_service(timeout_sec=1.0):
92-
if context.is_shutdown:
93-
self.__logger.warning(
94-
"Abandoning wait for the '{}' service, due to shutdown.".format(
95-
self.__rclpy_change_state_client.srv_name),
96-
)
97-
return
98-
99-
# Asynchronously wait so that we can periodically check for shutdown.
100-
event = threading.Event()
101-
102-
def unblock(future):
103-
nonlocal event
104-
event.set()
105-
106-
response_future = self.__rclpy_change_state_client.call_async(request)
107-
response_future.add_done_callback(unblock)
108-
109-
while not event.wait(1.0):
110-
if context.is_shutdown:
111-
self.__logger.warning(
112-
"Abandoning wait for the '{}' service response, due to shutdown.".format(
113-
self.__rclpy_change_state_client.srv_name),
114-
)
115-
response_future.cancel()
116-
return
117-
118-
if response_future.exception() is not None:
119-
raise response_future.exception()
120-
response = response_future.result()
121-
122-
if not response.success:
123-
self.__logger.error(
124-
"Failed to make transition '{}' for LifecycleNode '{}'".format(
125-
ChangeState.valid_transitions[request.transition.id],
126-
self.node_name,
127-
)
128-
)
129-
130-
def _on_change_state_event(self, context: launch.LaunchContext) -> None:
131-
typed_event = cast(ChangeState, context.locals.event)
132-
if not typed_event.lifecycle_node_matcher(self):
133-
return None
134-
request = lifecycle_msgs.srv.ChangeState.Request()
135-
request.transition.id = typed_event.transition_id
136-
context.add_completion_future(
137-
context.asyncio_loop.run_in_executor(None, self._call_change_state, request, context))
76+
self.__autostart = autostart
77+
self.__lifecycle_event_manager = None
78+
79+
@property
80+
def node_autostart(self):
81+
"""Getter for autostart."""
82+
return self.__autostart
83+
84+
@property
85+
def is_lifecycle_node(self):
86+
return True
87+
88+
def get_lifecycle_event_manager(self):
89+
return self.__lifecycle_event_manager
13890

13991
def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]:
14092
"""
@@ -145,21 +97,30 @@ def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]:
14597
self._perform_substitutions(context) # ensure self.node_name is expanded
14698
if '<node_name_unspecified>' in self.node_name:
14799
raise RuntimeError('node_name unexpectedly incomplete for lifecycle node')
148-
node = get_ros_node(context)
149-
# Create a subscription to monitor the state changes of the subprocess.
150-
self.__rclpy_subscription = node.create_subscription(
151-
lifecycle_msgs.msg.TransitionEvent,
152-
'{}/transition_event'.format(self.node_name),
153-
functools.partial(self._on_transition_event, context),
154-
10)
155-
# Create a service client to change state on demand.
156-
self.__rclpy_change_state_client = node.create_client(
157-
lifecycle_msgs.srv.ChangeState,
158-
'{}/change_state'.format(self.node_name))
159-
# Register an event handler to change states on a ChangeState lifecycle event.
160-
context.register_event_handler(launch.EventHandler(
161-
matcher=lambda event: isinstance(event, ChangeState),
162-
entities=[launch.actions.OpaqueFunction(function=self._on_change_state_event)],
163-
))
100+
101+
self.__lifecycle_event_manager = LifecycleEventManager(self)
102+
self.__lifecycle_event_manager.setup_lifecycle_manager(context)
103+
104+
# If autostart is enabled, transition to the 'active' state.
105+
autostart_actions = None
106+
if self.node_autostart:
107+
autostart_actions = [
108+
LifecycleTransition(
109+
lifecycle_node_names=[self.node_name],
110+
transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE]
111+
),
112+
LifecycleTransition(
113+
lifecycle_node_names=[self.node_name],
114+
transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE]
115+
),
116+
]
117+
164118
# Delegate execution to Node and ExecuteProcess.
165-
return super().execute(context)
119+
node_actions = super().execute(context) # type: Optional[List[Action]]
120+
if node_actions is not None and autostart_actions is not None:
121+
return node_actions + autostart_actions
122+
if node_actions is not None:
123+
return node_actions
124+
if autostart_actions is not None:
125+
return autostart_actions
126+
return None

‎launch_ros/launch_ros/actions/load_composable_nodes.py‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,10 @@
3838
from launch.utilities import perform_substitutions
3939
from launch_ros.parameter_descriptions import ParameterFile
4040

41+
import lifecycle_msgs.msg
42+
4143
from .composable_node_container import ComposableNodeContainer
44+
from .lifecycle_transition import LifecycleTransition
4245

4346
from ..descriptions import ComposableNode
4447
from ..ros_adapters import get_ros_node
@@ -230,19 +233,48 @@ def execute(
230233
# Generate load requests before execute() exits to avoid race with context changing
231234
# due to scope change (e.g. if loading nodes from within a GroupAction).
232235
load_node_requests = []
236+
autostart_actions = []
233237
for node_description in self.__composable_node_descriptions:
234238
request = get_composable_node_load_request(node_description, context)
235239
# The request can be None if the node description's condition evaluates to False
236240
if request is not None:
237241
load_node_requests.append(request)
238242

243+
# If autostart is enabled, transition to the 'active' state.
244+
if hasattr(node_description, 'node_autostart') and node_description.node_autostart:
245+
complete_node_name = request.node_namespace + request.node_name
246+
if not complete_node_name.startswith('/'):
247+
complete_node_name = '/' + complete_node_name
248+
self.__logger.info(
249+
'Autostart enabled for requested lifecycle node {}'.format(complete_node_name))
250+
node_description.init_lifecycle_event_manager(context)
251+
autostart_actions.append(
252+
LifecycleTransition(
253+
lifecycle_node_names=[complete_node_name],
254+
transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE]
255+
))
256+
autostart_actions.append(
257+
LifecycleTransition(
258+
lifecycle_node_names=[complete_node_name],
259+
transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE]
260+
),
261+
)
262+
239263
if load_node_requests:
240264
context.add_completion_future(
241265
context.asyncio_loop.run_in_executor(
242266
None, self._load_in_sequence, load_node_requests, context
243267
)
244268
)
245269

270+
load_actions = super().execute(context)
271+
if load_actions is not None and len(autostart_actions) != 0:
272+
return load_actions + autostart_actions
273+
if load_actions is not None:
274+
return load_actions
275+
if len(autostart_actions) != 0:
276+
return autostart_actions
277+
246278

247279
def get_composable_node_load_request(
248280
composable_node_description: ComposableNode,

‎launch_ros/launch_ros/descriptions/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
"""descriptions Module."""
1616

17+
from .composable_lifecycle_node import ComposableLifecycleNode
1718
from .composable_node import ComposableNode
1819
from ..parameter_descriptions import Parameter
1920
from ..parameter_descriptions import ParameterFile
@@ -22,6 +23,7 @@
2223

2324
__all__ = [
2425
'ComposableNode',
26+
'ComposableLifecycleNode',
2527
'Parameter',
2628
'ParameterFile',
2729
'ParameterValue',

0 commit comments

Comments
 (0)