2021-12-27 01:28:18 +00:00
|
|
|
# Copyright (c) 2018 Intel Corporation
|
|
|
|
#
|
|
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
|
|
# you may not use this file except in compliance with the License.
|
|
|
|
# You may obtain a copy of the License at
|
|
|
|
#
|
|
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
#
|
|
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
|
|
# See the License for the specific language governing permissions and
|
|
|
|
# limitations under the License.
|
|
|
|
|
|
|
|
"""
|
|
|
|
Example for spawing multiple robots in Gazebo.
|
|
|
|
|
|
|
|
This is an example on how to create a launch file for spawning multiple robots into Gazebo
|
|
|
|
and launch multiple instances of the navigation stack, each controlling one robot.
|
|
|
|
The robots co-exist on a shared environment and are controlled by independent nav stacks
|
|
|
|
"""
|
|
|
|
|
|
|
|
import os
|
|
|
|
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from launch import LaunchDescription, condition
|
|
|
|
from launch.actions import (
|
|
|
|
DeclareLaunchArgument,
|
|
|
|
ExecuteProcess,
|
|
|
|
GroupAction,
|
|
|
|
IncludeLaunchDescription,
|
|
|
|
LogInfo,
|
|
|
|
)
|
|
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
|
|
from launch.substitutions import LaunchConfiguration, TextSubstitution
|
2023-01-08 22:32:10 +00:00
|
|
|
from launch_ros.actions import Node
|
2021-12-27 01:28:18 +00:00
|
|
|
|
|
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
# Get the launch directory
|
|
|
|
bringup_dir = get_package_share_directory("nav2_bringup")
|
|
|
|
launch_dir = os.path.join(bringup_dir, "launch")
|
|
|
|
|
|
|
|
# Get the launch directory for multirobot_map_merge where we have a modified launch files
|
|
|
|
map_merge_dir = get_package_share_directory("multirobot_map_merge")
|
|
|
|
launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation")
|
|
|
|
|
|
|
|
# Names and poses of the robots for known poses demo
|
|
|
|
robots_known_poses = [
|
|
|
|
{"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01},
|
|
|
|
{"name": "robot2", "x_pose": -3.0, "y_pose": 1.5, "z_pose": 0.01},
|
|
|
|
]
|
2023-01-08 22:32:10 +00:00
|
|
|
# Names and poses of the robots for unknown poses demo, the must be very close at beginning
|
2021-12-27 01:28:18 +00:00
|
|
|
robots_unknown_poses = [
|
|
|
|
{"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01},
|
|
|
|
{"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01},
|
|
|
|
]
|
|
|
|
|
|
|
|
# Simulation settings
|
|
|
|
world = LaunchConfiguration("world")
|
|
|
|
simulator = LaunchConfiguration("simulator")
|
|
|
|
|
|
|
|
# On this example all robots are launched with the same settings
|
|
|
|
map_yaml_file = LaunchConfiguration("map")
|
|
|
|
|
|
|
|
autostart = LaunchConfiguration("autostart")
|
|
|
|
rviz_config_file = LaunchConfiguration("rviz_config")
|
|
|
|
use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
|
|
|
|
use_rviz = LaunchConfiguration("use_rviz")
|
|
|
|
log_settings = LaunchConfiguration("log_settings", default="true")
|
|
|
|
|
|
|
|
known_init_poses = LaunchConfiguration("known_init_poses")
|
|
|
|
declare_known_init_poses_cmd = DeclareLaunchArgument(
|
|
|
|
"known_init_poses",
|
|
|
|
default_value="True",
|
|
|
|
description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
|
|
|
|
)
|
|
|
|
|
|
|
|
# Declare the launch arguments
|
|
|
|
declare_world_cmd = DeclareLaunchArgument(
|
|
|
|
"world",
|
|
|
|
default_value=os.path.join(launch_dir_map_merge, "worlds", "world_only.model"),
|
|
|
|
description="Full path to world file to load",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_simulator_cmd = DeclareLaunchArgument(
|
|
|
|
"simulator",
|
|
|
|
default_value="gazebo",
|
|
|
|
description="The simulator to use (gazebo or gzserver)",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_map_yaml_cmd = DeclareLaunchArgument(
|
|
|
|
"map",
|
|
|
|
default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"),
|
|
|
|
description="Full path to map file to load",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_robot1_params_file_cmd = DeclareLaunchArgument(
|
|
|
|
"robot1_params_file",
|
|
|
|
default_value=os.path.join(
|
|
|
|
launch_dir_map_merge, "config", "nav2_multirobot_params_1.yaml"
|
|
|
|
),
|
|
|
|
description="Full path to the ROS2 parameters file to use for robot1 launched nodes",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_robot2_params_file_cmd = DeclareLaunchArgument(
|
|
|
|
"robot2_params_file",
|
|
|
|
default_value=os.path.join(
|
|
|
|
launch_dir_map_merge, "config", "nav2_multirobot_params_2.yaml"
|
|
|
|
),
|
|
|
|
description="Full path to the ROS2 parameters file to use for robot2 launched nodes",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_autostart_cmd = DeclareLaunchArgument(
|
|
|
|
"autostart",
|
|
|
|
default_value="true",
|
|
|
|
description="Automatically startup the stacks",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_rviz_config_file_cmd = DeclareLaunchArgument(
|
|
|
|
"rviz_config",
|
|
|
|
default_value=os.path.join(bringup_dir, "rviz", "nav2_namespaced_view.rviz"),
|
|
|
|
description="Full path to the RVIZ config file to use.",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
|
|
|
|
"use_robot_state_pub",
|
|
|
|
default_value="True",
|
|
|
|
description="Whether to start the robot state publisher",
|
|
|
|
)
|
|
|
|
|
|
|
|
declare_use_rviz_cmd = DeclareLaunchArgument(
|
|
|
|
"use_rviz", default_value="True", description="Whether to start RVIZ"
|
|
|
|
)
|
|
|
|
|
|
|
|
slam_toolbox = LaunchConfiguration("slam_toolbox")
|
|
|
|
slam_gmapping = LaunchConfiguration("slam_gmapping")
|
|
|
|
declare_slam_toolbox_cmd = DeclareLaunchArgument(
|
|
|
|
"slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
|
|
|
|
)
|
|
|
|
declare_slam_gmapping_cmd = DeclareLaunchArgument(
|
|
|
|
"slam_gmapping",
|
|
|
|
default_value="False",
|
|
|
|
description="Whether run a SLAM gmapping",
|
|
|
|
)
|
|
|
|
|
|
|
|
# Start Gazebo with plugin providing the robot spawing service
|
|
|
|
start_gazebo_cmd = ExecuteProcess(
|
|
|
|
cmd=[
|
|
|
|
simulator,
|
|
|
|
"--verbose",
|
|
|
|
"-s",
|
|
|
|
"libgazebo_ros_init.so",
|
|
|
|
"-s",
|
|
|
|
"libgazebo_ros_factory.so",
|
|
|
|
world,
|
|
|
|
],
|
|
|
|
output="screen",
|
|
|
|
)
|
|
|
|
|
2023-01-08 22:32:10 +00:00
|
|
|
robot_sdf = LaunchConfiguration("robot_sdf")
|
|
|
|
declare_robot_sdf_cmd = DeclareLaunchArgument(
|
|
|
|
"robot_sdf",
|
|
|
|
default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
|
|
|
|
description="Full path to robot sdf file to spawn the robot in gazebo",
|
|
|
|
)
|
|
|
|
|
2021-12-27 01:28:18 +00:00
|
|
|
# Define commands for spawing the robots into Gazebo
|
|
|
|
spawn_robots_cmds = []
|
|
|
|
for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses):
|
2023-01-08 22:32:10 +00:00
|
|
|
# after humble release, use spawn_entity.py
|
|
|
|
if os.getenv("ROS_DISTRO") == "humble":
|
|
|
|
spawn_robots_cmds.append(
|
|
|
|
Node(
|
|
|
|
package="gazebo_ros",
|
|
|
|
executable="spawn_entity.py",
|
|
|
|
output="screen",
|
|
|
|
arguments=[
|
|
|
|
"-entity",
|
|
|
|
robot_known["name"],
|
|
|
|
"-file",
|
|
|
|
robot_sdf,
|
|
|
|
"-robot_namespace",
|
|
|
|
TextSubstitution(text=str(robot_known["name"])),
|
|
|
|
"-x",
|
|
|
|
TextSubstitution(text=str(robot_known["x_pose"])),
|
|
|
|
"-y",
|
|
|
|
TextSubstitution(text=str(robot_known["y_pose"])),
|
|
|
|
"-z",
|
|
|
|
TextSubstitution(text=str(robot_known["z_pose"])),
|
|
|
|
"-R",
|
|
|
|
"0.0",
|
|
|
|
"-P",
|
|
|
|
"0.0",
|
|
|
|
"-Y",
|
|
|
|
"0.0",
|
|
|
|
],
|
|
|
|
condition=IfCondition(known_init_poses),
|
|
|
|
)
|
2021-12-27 01:28:18 +00:00
|
|
|
)
|
2023-01-08 22:32:10 +00:00
|
|
|
spawn_robots_cmds.append(
|
|
|
|
Node(
|
|
|
|
package="gazebo_ros",
|
|
|
|
executable="spawn_entity.py",
|
|
|
|
output="screen",
|
|
|
|
arguments=[
|
|
|
|
"-entity",
|
|
|
|
robot_unknown["name"],
|
|
|
|
"-file",
|
|
|
|
robot_sdf,
|
|
|
|
"-robot_namespace",
|
|
|
|
TextSubstitution(text=str(robot_unknown["name"])),
|
|
|
|
"-x",
|
|
|
|
TextSubstitution(text=str(robot_unknown["x_pose"])),
|
|
|
|
"-y",
|
|
|
|
TextSubstitution(text=str(robot_unknown["y_pose"])),
|
|
|
|
"-z",
|
|
|
|
TextSubstitution(text=str(robot_unknown["z_pose"])),
|
|
|
|
"-R",
|
|
|
|
"0.0",
|
|
|
|
"-P",
|
|
|
|
"0.0",
|
|
|
|
"-Y",
|
|
|
|
"0.0",
|
|
|
|
],
|
|
|
|
condition=UnlessCondition(known_init_poses),
|
|
|
|
)
|
|
|
|
)
|
|
|
|
else:
|
|
|
|
spawn_robots_cmds.append(
|
|
|
|
IncludeLaunchDescription(
|
|
|
|
PythonLaunchDescriptionSource(
|
|
|
|
os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
|
|
|
|
),
|
|
|
|
launch_arguments={
|
|
|
|
"x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
|
|
|
|
"y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
|
|
|
|
"z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
|
|
|
|
"robot_name": robot_known["name"],
|
|
|
|
"turtlebot_type": TextSubstitution(text="waffle"),
|
|
|
|
}.items(),
|
|
|
|
condition=IfCondition(known_init_poses),
|
|
|
|
)
|
|
|
|
)
|
|
|
|
spawn_robots_cmds.append(
|
|
|
|
IncludeLaunchDescription(
|
|
|
|
PythonLaunchDescriptionSource(
|
|
|
|
os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
|
|
|
|
),
|
|
|
|
launch_arguments={
|
|
|
|
"x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
|
|
|
|
"y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
|
|
|
|
"z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
|
|
|
|
"robot_name": robot_unknown["name"],
|
|
|
|
"turtlebot_type": TextSubstitution(text="waffle"),
|
|
|
|
}.items(),
|
|
|
|
condition=UnlessCondition(known_init_poses),
|
|
|
|
)
|
2021-12-27 01:28:18 +00:00
|
|
|
)
|
|
|
|
|
|
|
|
# Define commands for launching the navigation instances
|
|
|
|
nav_instances_cmds = []
|
|
|
|
for robot in robots_known_poses:
|
|
|
|
params_file = LaunchConfiguration(f"{robot['name']}_params_file")
|
|
|
|
|
|
|
|
group = GroupAction(
|
|
|
|
[
|
|
|
|
IncludeLaunchDescription(
|
|
|
|
PythonLaunchDescriptionSource(
|
|
|
|
os.path.join(launch_dir, "rviz_launch.py")
|
|
|
|
),
|
|
|
|
condition=IfCondition(use_rviz),
|
|
|
|
launch_arguments={
|
|
|
|
"namespace": TextSubstitution(text=robot["name"]),
|
|
|
|
"use_namespace": "True",
|
|
|
|
"rviz_config": rviz_config_file,
|
|
|
|
}.items(),
|
|
|
|
),
|
|
|
|
IncludeLaunchDescription(
|
|
|
|
PythonLaunchDescriptionSource(
|
|
|
|
os.path.join(launch_dir_map_merge, "tb3_simulation_launch.py")
|
|
|
|
),
|
|
|
|
launch_arguments={
|
|
|
|
"namespace": robot["name"],
|
|
|
|
"use_namespace": "True",
|
|
|
|
"map": map_yaml_file,
|
|
|
|
"use_sim_time": "True",
|
|
|
|
"params_file": params_file,
|
|
|
|
"autostart": autostart,
|
|
|
|
"use_rviz": "False",
|
|
|
|
"use_simulator": "False",
|
|
|
|
"headless": "False",
|
|
|
|
"slam": "True",
|
|
|
|
"slam_toolbox": slam_toolbox,
|
|
|
|
"slam_gmapping": slam_gmapping,
|
|
|
|
"use_robot_state_pub": use_robot_state_pub,
|
|
|
|
}.items(),
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=["Launching ", robot["name"]],
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=[robot["name"], " map yaml: ", map_yaml_file],
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=[robot["name"], " params yaml: ", params_file],
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=[robot["name"], " rviz config file: ", rviz_config_file],
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=[
|
|
|
|
robot["name"],
|
|
|
|
" using robot state pub: ",
|
|
|
|
use_robot_state_pub,
|
|
|
|
],
|
|
|
|
),
|
|
|
|
LogInfo(
|
|
|
|
condition=IfCondition(log_settings),
|
|
|
|
msg=[robot["name"], " autostart: ", autostart],
|
|
|
|
),
|
|
|
|
]
|
|
|
|
)
|
|
|
|
|
|
|
|
nav_instances_cmds.append(group)
|
|
|
|
|
|
|
|
# Create the launch description and populate
|
|
|
|
ld = LaunchDescription()
|
|
|
|
|
|
|
|
# Declare the launch options
|
|
|
|
ld.add_action(declare_simulator_cmd)
|
|
|
|
ld.add_action(declare_world_cmd)
|
|
|
|
ld.add_action(declare_map_yaml_cmd)
|
|
|
|
ld.add_action(declare_robot1_params_file_cmd)
|
|
|
|
ld.add_action(declare_robot2_params_file_cmd)
|
|
|
|
ld.add_action(declare_use_rviz_cmd)
|
|
|
|
ld.add_action(declare_autostart_cmd)
|
|
|
|
ld.add_action(declare_rviz_config_file_cmd)
|
|
|
|
ld.add_action(declare_use_robot_state_pub_cmd)
|
|
|
|
ld.add_action(declare_slam_toolbox_cmd)
|
|
|
|
ld.add_action(declare_slam_gmapping_cmd)
|
|
|
|
ld.add_action(declare_known_init_poses_cmd)
|
2023-01-08 22:32:10 +00:00
|
|
|
ld.add_action(declare_robot_sdf_cmd)
|
2021-12-27 01:28:18 +00:00
|
|
|
|
|
|
|
# Add the actions to start gazebo, robots and simulations
|
|
|
|
ld.add_action(start_gazebo_cmd)
|
|
|
|
|
|
|
|
for spawn_robot_cmd in spawn_robots_cmds:
|
|
|
|
ld.add_action(spawn_robot_cmd)
|
|
|
|
|
|
|
|
for simulation_instance_cmd in nav_instances_cmds:
|
|
|
|
ld.add_action(simulation_instance_cmd)
|
|
|
|
|
|
|
|
return ld
|