|
| 1 | +# Copyright (c) 2025 Leander Stephen Desouza |
| 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 | +import os |
| 16 | + |
| 17 | +from ament_index_python.packages import get_package_share_directory |
| 18 | +from launch import LaunchDescription |
| 19 | +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable |
| 20 | +from launch.conditions import IfCondition |
| 21 | +from launch.substitutions import LaunchConfiguration, PythonExpression |
| 22 | +from launch_ros.actions import LoadComposableNodes, Node, SetParameter |
| 23 | +from launch_ros.descriptions import ComposableNode, ParameterFile |
| 24 | +from nav2_common.launch import RewrittenYaml |
| 25 | + |
| 26 | + |
| 27 | +def generate_launch_description() -> LaunchDescription: |
| 28 | + # Get the launch directory |
| 29 | + bringup_dir = get_package_share_directory('nav2_bringup') |
| 30 | + |
| 31 | + namespace = LaunchConfiguration('namespace') |
| 32 | + speed_mask_yaml_file = LaunchConfiguration('speed_mask') |
| 33 | + use_sim_time = LaunchConfiguration('use_sim_time') |
| 34 | + autostart = LaunchConfiguration('autostart') |
| 35 | + params_file = LaunchConfiguration('params_file') |
| 36 | + use_composition = LaunchConfiguration('use_composition') |
| 37 | + container_name = LaunchConfiguration('container_name') |
| 38 | + container_name_full = (namespace, '/', container_name) |
| 39 | + use_respawn = LaunchConfiguration('use_respawn') |
| 40 | + use_speed_zones = LaunchConfiguration('use_speed_zones') |
| 41 | + log_level = LaunchConfiguration('log_level') |
| 42 | + |
| 43 | + lifecycle_nodes = ['speed_filter_mask_server', 'speed_costmap_filter_info_server'] |
| 44 | + |
| 45 | + # Map fully qualified names to relative ones so the node's namespace can be prepended. |
| 46 | + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] |
| 47 | + |
| 48 | + configured_params = ParameterFile( |
| 49 | + RewrittenYaml( |
| 50 | + source_file=params_file, |
| 51 | + root_key=namespace, |
| 52 | + param_rewrites={}, |
| 53 | + convert_types=True, |
| 54 | + ), |
| 55 | + allow_substs=True, |
| 56 | + ) |
| 57 | + |
| 58 | + stdout_linebuf_envvar = SetEnvironmentVariable( |
| 59 | + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' |
| 60 | + ) |
| 61 | + |
| 62 | + declare_namespace_cmd = DeclareLaunchArgument( |
| 63 | + 'namespace', default_value='', description='Top-level namespace' |
| 64 | + ) |
| 65 | + |
| 66 | + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( |
| 67 | + 'speed_mask', |
| 68 | + default_value='', |
| 69 | + description='Full path to speed mask yaml file to load', |
| 70 | + ) |
| 71 | + |
| 72 | + declare_use_sim_time_cmd = DeclareLaunchArgument( |
| 73 | + 'use_sim_time', |
| 74 | + default_value='false', |
| 75 | + description='Use simulation (Gazebo) clock if true', |
| 76 | + ) |
| 77 | + |
| 78 | + declare_params_file_cmd = DeclareLaunchArgument( |
| 79 | + 'params_file', |
| 80 | + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), |
| 81 | + description='Full path to the ROS2 parameters file to use for all launched nodes', |
| 82 | + ) |
| 83 | + |
| 84 | + declare_use_composition_cmd = DeclareLaunchArgument( |
| 85 | + 'use_composition', |
| 86 | + default_value='False', |
| 87 | + description='Use composed bringup if True', |
| 88 | + ) |
| 89 | + |
| 90 | + declare_container_name_cmd = DeclareLaunchArgument( |
| 91 | + 'container_name', |
| 92 | + default_value='nav2_container', |
| 93 | + description='the name of container that nodes will load in if use composition', |
| 94 | + ) |
| 95 | + |
| 96 | + declare_use_respawn_cmd = DeclareLaunchArgument( |
| 97 | + 'use_respawn', |
| 98 | + default_value='False', |
| 99 | + description='Whether to respawn if a node crashes. Applied when composition is disabled.', |
| 100 | + ) |
| 101 | + |
| 102 | + declare_use_speed_zones_cmd = DeclareLaunchArgument( |
| 103 | + 'use_speed_zones', default_value='True', |
| 104 | + description='Whether to enable speed zones or not' |
| 105 | + ) |
| 106 | + |
| 107 | + declare_log_level_cmd = DeclareLaunchArgument( |
| 108 | + 'log_level', default_value='info', description='log level' |
| 109 | + ) |
| 110 | + |
| 111 | + load_nodes = GroupAction( |
| 112 | + condition=IfCondition(PythonExpression(['not ', use_composition])), |
| 113 | + actions=[ |
| 114 | + SetParameter('use_sim_time', use_sim_time), |
| 115 | + Node( |
| 116 | + condition=IfCondition(use_speed_zones), |
| 117 | + package='nav2_map_server', |
| 118 | + executable='map_server', |
| 119 | + name='speed_filter_mask_server', |
| 120 | + output='screen', |
| 121 | + respawn=use_respawn, |
| 122 | + respawn_delay=2.0, |
| 123 | + parameters=[configured_params, {'yaml_filename': speed_mask_yaml_file}], |
| 124 | + arguments=['--ros-args', '--log-level', log_level], |
| 125 | + remappings=remappings, |
| 126 | + ), |
| 127 | + Node( |
| 128 | + condition=IfCondition(use_speed_zones), |
| 129 | + package='nav2_map_server', |
| 130 | + executable='costmap_filter_info_server', |
| 131 | + name='speed_costmap_filter_info_server', |
| 132 | + output='screen', |
| 133 | + respawn=use_respawn, |
| 134 | + respawn_delay=2.0, |
| 135 | + parameters=[configured_params], |
| 136 | + arguments=['--ros-args', '--log-level', log_level], |
| 137 | + remappings=remappings, |
| 138 | + ), |
| 139 | + Node( |
| 140 | + package='nav2_lifecycle_manager', |
| 141 | + executable='lifecycle_manager', |
| 142 | + name='lifecycle_manager_speed_zone', |
| 143 | + output='screen', |
| 144 | + arguments=['--ros-args', '--log-level', log_level], |
| 145 | + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], |
| 146 | + ), |
| 147 | + ], |
| 148 | + ) |
| 149 | + # LoadComposableNode for map server twice depending if we should use the |
| 150 | + # value of map from a CLI or launch default or user defined value in the |
| 151 | + # yaml configuration file. They are separated since the conditions |
| 152 | + # currently only work on the LoadComposableNodes commands and not on the |
| 153 | + # ComposableNode node function itself |
| 154 | + load_composable_nodes = GroupAction( |
| 155 | + condition=IfCondition(use_composition), |
| 156 | + actions=[ |
| 157 | + SetParameter('use_sim_time', use_sim_time), |
| 158 | + LoadComposableNodes( |
| 159 | + target_container=container_name_full, |
| 160 | + condition=IfCondition(use_speed_zones), |
| 161 | + composable_node_descriptions=[ |
| 162 | + ComposableNode( |
| 163 | + package='nav2_map_server', |
| 164 | + plugin='nav2_map_server::MapServer', |
| 165 | + name='speed_filter_mask_server', |
| 166 | + parameters=[ |
| 167 | + configured_params, |
| 168 | + {'yaml_filename': speed_mask_yaml_file} |
| 169 | + ], |
| 170 | + remappings=remappings, |
| 171 | + ), |
| 172 | + ComposableNode( |
| 173 | + package='nav2_map_server', |
| 174 | + plugin='nav2_map_server::CostmapFilterInfoServer', |
| 175 | + name='speed_costmap_filter_info_server', |
| 176 | + parameters=[configured_params], |
| 177 | + remappings=remappings, |
| 178 | + ), |
| 179 | + ], |
| 180 | + ), |
| 181 | + |
| 182 | + LoadComposableNodes( |
| 183 | + target_container=container_name_full, |
| 184 | + composable_node_descriptions=[ |
| 185 | + ComposableNode( |
| 186 | + package='nav2_lifecycle_manager', |
| 187 | + plugin='nav2_lifecycle_manager::LifecycleManager', |
| 188 | + name='lifecycle_manager_speed_zone', |
| 189 | + parameters=[ |
| 190 | + {'autostart': autostart, 'node_names': lifecycle_nodes} |
| 191 | + ], |
| 192 | + ), |
| 193 | + ], |
| 194 | + ), |
| 195 | + ], |
| 196 | + ) |
| 197 | + |
| 198 | + # Create the launch description and populate |
| 199 | + ld = LaunchDescription() |
| 200 | + |
| 201 | + # Set environment variables |
| 202 | + ld.add_action(stdout_linebuf_envvar) |
| 203 | + |
| 204 | + # Declare the launch options |
| 205 | + ld.add_action(declare_namespace_cmd) |
| 206 | + ld.add_action(declare_speed_mask_yaml_cmd) |
| 207 | + ld.add_action(declare_use_sim_time_cmd) |
| 208 | + ld.add_action(declare_params_file_cmd) |
| 209 | + ld.add_action(declare_use_composition_cmd) |
| 210 | + ld.add_action(declare_container_name_cmd) |
| 211 | + ld.add_action(declare_use_respawn_cmd) |
| 212 | + ld.add_action(declare_use_speed_zones_cmd) |
| 213 | + ld.add_action(declare_log_level_cmd) |
| 214 | + |
| 215 | + # Add the actions to launch all of the map modifier nodes |
| 216 | + ld.add_action(load_nodes) |
| 217 | + ld.add_action(load_composable_nodes) |
| 218 | + |
| 219 | + return ld |
0 commit comments