Nav2 Navigation Stack - kilted  kilted
ROS 2 Navigation Stack
route_example_launch.py
1 # Copyright (c) 2025 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 import os
16 from pathlib import Path
17 import tempfile
18 
19 from ament_index_python.packages import get_package_share_directory
20 from launch import LaunchDescription
21 from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
22  IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
23 from launch.conditions import IfCondition
24 from launch.event_handlers import OnShutdown
25 from launch.launch_description_sources import PythonLaunchDescriptionSource
26 from launch.substitutions import Command, LaunchConfiguration, PythonExpression
27 from launch_ros.actions import Node
28 
29 # Define local map types
30 MAP_POSES_DICT = {
31  'depot': {
32  'x': -8.00, 'y': 0.00, 'z': 0.01,
33  'R': 0.00, 'P': 0.00, 'Y': 0.00
34  },
35  'warehouse': {
36  'x': 2.00, 'y': -19.65, 'z': 0.01,
37  'R': 0.00, 'P': 0.00, 'Y': 0.00
38  }
39 }
40 
41 ROUTE_POSES_DICT = {
42  'start': {
43  'depot': {
44  'x': 7.5, 'y': 7.5, 'yaw': 0.00 # 3rd node
45  },
46  'warehouse': {
47  'x': 2.00, 'y': -19.65, 'yaw': 0.00 # 0th node
48  }
49  },
50  'goal': {
51  'depot': {
52  'x': 20.12, 'y': 11.83, 'yaw': 0.00 # 13th node
53  },
54  'warehouse': {
55  'x': -13.5, 'y': -3.15, 'yaw': 0.00 # 61st node
56  }
57  }
58 }
59 MAP_TYPE = 'depot' # Change this to 'warehouse' for warehouse map
60 
61 
62 def generate_launch_description() -> LaunchDescription:
63  nav2_bringup_dir = get_package_share_directory('nav2_bringup')
64  sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
65  desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
66 
67  robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro')
68  world = os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf')
69  map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}.yaml')
70  graph_filepath = os.path.join(
71  nav2_bringup_dir, 'graphs', f'{MAP_TYPE}_graph.geojson')
72 
73  # Launch configuration variables
74  headless = LaunchConfiguration('headless')
75  use_keepout_zones = LaunchConfiguration('use_keepout_zones')
76  use_speed_zones = LaunchConfiguration('use_speed_zones')
77  keepout_mask_yaml_file = LaunchConfiguration('keepout_mask')
78  speed_mask_yaml_file = LaunchConfiguration('speed_mask')
79 
80  # Declare the launch arguments
81  declare_headless_cmd = DeclareLaunchArgument(
82  'headless', default_value='False', description='Whether to execute gzclient)'
83  )
84  declare_use_keepout_zones_cmd = DeclareLaunchArgument(
85  'use_keepout_zones', default_value='True',
86  description='Whether to enable keepout zones or not'
87  )
88 
89  declare_use_speed_zones_cmd = DeclareLaunchArgument(
90  'use_speed_zones', default_value='True',
91  description='Whether to enable speed zones or not'
92  )
93  declare_keepout_mask_yaml_cmd = DeclareLaunchArgument(
94  'keepout_mask',
95  default_value=os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}_keepout.yaml'),
96  description='Full path to keepout mask file to load',
97  )
98  declare_speed_mask_yaml_cmd = DeclareLaunchArgument(
99  'speed_mask',
100  default_value=os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}_speed.yaml'),
101  description='Full path to speed mask file to load',
102  )
103 
104  # start the simulation
105  world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
106  world_sdf_xacro = ExecuteProcess(
107  cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
108  start_gazebo_server_cmd = IncludeLaunchDescription(
109  PythonLaunchDescriptionSource(
110  os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
111  'gz_sim.launch.py')),
112  launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
113 
114  remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
115  on_shutdown=[
116  OpaqueFunction(function=lambda _: os.remove(world_sdf))
117  ]))
118 
119  set_env_vars_resources = AppendEnvironmentVariable(
120  'GZ_SIM_RESOURCE_PATH',
121  os.path.join(sim_dir, 'worlds'))
122  start_gazebo_client_cmd = IncludeLaunchDescription(
123  PythonLaunchDescriptionSource(
124  os.path.join(get_package_share_directory('ros_gz_sim'),
125  'launch',
126  'gz_sim.launch.py')
127  ),
128  condition=IfCondition(PythonExpression(
129  ['not ', headless])),
130  launch_arguments={'gz_args': ['-v4 -g ']}.items(),
131  )
132 
133  spawn_robot_cmd = IncludeLaunchDescription(
134  PythonLaunchDescriptionSource(
135  os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')),
136  launch_arguments={'use_sim_time': 'True',
137  'robot_sdf': robot_sdf,
138  'x_pose': str(MAP_POSES_DICT[MAP_TYPE]['x']),
139  'y_pose': str(MAP_POSES_DICT[MAP_TYPE]['y']),
140  'z_pose': str(MAP_POSES_DICT[MAP_TYPE]['z']),
141  'roll': str(MAP_POSES_DICT[MAP_TYPE]['R']),
142  'pitch': str(MAP_POSES_DICT[MAP_TYPE]['P']),
143  'yaw': str(MAP_POSES_DICT[MAP_TYPE]['Y']),
144  }.items())
145 
146  start_robot_state_publisher_cmd = Node(
147  package='robot_state_publisher',
148  executable='robot_state_publisher',
149  name='robot_state_publisher',
150  output='screen',
151  parameters=[
152  {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])}
153  ]
154  )
155 
156  # start the visualization
157  rviz_cmd = IncludeLaunchDescription(
158  PythonLaunchDescriptionSource(
159  os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
160  launch_arguments={'namespace': '',
161  'use_namespace': 'False'}.items())
162 
163  # start navigation
164  bringup_cmd = IncludeLaunchDescription(
165  PythonLaunchDescriptionSource(
166  os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
167  launch_arguments={
168  'map': map_yaml_file,
169  'graph': graph_filepath,
170  'use_keepout_zones': use_keepout_zones,
171  'use_speed_zones': use_speed_zones,
172  'keepout_mask': keepout_mask_yaml_file,
173  'speed_mask': speed_mask_yaml_file,
174  }.items())
175 
176  # start the demo autonomy task
177  demo_cmd = Node(
178  package='nav2_simple_commander',
179  executable='example_route',
180  parameters=[{
181  'start_pose': {
182  'x': ROUTE_POSES_DICT['start'][MAP_TYPE]['x'],
183  'y': ROUTE_POSES_DICT['start'][MAP_TYPE]['y'],
184  'yaw': ROUTE_POSES_DICT['start'][MAP_TYPE]['yaw']
185  },
186  'goal_pose': {
187  'x': ROUTE_POSES_DICT['goal'][MAP_TYPE]['x'],
188  'y': ROUTE_POSES_DICT['goal'][MAP_TYPE]['y'],
189  'yaw': ROUTE_POSES_DICT['goal'][MAP_TYPE]['yaw']
190  },
191  }],
192  emulate_tty=True,
193  output='screen')
194 
195  set_env_vars_resources2 = AppendEnvironmentVariable(
196  'GZ_SIM_RESOURCE_PATH',
197  str(Path(os.path.join(desc_dir)).parent.resolve()))
198 
199  ld = LaunchDescription()
200  ld.add_action(declare_headless_cmd)
201  ld.add_action(declare_use_keepout_zones_cmd)
202  ld.add_action(declare_use_speed_zones_cmd)
203  ld.add_action(declare_keepout_mask_yaml_cmd)
204  ld.add_action(declare_speed_mask_yaml_cmd)
205  ld.add_action(set_env_vars_resources)
206  ld.add_action(world_sdf_xacro)
207  ld.add_action(remove_temp_sdf_file)
208  ld.add_action(start_gazebo_server_cmd)
209  ld.add_action(start_gazebo_client_cmd)
210  ld.add_action(spawn_robot_cmd)
211  ld.add_action(start_robot_state_publisher_cmd)
212  ld.add_action(rviz_cmd)
213  ld.add_action(bringup_cmd)
214  ld.add_action(demo_cmd)
215  ld.add_action(set_env_vars_resources2)
216  return ld
Generated by doxygen 1.9.1