|
26 | 26 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression |
27 | 27 | from launch_ros.actions import Node |
28 | 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.12, 'y': -21.3, 'z': 0.01, |
| 37 | + 'R': 0.00, 'P': 0.00, 'Y': 1.57 |
| 38 | + } |
| 39 | +} |
| 40 | +MAP_TYPE = 'depot' # Change this to 'warehouse' for warehouse map |
| 41 | + |
29 | 42 |
|
30 | 43 | def generate_launch_description() -> LaunchDescription: |
31 | 44 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') |
32 | 45 | sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') |
33 | 46 | desc_dir = get_package_share_directory('nav2_minimal_tb4_description') |
34 | 47 |
|
35 | 48 | robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') |
36 | | - world = os.path.join(sim_dir, 'worlds', 'depot.sdf') |
37 | | - map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') |
38 | | - graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot4_graph.geojson') |
| 49 | + world = os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf') |
| 50 | + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}.yaml') |
| 51 | + graph_filepath = os.path.join( |
| 52 | + nav2_bringup_dir, 'graphs', f'turtlebot4_graph_{MAP_TYPE}.geojson') |
39 | 53 |
|
40 | 54 | # Launch configuration variables |
41 | 55 | headless = LaunchConfiguration('headless') |
@@ -79,12 +93,13 @@ def generate_launch_description() -> LaunchDescription: |
79 | 93 | os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), |
80 | 94 | launch_arguments={'use_sim_time': 'True', |
81 | 95 | 'robot_sdf': robot_sdf, |
82 | | - 'x_pose': '-8.0', |
83 | | - 'y_pose': '0.0', |
84 | | - 'z_pose': '0.0', |
85 | | - 'roll': '0.0', |
86 | | - 'pitch': '0.0', |
87 | | - 'yaw': '0.0'}.items()) |
| 96 | + 'x_pose': str(MAP_POSES_DICT[MAP_TYPE]['x']), |
| 97 | + 'y_pose': str(MAP_POSES_DICT[MAP_TYPE]['y']), |
| 98 | + 'z_pose': str(MAP_POSES_DICT[MAP_TYPE]['z']), |
| 99 | + 'roll': str(MAP_POSES_DICT[MAP_TYPE]['R']), |
| 100 | + 'pitch': str(MAP_POSES_DICT[MAP_TYPE]['P']), |
| 101 | + 'yaw': str(MAP_POSES_DICT[MAP_TYPE]['Y']), |
| 102 | + }.items()) |
88 | 103 |
|
89 | 104 | start_robot_state_publisher_cmd = Node( |
90 | 105 | package='robot_state_publisher', |
|
0 commit comments