|
| 1 | +# Copyright (c) 2023 LG Electronics. |
| 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 | + |
| 16 | +import os |
| 17 | +from ament_index_python.packages import get_package_share_directory |
| 18 | +from launch import LaunchDescription |
| 19 | +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, |
| 20 | + IncludeLaunchDescription, LogInfo) |
| 21 | +from launch.conditions import IfCondition |
| 22 | +from launch.launch_description_sources import PythonLaunchDescriptionSource |
| 23 | +from launch.substitutions import LaunchConfiguration, TextSubstitution |
| 24 | +from nav2_common.launch import ParseMultiRobotPose |
| 25 | + |
| 26 | + |
| 27 | +def generate_launch_description(): |
| 28 | + """ |
| 29 | + Bring up the multi-robots with given launch arguments. |
| 30 | +
|
| 31 | + Launch arguments consist of robot name(which is namespace) and pose for initialization. |
| 32 | + Keep general yaml format for pose information. |
| 33 | + ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" |
| 34 | + ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; |
| 35 | + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" |
| 36 | + """ |
| 37 | + # Get the launch directory |
| 38 | + bringup_dir = get_package_share_directory('nav2_bringup') |
| 39 | + launch_dir = os.path.join(bringup_dir, 'launch') |
| 40 | + |
| 41 | + # Simulation settings |
| 42 | + world = LaunchConfiguration('world') |
| 43 | + simulator = LaunchConfiguration('simulator') |
| 44 | + |
| 45 | + # On this example all robots are launched with the same settings |
| 46 | + map_yaml_file = LaunchConfiguration('map') |
| 47 | + params_file = LaunchConfiguration('params_file') |
| 48 | + autostart = LaunchConfiguration('autostart') |
| 49 | + rviz_config_file = LaunchConfiguration('rviz_config') |
| 50 | + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') |
| 51 | + use_rviz = LaunchConfiguration('use_rviz') |
| 52 | + log_settings = LaunchConfiguration('log_settings', default='true') |
| 53 | + |
| 54 | + # Declare the launch arguments |
| 55 | + declare_world_cmd = DeclareLaunchArgument( |
| 56 | + 'world', |
| 57 | + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), |
| 58 | + description='Full path to world file to load') |
| 59 | + |
| 60 | + declare_simulator_cmd = DeclareLaunchArgument( |
| 61 | + 'simulator', |
| 62 | + default_value='gazebo', |
| 63 | + description='The simulator to use (gazebo or gzserver)') |
| 64 | + |
| 65 | + declare_map_yaml_cmd = DeclareLaunchArgument( |
| 66 | + 'map', |
| 67 | + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), |
| 68 | + description='Full path to map file to load') |
| 69 | + |
| 70 | + declare_params_file_cmd = DeclareLaunchArgument( |
| 71 | + 'params_file', |
| 72 | + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), |
| 73 | + description='Full path to the ROS2 parameters file to use for all launched nodes') |
| 74 | + |
| 75 | + declare_autostart_cmd = DeclareLaunchArgument( |
| 76 | + 'autostart', default_value='false', |
| 77 | + description='Automatically startup the stacks') |
| 78 | + |
| 79 | + declare_rviz_config_file_cmd = DeclareLaunchArgument( |
| 80 | + 'rviz_config', |
| 81 | + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), |
| 82 | + description='Full path to the RVIZ config file to use.') |
| 83 | + |
| 84 | + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( |
| 85 | + 'use_robot_state_pub', |
| 86 | + default_value='True', |
| 87 | + description='Whether to start the robot state publisher') |
| 88 | + |
| 89 | + declare_use_rviz_cmd = DeclareLaunchArgument( |
| 90 | + 'use_rviz', |
| 91 | + default_value='True', |
| 92 | + description='Whether to start RVIZ') |
| 93 | + |
| 94 | + # Start Gazebo with plugin providing the robot spawning service |
| 95 | + start_gazebo_cmd = ExecuteProcess( |
| 96 | + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', |
| 97 | + '-s', 'libgazebo_ros_factory.so', world], |
| 98 | + output='screen') |
| 99 | + |
| 100 | + robots_list = ParseMultiRobotPose('robots').value() |
| 101 | + |
| 102 | + # Define commands for launching the navigation instances |
| 103 | + bringup_cmd_group = [] |
| 104 | + for robot_name in robots_list: |
| 105 | + init_pose = robots_list[robot_name] |
| 106 | + group = GroupAction([ |
| 107 | + LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), |
| 108 | + |
| 109 | + IncludeLaunchDescription( |
| 110 | + PythonLaunchDescriptionSource( |
| 111 | + os.path.join(launch_dir, 'rviz_launch.py')), |
| 112 | + condition=IfCondition(use_rviz), |
| 113 | + launch_arguments={'namespace': TextSubstitution(text=robot_name), |
| 114 | + 'use_namespace': 'True', |
| 115 | + 'rviz_config': rviz_config_file}.items()), |
| 116 | + |
| 117 | + IncludeLaunchDescription( |
| 118 | + PythonLaunchDescriptionSource(os.path.join(bringup_dir, |
| 119 | + 'launch', |
| 120 | + 'tb3_simulation_launch.py')), |
| 121 | + launch_arguments={'namespace': robot_name, |
| 122 | + 'use_namespace': 'True', |
| 123 | + 'map': map_yaml_file, |
| 124 | + 'use_sim_time': 'True', |
| 125 | + 'params_file': params_file, |
| 126 | + 'autostart': autostart, |
| 127 | + 'use_rviz': 'False', |
| 128 | + 'use_simulator': 'False', |
| 129 | + 'headless': 'False', |
| 130 | + 'use_robot_state_pub': use_robot_state_pub, |
| 131 | + 'x_pose': TextSubstitution(text=str(init_pose['x'])), |
| 132 | + 'y_pose': TextSubstitution(text=str(init_pose['y'])), |
| 133 | + 'z_pose': TextSubstitution(text=str(init_pose['z'])), |
| 134 | + 'roll': TextSubstitution(text=str(init_pose['roll'])), |
| 135 | + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), |
| 136 | + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), |
| 137 | + 'robot_name':TextSubstitution(text=robot_name), }.items()) |
| 138 | + ]) |
| 139 | + |
| 140 | + bringup_cmd_group.append(group) |
| 141 | + |
| 142 | + # Create the launch description and populate |
| 143 | + ld = LaunchDescription() |
| 144 | + |
| 145 | + # Declare the launch options |
| 146 | + ld.add_action(declare_simulator_cmd) |
| 147 | + ld.add_action(declare_world_cmd) |
| 148 | + ld.add_action(declare_map_yaml_cmd) |
| 149 | + ld.add_action(declare_params_file_cmd) |
| 150 | + ld.add_action(declare_use_rviz_cmd) |
| 151 | + ld.add_action(declare_autostart_cmd) |
| 152 | + ld.add_action(declare_rviz_config_file_cmd) |
| 153 | + ld.add_action(declare_use_robot_state_pub_cmd) |
| 154 | + |
| 155 | + # Add the actions to start gazebo, robots and simulations |
| 156 | + ld.add_action(start_gazebo_cmd) |
| 157 | + |
| 158 | + ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) |
| 159 | + |
| 160 | + ld.add_action(LogInfo(condition=IfCondition(log_settings), |
| 161 | + msg=['map yaml: ', map_yaml_file])) |
| 162 | + ld.add_action(LogInfo(condition=IfCondition(log_settings), |
| 163 | + msg=['params yaml: ', params_file])) |
| 164 | + ld.add_action(LogInfo(condition=IfCondition(log_settings), |
| 165 | + msg=['rviz config file: ', rviz_config_file])) |
| 166 | + ld.add_action(LogInfo(condition=IfCondition(log_settings), |
| 167 | + msg=['using robot state pub: ', use_robot_state_pub])) |
| 168 | + ld.add_action(LogInfo(condition=IfCondition(log_settings), |
| 169 | + msg=['autostart: ', autostart])) |
| 170 | + |
| 171 | + for cmd in bringup_cmd_group: |
| 172 | + ld.add_action(cmd) |
| 173 | + |
| 174 | + return ld |
0 commit comments