diff --git a/launch_ros/examples/lifecycle_autostart_pub_sub_launch.py b/launch_ros/examples/lifecycle_autostart_pub_sub_launch.py new file mode 100644 index 00000000..2138b26a --- /dev/null +++ b/launch_ros/examples/lifecycle_autostart_pub_sub_launch.py @@ -0,0 +1,55 @@ +# Copyright 2024 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch a lifecycle talker and a lifecycle listener.""" + +import os +import sys +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa + +import launch # noqa: E402 +import launch.actions # noqa: E402 +import launch.events # noqa: E402 + +import launch_ros.actions # noqa: E402 +import launch_ros.events # noqa: E402 +import launch_ros.events.lifecycle # noqa: E402 + + +def main(argv=sys.argv[1:]): + ld = launch.LaunchDescription() + talker_node = launch_ros.actions.LifecycleNode( + name='talker', + namespace='', + package='lifecycle', + executable='lifecycle_talker', + output='screen', + autostart=True) + listener_node = launch_ros.actions.LifecycleNode( + name='listener', + namespace='', + package='lifecycle', + executable='lifecycle_listener', + output='screen', + autostart=True) + ld.add_action(talker_node) + ld.add_action(listener_node) + ls = launch.LaunchService(argv=argv) + ls.include_launch_description(ld) + return ls.run() + + +if __name__ == '__main__': + main() diff --git a/launch_ros/examples/lifecycle_component_autostart_pub_sub_launch.py b/launch_ros/examples/lifecycle_component_autostart_pub_sub_launch.py new file mode 100644 index 00000000..65a03f5f --- /dev/null +++ b/launch_ros/examples/lifecycle_component_autostart_pub_sub_launch.py @@ -0,0 +1,63 @@ +# Copyright 2024 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch a lifecycle talker and a lifecycle listener.""" + +import sys + +import launch # noqa: E402 + +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableLifecycleNode + + +def main(argv=sys.argv[1:]): + ld = launch.LaunchDescription() + + # Can autostart from the container + container = ComposableNodeContainer( + name='lifecycle_component_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableLifecycleNode( + package='composition', + plugin='composition::Listener', + name='listener', + autostart=True), + ] + ) + + # ... and also from an external loader + loader = LoadComposableNodes( + target_container='lifecycle_component_container', + composable_node_descriptions=[ + ComposableLifecycleNode( + package='composition', + plugin='composition::Talker', + name='talker', + autostart=True), + ], + ) + + ld.add_action(container) + ld.add_action(loader) + ls = launch.LaunchService(argv=argv) + ls.include_launch_description(ld) + return ls.run() + + +if __name__ == '__main__': + main() diff --git a/launch_ros/launch_ros/actions/lifecycle_node.py b/launch_ros/launch_ros/actions/lifecycle_node.py index dd5cb46b..79bca627 100644 --- a/launch_ros/launch_ros/actions/lifecycle_node.py +++ b/launch_ros/launch_ros/actions/lifecycle_node.py @@ -14,9 +14,6 @@ """Module for the LifecycleNode action.""" -import functools -import threading -from typing import cast from typing import List from typing import Optional @@ -28,11 +25,10 @@ import lifecycle_msgs.msg import lifecycle_msgs.srv +from .lifecycle_transition import LifecycleTransition from .node import Node -from ..events.lifecycle import ChangeState -from ..events.lifecycle import StateTransition -from ..ros_adapters import get_ros_node +from ..utilities import LifecycleEventManager class LifecycleNode(Node): @@ -43,6 +39,7 @@ def __init__( *, name: SomeSubstitutionsType, namespace: SomeSubstitutionsType, + autostart: bool = False, **kwargs ) -> None: """ @@ -71,70 +68,25 @@ def __init__( :param name: The name of the lifecycle node. Although it defaults to None it is a required parameter and the default will be removed in a future release. + :param namespace: The ROS namespace for this Node + :param autostart: Whether or not to automatically transition to the 'active' state. """ super().__init__(name=name, namespace=namespace, **kwargs) self.__logger = launch.logging.get_logger(__name__) - self.__rclpy_subscription = None - self.__current_state = \ - ChangeState.valid_states[lifecycle_msgs.msg.State.PRIMARY_STATE_UNKNOWN] - - def _on_transition_event(self, context, msg): - try: - event = StateTransition(action=self, msg=msg) - self.__current_state = ChangeState.valid_states[msg.goal_state.id] - context.asyncio_loop.call_soon_threadsafe(lambda: context.emit_event_sync(event)) - except Exception as exc: - self.__logger.error( - "Exception in handling of 'lifecycle.msg.TransitionEvent': {}".format(exc)) - - def _call_change_state(self, request, context: launch.LaunchContext): - while not self.__rclpy_change_state_client.wait_for_service(timeout_sec=1.0): - if context.is_shutdown: - self.__logger.warning( - "Abandoning wait for the '{}' service, due to shutdown.".format( - self.__rclpy_change_state_client.srv_name), - ) - return - - # Asynchronously wait so that we can periodically check for shutdown. - event = threading.Event() - - def unblock(future): - nonlocal event - event.set() - - response_future = self.__rclpy_change_state_client.call_async(request) - response_future.add_done_callback(unblock) - - while not event.wait(1.0): - if context.is_shutdown: - self.__logger.warning( - "Abandoning wait for the '{}' service response, due to shutdown.".format( - self.__rclpy_change_state_client.srv_name), - ) - response_future.cancel() - return - - if response_future.exception() is not None: - raise response_future.exception() - response = response_future.result() - - if not response.success: - self.__logger.error( - "Failed to make transition '{}' for LifecycleNode '{}'".format( - ChangeState.valid_transitions[request.transition.id], - self.node_name, - ) - ) - - def _on_change_state_event(self, context: launch.LaunchContext) -> None: - typed_event = cast(ChangeState, context.locals.event) - if not typed_event.lifecycle_node_matcher(self): - return None - request = lifecycle_msgs.srv.ChangeState.Request() - request.transition.id = typed_event.transition_id - context.add_completion_future( - context.asyncio_loop.run_in_executor(None, self._call_change_state, request, context)) + self.__autostart = autostart + self.__lifecycle_event_manager = None + + @property + def node_autostart(self): + """Getter for autostart.""" + return self.__autostart + + @property + def is_lifecycle_node(self): + return True + + def get_lifecycle_event_manager(self): + return self.__lifecycle_event_manager def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]: """ @@ -145,21 +97,30 @@ def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]: self._perform_substitutions(context) # ensure self.node_name is expanded if '' in self.node_name: raise RuntimeError('node_name unexpectedly incomplete for lifecycle node') - node = get_ros_node(context) - # Create a subscription to monitor the state changes of the subprocess. - self.__rclpy_subscription = node.create_subscription( - lifecycle_msgs.msg.TransitionEvent, - '{}/transition_event'.format(self.node_name), - functools.partial(self._on_transition_event, context), - 10) - # Create a service client to change state on demand. - self.__rclpy_change_state_client = node.create_client( - lifecycle_msgs.srv.ChangeState, - '{}/change_state'.format(self.node_name)) - # Register an event handler to change states on a ChangeState lifecycle event. - context.register_event_handler(launch.EventHandler( - matcher=lambda event: isinstance(event, ChangeState), - entities=[launch.actions.OpaqueFunction(function=self._on_change_state_event)], - )) + + self.__lifecycle_event_manager = LifecycleEventManager(self) + self.__lifecycle_event_manager.setup_lifecycle_manager(context) + + # If autostart is enabled, transition to the 'active' state. + autostart_actions = None + if self.node_autostart: + autostart_actions = [ + LifecycleTransition( + lifecycle_node_names=[self.node_name], + transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE] + ), + LifecycleTransition( + lifecycle_node_names=[self.node_name], + transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE] + ), + ] + # Delegate execution to Node and ExecuteProcess. - return super().execute(context) + node_actions = super().execute(context) # type: Optional[List[Action]] + if node_actions is not None and autostart_actions is not None: + return node_actions + autostart_actions + if node_actions is not None: + return node_actions + if autostart_actions is not None: + return autostart_actions + return None diff --git a/launch_ros/launch_ros/actions/load_composable_nodes.py b/launch_ros/launch_ros/actions/load_composable_nodes.py index 6ab36f7f..ba4f6e21 100644 --- a/launch_ros/launch_ros/actions/load_composable_nodes.py +++ b/launch_ros/launch_ros/actions/load_composable_nodes.py @@ -38,7 +38,10 @@ from launch.utilities import perform_substitutions from launch_ros.parameter_descriptions import ParameterFile +import lifecycle_msgs.msg + from .composable_node_container import ComposableNodeContainer +from .lifecycle_transition import LifecycleTransition from ..descriptions import ComposableNode from ..ros_adapters import get_ros_node @@ -230,12 +233,33 @@ def execute( # Generate load requests before execute() exits to avoid race with context changing # due to scope change (e.g. if loading nodes from within a GroupAction). load_node_requests = [] + autostart_actions = [] for node_description in self.__composable_node_descriptions: request = get_composable_node_load_request(node_description, context) # The request can be None if the node description's condition evaluates to False if request is not None: load_node_requests.append(request) + # If autostart is enabled, transition to the 'active' state. + if hasattr(node_description, 'node_autostart') and node_description.node_autostart: + complete_node_name = request.node_namespace + request.node_name + if not complete_node_name.startswith('/'): + complete_node_name = '/' + complete_node_name + self.__logger.info( + 'Autostart enabled for requested lifecycle node {}'.format(complete_node_name)) + node_description.init_lifecycle_event_manager(context) + autostart_actions.append( + LifecycleTransition( + lifecycle_node_names=[complete_node_name], + transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE] + )) + autostart_actions.append( + LifecycleTransition( + lifecycle_node_names=[complete_node_name], + transition_ids=[lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE] + ), + ) + if load_node_requests: context.add_completion_future( context.asyncio_loop.run_in_executor( @@ -243,6 +267,14 @@ def execute( ) ) + load_actions = super().execute(context) + if load_actions is not None and len(autostart_actions) != 0: + return load_actions + autostart_actions + if load_actions is not None: + return load_actions + if len(autostart_actions) != 0: + return autostart_actions + def get_composable_node_load_request( composable_node_description: ComposableNode, diff --git a/launch_ros/launch_ros/descriptions/__init__.py b/launch_ros/launch_ros/descriptions/__init__.py index fd5d9beb..1d693013 100644 --- a/launch_ros/launch_ros/descriptions/__init__.py +++ b/launch_ros/launch_ros/descriptions/__init__.py @@ -14,6 +14,7 @@ """descriptions Module.""" +from .composable_lifecycle_node import ComposableLifecycleNode from .composable_node import ComposableNode from ..parameter_descriptions import Parameter from ..parameter_descriptions import ParameterFile @@ -22,6 +23,7 @@ __all__ = [ 'ComposableNode', + 'ComposableLifecycleNode', 'Parameter', 'ParameterFile', 'ParameterValue', diff --git a/launch_ros/launch_ros/descriptions/composable_lifecycle_node.py b/launch_ros/launch_ros/descriptions/composable_lifecycle_node.py new file mode 100644 index 00000000..ecd30fdc --- /dev/null +++ b/launch_ros/launch_ros/descriptions/composable_lifecycle_node.py @@ -0,0 +1,93 @@ +# Copyright 2025 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for a description of a ComposableLifecycleNode.""" + +from typing import List +from typing import Optional + +import launch +from launch.substitution import Substitution +from launch.utilities import perform_substitutions +from launch_ros.parameters_type import Parameters +from launch_ros.remap_rule_type import RemapRules +from launch_ros.utilities import LifecycleEventManager + +from .composable_node import ComposableNode + + +class ComposableLifecycleNode(ComposableNode): + """Describes a lifecycle node that can be loaded into a container with other nodes.""" + + def __init__( + self, *, + autostart: bool = False, + **kwargs + ) -> None: + """ + Initialize a ComposableLifecycleNode description. + + :param autostart: Whether to autostart lifecycle node in the activated state + """ + super().__init__(**kwargs) + + self.__autostart = autostart + self.__lifecycle_event_manager = None + self.__node_name = super().node_name + + def init_lifecycle_event_manager(self, context: launch.LaunchContext) -> None: + # LifecycleEventManager needs a pre-substitution node name + self.__node_name = perform_substitutions(context, self.node_name) + self.__lifecycle_event_manager = LifecycleEventManager(self) + self.__lifecycle_event_manager.setup_lifecycle_manager(context) + + @property + def package(self) -> List[Substitution]: + """Get node package name as a sequence of substitutions to be performed.""" + return super().package + + @property + def node_plugin(self) -> List[Substitution]: + """Get node plugin name as a sequence of substitutions to be performed.""" + return super().node_plugin + + @property + def node_name(self) -> Optional[List[Substitution]]: + """Get node name as a sequence of substitutions to be performed.""" + return self.__node_name + + @property + def node_namespace(self) -> Optional[List[Substitution]]: + """Get node namespace as a sequence of substitutions to be performed.""" + return super().node_namespace + + @property + def node_autostart(self): + """Getter for autostart.""" + return self.__autostart + + @property + def parameters(self) -> Optional[Parameters]: + """Get node parameter YAML files or dicts with substitutions to be performed.""" + return super().parameters + + @property + def remappings(self) -> Optional[RemapRules]: + """Get node remapping rules as (from, to) tuples with substitutions to be performed.""" + return super().remappings + + @property + def extra_arguments(self) -> Optional[Parameters]: + """Get container extra arguments YAML files or dicts with substitutions to be performed.""" + return super().extra_arguments diff --git a/launch_ros/launch_ros/descriptions/composable_node.py b/launch_ros/launch_ros/descriptions/composable_node.py index 8ad35c5c..f5508daa 100644 --- a/launch_ros/launch_ros/descriptions/composable_node.py +++ b/launch_ros/launch_ros/descriptions/composable_node.py @@ -23,7 +23,6 @@ from launch.frontend import Parser from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitution import Substitution -# from launch.utilities import ensure_argument_type from launch.utilities import normalize_to_list_of_substitutions from launch_ros.parameters_type import Parameters from launch_ros.parameters_type import SomeParameters diff --git a/launch_ros/launch_ros/event_handlers/on_state_transition.py b/launch_ros/launch_ros/event_handlers/on_state_transition.py index f9b381e1..9c5acdfe 100644 --- a/launch_ros/launch_ros/event_handlers/on_state_transition.py +++ b/launch_ros/launch_ros/event_handlers/on_state_transition.py @@ -23,7 +23,6 @@ from launch.some_entities_type import SomeEntitiesType from launch.some_substitutions_type import SomeSubstitutionsType -from ..actions import LifecycleNode from ..events.lifecycle import StateTransition @@ -34,7 +33,7 @@ def __init__( self, *, entities: SomeEntitiesType, - target_lifecycle_node: LifecycleNode = None, + target_lifecycle_node: Optional['LifecycleNode'] = None, # noqa: F821 transition: Optional[SomeSubstitutionsType] = None, start_state: Optional[SomeSubstitutionsType] = None, goal_state: Optional[SomeSubstitutionsType] = None, @@ -51,8 +50,19 @@ def __init__( If matcher is given, the other conditions are not considered. """ + from ..actions import LifecycleNode if not isinstance(target_lifecycle_node, (LifecycleNode, type(None))): - raise RuntimeError("OnStateTransition requires a 'LifecycleNode' action as the target") + raise RuntimeError('OnStateTransition requires a lifecycle enabled node as the target,' + ' target_lifecycle_node is not a lifecycle-enabled node type.') + + if ( + target_lifecycle_node and + hasattr(target_lifecycle_node, 'is_lifecycle_node') and + not target_lifecycle_node.is_lifecycle_node + ): + raise RuntimeError('OnStateTransition requires a lifecycle enabled node as the target,' + ' target_lifecycle_node is not lifecycle-enabled.') + # Handle optional matcher argument. self.__custom_matcher = matcher if self.__custom_matcher is None: diff --git a/launch_ros/launch_ros/utilities/__init__.py b/launch_ros/launch_ros/utilities/__init__.py index 8a530a7a..152479ac 100644 --- a/launch_ros/launch_ros/utilities/__init__.py +++ b/launch_ros/launch_ros/utilities/__init__.py @@ -19,6 +19,7 @@ """ from .evaluate_parameters import evaluate_parameters +from .lifecycle_event_manager import LifecycleEventManager from .namespace_utils import is_namespace_absolute from .namespace_utils import is_root_namespace from .namespace_utils import make_namespace_absolute @@ -37,6 +38,7 @@ 'get_node_name_count', 'is_namespace_absolute', 'is_root_namespace', + 'LifecycleEventManager', 'make_namespace_absolute', 'normalize_parameters', 'normalize_parameters_dict', diff --git a/launch_ros/launch_ros/utilities/lifecycle_event_manager.py b/launch_ros/launch_ros/utilities/lifecycle_event_manager.py new file mode 100644 index 00000000..9e384d03 --- /dev/null +++ b/launch_ros/launch_ros/utilities/lifecycle_event_manager.py @@ -0,0 +1,142 @@ +# Copyright 2024 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for the management of LifecycleNode events.""" + +import functools +import threading +from typing import cast + +import launch +import launch.logging + +import lifecycle_msgs.msg +import lifecycle_msgs.srv + +from ..events.lifecycle import ChangeState +from ..events.lifecycle import StateTransition + +from ..ros_adapters import get_ros_node + + +class LifecycleEventManager: + + def __init__(self, lifecycle_node) -> None: + """ + Construct a LifecycleEventManager utility. + + This utility emits some event(s) in certain circumstances: + + - :class:`launch.events.lifecycle.StateTransition`: + + - this event is emitted when a message is published to the + "//transition_event" topic, indicating the lifecycle + node represented by this action changed state + + This utility also handles some events related to lifecycle: + + - :class:`launch.events.lifecycle.ChangeState` + + - this event can be targeted to a single lifecycle node, or more than + one, or even all lifecycle nodes, and it requests the targeted nodes + to change state, see its documentation for more details. + + :param lifecycle_node: The lifecycle node to event manage. + """ + self.__logger = launch.logging.get_logger(__name__) + self.__lifecycle_node = lifecycle_node + self.__rclpy_subscription = None + self.__rclpy_change_state_client = None + + @property + def node_name(self): + return self.__lifecycle_node.node_name + + def _on_transition_event(self, context, msg): + try: + event = StateTransition(action=self.__lifecycle_node, msg=msg) + context.asyncio_loop.call_soon_threadsafe(lambda: context.emit_event_sync(event)) + except Exception as exc: + self.__logger.error( + "Exception in handling of 'lifecycle.msg.TransitionEvent': {}".format(exc)) + + def _call_change_state(self, request, context: launch.LaunchContext): + while not self.__rclpy_change_state_client.wait_for_service(timeout_sec=1.0): + if context.is_shutdown: + self.__logger.warning( + "Abandoning wait for the '{}' service, due to shutdown.".format( + self.__rclpy_change_state_client.srv_name), + ) + return + + # Asynchronously wait so that we can periodically check for shutdown. + event = threading.Event() + + def unblock(future): + nonlocal event + event.set() + + response_future = self.__rclpy_change_state_client.call_async(request) + response_future.add_done_callback(unblock) + + while not event.wait(1.0): + if context.is_shutdown: + self.__logger.warning( + "Abandoning wait for the '{}' service response, due to shutdown.".format( + self.__rclpy_change_state_client.srv_name), + ) + response_future.cancel() + return + + if response_future.exception() is not None: + raise response_future.exception() + response = response_future.result() + + if not response.success: + self.__logger.error( + "Failed to make transition '{}' for LifecycleNode '{}'".format( + ChangeState.valid_transitions[request.transition.id], + self.node_name, + ) + ) + + def _on_change_state_event(self, context: launch.LaunchContext) -> None: + typed_event = cast(ChangeState, context.locals.event) + if not typed_event.lifecycle_node_matcher(self.__lifecycle_node): + return None + request = lifecycle_msgs.srv.ChangeState.Request() + request.transition.id = typed_event.transition_id + context.add_completion_future( + context.asyncio_loop.run_in_executor(None, self._call_change_state, request, context)) + + def setup_lifecycle_manager(self, context: launch.LaunchContext) -> None: + node = get_ros_node(context) + + # Create a subscription to monitor the state changes of the subprocess. + self.__rclpy_subscription = node.create_subscription( + lifecycle_msgs.msg.TransitionEvent, + '{}/transition_event'.format(self.node_name), + functools.partial(self._on_transition_event, context), + 10) + + # Create a service client to change state on demand. + self.__rclpy_change_state_client = node.create_client( + lifecycle_msgs.srv.ChangeState, + '{}/change_state'.format(self.node_name)) + + # Register an event handler to change states on a ChangeState lifecycle event. + context.register_event_handler(launch.EventHandler( + matcher=lambda event: isinstance(event, ChangeState), + entities=[launch.actions.OpaqueFunction(function=self._on_change_state_event)], + )) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py index d195a57f..ca9b7f01 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py @@ -41,6 +41,13 @@ def test_lifecycle_node_constructor(): name='my_node', namespace='my_ns', ) + LifecycleNode( + package='asd', + executable='bsd', + name='my_node', + namespace='my_ns', + autostart=True, + ) def test_node_name():