multi_tb3_simulation.py code

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/LICENS...

#

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.

"""This is all-in-one launch script intended for use by nav2 developers."""

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, IncludeLaunchDescription, LogInfo) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution

def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch')

# Names and poses of the robots
robots = [
    {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01},
    {'name': 'robot2', 'x_pose': 0.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')

# Create the launch configuration variables
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
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')

#slam = LaunchConfiguration('slam')
#namespace = LaunchConfiguration('namespace')
#use_namespace = LaunchConfiguration('use_namespace')
#use_sim_time = LaunchConfiguration('use_sim_time')
#params_file = LaunchConfiguration('params_file')
# Launch configuration variables specific to simulation
#rviz_config_file = LaunchConfiguration('rviz_config')
#simulator = LaunchConfiguration('simulator')
#use_rviz = LaunchConfiguration('use_rviz')
#headless = LaunchConfiguration('headless')



# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
#              https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'),
              ('/tf_static', 'tf_static')]

# Declare the launch arguments
declare_world_cmd = DeclareLaunchArgument(
    'world',
    # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
    #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
    # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
    #                            'worlds/turtlebot3_worlds/waffle.model'),
    default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
    description='Full path to world model 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(bringup_dir, 'params', '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(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'),
    description='Full path to the ROS2 parameters file to use for robot2 launched nodes')

declare_bt_xml_cmd = DeclareLaunchArgument(
    'default_bt_xml_filename',
    default_value=os.path.join(
        get_package_share_directory('nav2_bt_navigator'),
        'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
    description='Full path to the behavior tree xml file to use')

declare_autostart_cmd = DeclareLaunchArgument(
    'autostart', default_value='true',
    description='Automatically startup the nav2 stack')

declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    'use_robot_state_pub',
    default_value='True ...