|
| 1 | +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES |
| 2 | +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. |
| 3 | +# |
| 4 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | +# you may not use this file except in compliance with the License. |
| 6 | +# You may obtain a copy of the License at |
| 7 | +# |
| 8 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | +# |
| 10 | +# Unless required by applicable law or agreed to in writing, software |
| 11 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | +# See the License for the specific language governing permissions and |
| 14 | +# limitations under the License. |
| 15 | +# |
| 16 | +# SPDX-License-Identifier: Apache-2.0 |
| 17 | + |
| 18 | + |
| 19 | +import isaac_ros_launch_utils as lu |
| 20 | +import isaac_ros_launch_utils.all_types as lut |
| 21 | +from nav2_common.launch import RewrittenYaml |
| 22 | + |
| 23 | + |
| 24 | +# Override parameters for carter in MEGA using static map costmap only |
| 25 | +def set_override_parameters_for_mega(args: lu.ArgumentContainer) -> list[lut.Action]: |
| 26 | + local_costmap_plugins = ['static_map_layer', 'inflation_layer'] |
| 27 | + global_costmap_plugins = ['static_map_layer', 'inflation_layer'] |
| 28 | + |
| 29 | + print(f'Using local costmap plugins: {local_costmap_plugins}') |
| 30 | + print(f'Using global costmap plugins: {global_costmap_plugins}') |
| 31 | + actions: list[lut.Action] = [] |
| 32 | + actions.append( |
| 33 | + lu.set_parameter( |
| 34 | + namespace='/local_costmap/local_costmap', |
| 35 | + parameter='plugins', |
| 36 | + value=local_costmap_plugins, |
| 37 | + )) |
| 38 | + actions.append( |
| 39 | + lu.set_parameter( |
| 40 | + namespace='/local_costmap/local_costmap', |
| 41 | + parameter='global_frame', |
| 42 | + value='map', |
| 43 | + )) |
| 44 | + actions.append( |
| 45 | + lu.set_parameter( |
| 46 | + namespace='/global_costmap/global_costmap', |
| 47 | + parameter='plugins', |
| 48 | + value=global_costmap_plugins, |
| 49 | + )) |
| 50 | + |
| 51 | + actions.append( |
| 52 | + lu.set_parameter( |
| 53 | + namespace='/bt_navigator', |
| 54 | + parameter='odom_topic', |
| 55 | + value='/chassis/odom', |
| 56 | + )) |
| 57 | + actions.append( |
| 58 | + lu.set_parameter( |
| 59 | + namespace='/velocity_smoother', |
| 60 | + parameter='odom_topic', |
| 61 | + value='/chassis/odom', |
| 62 | + )) |
| 63 | + actions.append( |
| 64 | + lu.set_parameter( |
| 65 | + namespace='/controller_server', |
| 66 | + parameter='odom_topic', |
| 67 | + value='/chassis/odom', |
| 68 | + )) |
| 69 | + |
| 70 | + return actions |
| 71 | + |
| 72 | + |
| 73 | +def generate_launch_description() -> lut.LaunchDescription: |
| 74 | + args = lu.ArgumentContainer() |
| 75 | + args.add_arg('rosbag', 'None', cli=True) |
| 76 | + args.add_arg('mode', 'mega', cli=True) |
| 77 | + args.add_arg('navigation_parameters_path', |
| 78 | + lu.get_path('isaac_ros_mega_controller', |
| 79 | + 'params/nova_carter_navigation_mega.yaml'), |
| 80 | + cli=True) |
| 81 | + args.add_arg('map_yaml_path', 'None', cli=True) |
| 82 | + args.add_arg('enable_navigation', True, cli=True) |
| 83 | + args.add_arg('enable_mission_client', False, cli=True) |
| 84 | + args.add_arg('enable_3d_lidar_costmap', False, cli=True) |
| 85 | + args.add_arg('enable_2d_lidar_costmap', True, cli=True) |
| 86 | + args.add_arg('enable_nvblox_costmap', True, cli=True) |
| 87 | + args.add_arg('enable_wheel_odometry', True, cli=True) |
| 88 | + args.add_arg('enable_3d_lidar_localization', True, cli=True) |
| 89 | + args.add_arg('enable_visual_localization', False, cli=True) |
| 90 | + args.add_arg('stereo_camera_configuration', |
| 91 | + default='front_configuration', |
| 92 | + choices=['no_cameras', 'front_configuration', |
| 93 | + 'front_left_right_configuration', 'front_driver_rectify', |
| 94 | + 'front_left_right_vslam_configuration'], |
| 95 | + cli=True) |
| 96 | + args.add_arg('enabled_fisheye_cameras', '', cli=True) |
| 97 | + args.add_arg('use_foxglove_whitelist', True, cli=True) |
| 98 | + args.add_arg('init_pose_x', '', cli=True) |
| 99 | + args.add_arg('init_pose_y', '', cli=True) |
| 100 | + args.add_arg('init_pose_yaw', '', cli=True) |
| 101 | + args.add_arg('enable_controller', False, cli=True) |
| 102 | + args.add_arg('enable_metrics', False, cli=True) |
| 103 | + args.add_arg('metrics_interval', 15.0, cli=True) |
| 104 | + args.add_arg('waypoints', 'None', cli=True) |
| 105 | + args.add_arg('type_negotiation_duration_s', lu.get_default_negotiation_time(), cli=True) |
| 106 | + |
| 107 | + enable_global_navigation = lu.is_valid(args.map_yaml_path) |
| 108 | + |
| 109 | + # map_only is true if nvblox and lidars are not enabled |
| 110 | + enable_lidar = lut.OrSubstitution( |
| 111 | + args.enable_3d_lidar_costmap, args.enable_2d_lidar_costmap) |
| 112 | + map_only = lut.AndSubstitution( |
| 113 | + lut.NotSubstitution(enable_lidar), lut.NotSubstitution(args.enable_nvblox_costmap)) |
| 114 | + |
| 115 | + actions = args.get_launch_actions() |
| 116 | + actions.append(lut.SetParameter('type_negotiation_duration_s', |
| 117 | + args.type_negotiation_duration_s)) |
| 118 | + actions.append( |
| 119 | + lu.log_info(['Using type negotiation duration: ', args.type_negotiation_duration_s])) |
| 120 | + actions.append(lut.SetParameter('use_sim_time', True)) |
| 121 | + |
| 122 | + # NOTE: If running in sim mode we do mapping in another frame to not clash with ground truth |
| 123 | + # coming out of Isaac Sim. |
| 124 | + # Set global frame to odom_vslam when: |
| 125 | + # - running local navigation in Isaac Sim (i.e. no global map passed) |
| 126 | + run_local_navigation_in_sim = lu.is_false(enable_global_navigation) |
| 127 | + global_frame = lu.if_else_substitution(run_local_navigation_in_sim, 'odom_vslam', 'odom') |
| 128 | + |
| 129 | + # NOTE: If running in sim mode we are setting the reliability policy of vslam to reliable |
| 130 | + # as we were experiencing issues with the best effort policy |
| 131 | + # (i.e. not able to synchronize input messages due to dropped images). |
| 132 | + vslam_image_qos = 'DEFAULT' |
| 133 | + |
| 134 | + # Add the navigation. |
| 135 | + actions.append( |
| 136 | + lu.include( |
| 137 | + 'nova_carter_bringup', |
| 138 | + 'launch/include/navigation_include.launch.py', |
| 139 | + launch_arguments={ |
| 140 | + 'global_frame': global_frame, |
| 141 | + 'vslam_image_qos': vslam_image_qos, |
| 142 | + 'init_pose_x': args.init_pose_x, |
| 143 | + 'init_pose_y': args.init_pose_y, |
| 144 | + 'init_pose_yaw': args.init_pose_yaw, |
| 145 | + 'set_init_pose': True, |
| 146 | + }, |
| 147 | + condition=lut.IfCondition(lut.NotSubstitution(map_only)), |
| 148 | + )) |
| 149 | + |
| 150 | + actions.append( |
| 151 | + lu.include( |
| 152 | + 'isaac_ros_perceptor_bringup', |
| 153 | + 'launch/tools/visualization.launch.py', |
| 154 | + launch_arguments={'use_foxglove_whitelist': args.use_foxglove_whitelist}, |
| 155 | + condition=lut.IfCondition(lut.NotSubstitution(map_only)), |
| 156 | + )) |
| 157 | + |
| 158 | + controller = lut.Node( |
| 159 | + package='isaac_ros_mega_controller', |
| 160 | + executable='isaac_ros_mega_controller', |
| 161 | + name='isaac_ros_mega_controller', |
| 162 | + output='screen', |
| 163 | + parameters=[{'waypoints': args.waypoints}], |
| 164 | + condition=lut.IfCondition(args.enable_controller), |
| 165 | + ) |
| 166 | + actions.append(controller) |
| 167 | + |
| 168 | + # We use a negative niceness to increase the priority. |
| 169 | + actions.append(lu.component_container('nova_container', prefix='nice -n -10')) |
| 170 | + # A separate container is needed for Nav2 because ROS2 has a race condition for actions when |
| 171 | + # using non-isolated containers. |
| 172 | + # Nav container has to be added last, else Nav2 parameters are not picked up properly. |
| 173 | + actions.append(lu.component_container('navigation_container', container_type='isolated')) |
| 174 | + |
| 175 | + # If only static map is used, override parameter file to use static map |
| 176 | + # for local and global costmap, launch nav2 navigation and map server, |
| 177 | + # pulish map->odom tf. |
| 178 | + navigation_parameters_path = lu.if_else_substitution( |
| 179 | + map_only, |
| 180 | + RewrittenYaml(source_file=args.navigation_parameters_path, |
| 181 | + root_key='', |
| 182 | + param_rewrites={'yaml_filename': args.map_yaml_path}, |
| 183 | + convert_types=True), |
| 184 | + args.navigation_parameters_path |
| 185 | + ) |
| 186 | + |
| 187 | + actions.append(lut.SetParametersFromFile(navigation_parameters_path)) |
| 188 | + actions.append( |
| 189 | + args.add_opaque_function( |
| 190 | + set_override_parameters_for_mega, |
| 191 | + condition=lut.IfCondition(map_only), |
| 192 | + )) |
| 193 | + |
| 194 | + actions.append( |
| 195 | + lu.include( |
| 196 | + 'nav2_bringup', |
| 197 | + 'launch/navigation_launch.py', |
| 198 | + launch_arguments={ |
| 199 | + 'use_sim_time': True, |
| 200 | + 'params_file': navigation_parameters_path |
| 201 | + }, |
| 202 | + condition=lut.IfCondition(map_only), |
| 203 | + )) |
| 204 | + |
| 205 | + actions.append( |
| 206 | + lu.include( |
| 207 | + 'isaac_ros_mega_node_monitor', |
| 208 | + 'launch/isaac_ros_mega_node_monitor.launch.py', |
| 209 | + launch_arguments={ |
| 210 | + 'enable_metrics': args.enable_metrics, |
| 211 | + 'metrics_interval': args.metrics_interval, |
| 212 | + 'enable_3d_lidar_costmap': args.enable_3d_lidar_costmap |
| 213 | + }, |
| 214 | + condition=lut.IfCondition(args.enable_navigation) |
| 215 | + )) |
| 216 | + |
| 217 | + actions.append( |
| 218 | + lut.Node( |
| 219 | + package='tf2_ros', |
| 220 | + executable='static_transform_publisher', |
| 221 | + name='static_transform_publisher_map_odom', |
| 222 | + arguments=[args.init_pose_x, args.init_pose_y, |
| 223 | + '0', args.init_pose_yaw, '0', '0', 'map', 'odom'], |
| 224 | + output='screen', |
| 225 | + condition=lut.IfCondition(map_only), |
| 226 | + ) |
| 227 | + ) |
| 228 | + |
| 229 | + actions.append( |
| 230 | + lu.include( |
| 231 | + 'nova_carter_description', |
| 232 | + 'launch/nova_carter_description.launch.py', |
| 233 | + condition=lut.IfCondition(map_only), |
| 234 | + )) |
| 235 | + |
| 236 | + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] |
| 237 | + |
| 238 | + actions.append( |
| 239 | + lut.Node( |
| 240 | + package='nav2_map_server', |
| 241 | + executable='map_server', |
| 242 | + name='map_server', |
| 243 | + output='screen', |
| 244 | + respawn_delay=2.0, |
| 245 | + parameters=[{'yaml_filename': args.map_yaml_path}], |
| 246 | + arguments=['--ros-args', '--log-level', 'info'], |
| 247 | + remappings=remappings, |
| 248 | + condition=lut.IfCondition(map_only), |
| 249 | + )) |
| 250 | + |
| 251 | + actions.append( |
| 252 | + lut.Node( |
| 253 | + package='nav2_lifecycle_manager', |
| 254 | + executable='lifecycle_manager', |
| 255 | + name='lifecycle_manager_localization', |
| 256 | + output='screen', |
| 257 | + arguments=['--ros-args', '--log-level', 'info'], |
| 258 | + parameters=[{'autostart': True}, {'node_names': ['map_server']}], |
| 259 | + condition=lut.IfCondition(map_only), |
| 260 | + )) |
| 261 | + |
| 262 | + actions.append( |
| 263 | + lu.include( |
| 264 | + 'isaac_ros_vda5050_nav2_client_bringup', |
| 265 | + 'launch/isaac_ros_vda5050_client.launch.py', |
| 266 | + launch_arguments={ |
| 267 | + 'docking_server_enabled': False, |
| 268 | + }, |
| 269 | + condition=lut.IfCondition( |
| 270 | + lut.AndSubstitution(args.enable_mission_client, map_only) |
| 271 | + ), |
| 272 | + )) |
| 273 | + |
| 274 | + return lut.LaunchDescription(actions) |
0 commit comments