|
| 1 | +# Copyright 2019 Open Source Robotics Foundation, Inc. |
| 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 | +"""Module for the LoadComposableNodes action.""" |
| 16 | + |
| 17 | +from typing import List |
| 18 | +from typing import Optional |
| 19 | + |
| 20 | +import composition_interfaces.srv |
| 21 | + |
| 22 | +from launch.action import Action |
| 23 | +from launch.actions import RegisterEventHandler |
| 24 | +from launch.event_handlers.on_process_start import OnProcessStart |
| 25 | +from launch.events.process import ProcessStarted |
| 26 | +from launch.launch_context import LaunchContext |
| 27 | +import launch.logging |
| 28 | +from launch.utilities import ensure_argument_type |
| 29 | +from launch.utilities import perform_substitutions |
| 30 | + |
| 31 | +from .composable_node_container import ComposableNodeContainer |
| 32 | + |
| 33 | +from ..descriptions import ComposableNode |
| 34 | +from ..utilities import evaluate_parameters |
| 35 | +from ..utilities import to_parameters_list |
| 36 | + |
| 37 | + |
| 38 | +class LoadComposableNodes(Action): |
| 39 | + """Action that loads composable ROS nodes into a running container.""" |
| 40 | + |
| 41 | + def __init__( |
| 42 | + self, |
| 43 | + *, |
| 44 | + composable_node_descriptions: List[ComposableNode], |
| 45 | + target_container: ComposableNodeContainer, |
| 46 | + **kwargs, |
| 47 | + ) -> None: |
| 48 | + """ |
| 49 | + Construct a LoadComposableNodes action. |
| 50 | +
|
| 51 | + The container node is expected to provide a `~/_container/load_node` service for |
| 52 | + loading purposes. |
| 53 | + Loading will be performed sequentially. |
| 54 | +
|
| 55 | + :param composable_node_descriptions: descriptions of composable nodes to be loaded |
| 56 | + :param target_container: the container to load the nodes into |
| 57 | + """ |
| 58 | + ensure_argument_type( |
| 59 | + target_container, |
| 60 | + ComposableNodeContainer, |
| 61 | + 'target_container', |
| 62 | + 'LoadComposableNodes' |
| 63 | + ) |
| 64 | + super().__init__(**kwargs) |
| 65 | + self.__composable_node_descriptions = composable_node_descriptions |
| 66 | + self.__target_container = target_container |
| 67 | + self.__logger = launch.logging.get_logger(__name__) |
| 68 | + |
| 69 | + def _load_node( |
| 70 | + self, |
| 71 | + composable_node_description: ComposableNode, |
| 72 | + context: LaunchContext |
| 73 | + ) -> None: |
| 74 | + """ |
| 75 | + Load node synchronously. |
| 76 | +
|
| 77 | + :param composable_node_description: description of composable node to be loaded |
| 78 | + :param context: current launch context |
| 79 | + """ |
| 80 | + while not self.__rclpy_load_node_client.wait_for_service(timeout_sec=1.0): |
| 81 | + if context.is_shutdown: |
| 82 | + self.__logger.warning( |
| 83 | + "Abandoning wait for the '{}' service, due to shutdown.".format( |
| 84 | + self.__rclpy_load_node_client.srv_name |
| 85 | + ) |
| 86 | + ) |
| 87 | + return |
| 88 | + request = composition_interfaces.srv.LoadNode.Request() |
| 89 | + request.package_name = perform_substitutions( |
| 90 | + context, composable_node_description.package |
| 91 | + ) |
| 92 | + request.plugin_name = perform_substitutions( |
| 93 | + context, composable_node_description.node_plugin |
| 94 | + ) |
| 95 | + if composable_node_description.node_name is not None: |
| 96 | + request.node_name = perform_substitutions( |
| 97 | + context, composable_node_description.node_name |
| 98 | + ) |
| 99 | + if composable_node_description.node_namespace is not None: |
| 100 | + request.node_namespace = perform_substitutions( |
| 101 | + context, composable_node_description.node_namespace |
| 102 | + ) |
| 103 | + # request.log_level = perform_substitutions(context, node_description.log_level) |
| 104 | + if composable_node_description.remappings is not None: |
| 105 | + for from_, to in composable_node_description.remappings: |
| 106 | + request.remap_rules.append('{}:={}'.format( |
| 107 | + perform_substitutions(context, list(from_)), |
| 108 | + perform_substitutions(context, list(to)), |
| 109 | + )) |
| 110 | + if composable_node_description.parameters is not None: |
| 111 | + request.parameters = [ |
| 112 | + param.to_parameter_msg() for param in to_parameters_list( |
| 113 | + context, evaluate_parameters( |
| 114 | + context, composable_node_description.parameters |
| 115 | + ) |
| 116 | + ) |
| 117 | + ] |
| 118 | + if composable_node_description.extra_arguments is not None: |
| 119 | + request.extra_arguments = [ |
| 120 | + param.to_parameter_msg() for param in to_parameters_list( |
| 121 | + context, evaluate_parameters( |
| 122 | + context, composable_node_description.extra_arguments |
| 123 | + ) |
| 124 | + ) |
| 125 | + ] |
| 126 | + response = self.__rclpy_load_node_client.call(request) |
| 127 | + if not response.success: |
| 128 | + self.__logger.error( |
| 129 | + "Failed to load node '{}' of type '{}' in container '{}': {}".format( |
| 130 | + response.full_node_name if response.full_node_name else request.node_name, |
| 131 | + request.plugin_name, self.__target_container.node_name, response.error_message |
| 132 | + ) |
| 133 | + ) |
| 134 | + self.__logger.info("Loaded node '{}' in container '{}'".format( |
| 135 | + response.full_node_name, self.__target_container.node_name |
| 136 | + )) |
| 137 | + |
| 138 | + def _load_in_sequence( |
| 139 | + self, |
| 140 | + composable_node_descriptions: List[ComposableNode], |
| 141 | + context: LaunchContext |
| 142 | + ) -> None: |
| 143 | + """ |
| 144 | + Load composable nodes sequentially. |
| 145 | +
|
| 146 | + :param composable_node_descriptions: descriptions of composable nodes to be loaded |
| 147 | + :param context: current launch context |
| 148 | + """ |
| 149 | + next_composable_node_description = composable_node_descriptions[0] |
| 150 | + composable_node_descriptions = composable_node_descriptions[1:] |
| 151 | + self._load_node(next_composable_node_description, context) |
| 152 | + if len(composable_node_descriptions) > 0: |
| 153 | + context.add_completion_future( |
| 154 | + context.asyncio_loop.run_in_executor( |
| 155 | + None, self._load_in_sequence, composable_node_descriptions, context |
| 156 | + ) |
| 157 | + ) |
| 158 | + |
| 159 | + def _on_container_start( |
| 160 | + self, |
| 161 | + event: ProcessStarted, |
| 162 | + context: LaunchContext |
| 163 | + ) -> Optional[List[Action]]: |
| 164 | + """Load nodes on container process start.""" |
| 165 | + self._load_in_sequence(self.__composable_node_descriptions, context) |
| 166 | + return None |
| 167 | + |
| 168 | + def execute( |
| 169 | + self, |
| 170 | + context: LaunchContext |
| 171 | + ) -> Optional[List[Action]]: |
| 172 | + """Execute the action.""" |
| 173 | + # Create a client to load nodes in the target container. |
| 174 | + self.__rclpy_load_node_client = context.locals.launch_ros_node.create_client( |
| 175 | + composition_interfaces.srv.LoadNode, '{}/_container/load_node'.format( |
| 176 | + self.__target_container.node_name |
| 177 | + ) |
| 178 | + ) |
| 179 | + # Perform load action once the container has started. |
| 180 | + return [RegisterEventHandler( |
| 181 | + event_handler=OnProcessStart( |
| 182 | + target_action=self.__target_container, |
| 183 | + on_start=self._on_container_start, |
| 184 | + ) |
| 185 | + )] |
0 commit comments