Adding multi-robot launch example

nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py

@@ -0,0 +1,170 @@
+# 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.
+
+''' 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_prefix
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
+    LogInfo, GroupAction, ExecuteProcess)
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import PushRosNamespace
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # 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')
+    params_file = LaunchConfiguration('params_file')
+    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    log_settings = LaunchConfiguration('log_settings', default='true')
+
+    # Declare the launch arguments
+    declare_world_cmd = DeclareLaunchArgument(
+        'world',
+        default_value=os.path.join(bringup_dir, '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_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params_namespaced.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_bt_xml_cmd = DeclareLaunchArgument(
+        'bt_xml_file',
+        default_value=os.path.join(
+            get_package_prefix('nav2_bt_navigator'),
+            'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
+        description='Full path to the behavior tree xml file to use')
+
+   # Start Gazebo with plugin providing the robot spawing service
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
+        output='screen')
+
+    # Define commands for spawing the robots into Gazebo
+    spawn_robots_cmds = []
+    for robot in robots:
+        spawn_robots_cmds.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'spawn_robot_launch.py')),
+                launch_arguments={
+                                  'x_pose': TextSubstitution(text=str(robot['x_pose'])),
+                                  'y_pose': TextSubstitution(text=str(robot['y_pose'])),
+                                  'z_pose': TextSubstitution(text=str(robot['z_pose'])),
+                                  'robot_name': robot['name']
+                                  }.items()))
+
+    # Define commands for launching the navigation instances
+    nav_instances_cmds = []
+    for robot in robots:
+        group = GroupAction([
+            # TODO(orduno) Each `action.Node` within the `localization` and `navigation` launch
+            #              files has two versions, one with the required remaps and another without.
+            #              The `use_remappings` flag specifies which runs.
+            #              A better mechanism would be to have a PushNodeRemapping() action:
+            #              https://github.com/ros2/launch_ros/issues/56
+            #              For more on why we're remapping topics, see the note below
+
+            # PushNodeRemapping(remappings)
+
+            # Instances use the robot's name for namespace
+            PushRosNamespace(robot['name']),
+
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'nav2_tb3_simulation_launch.py')),
+                launch_arguments={
+                                  #TODO(orduno) might not be necessary to pass the robot name
+                                  'namespace': robot['name'],
+                                  'map_yaml_file': map_yaml_file,
+                                  'use_sim_time': 'True',
+                                  'params_file': params_file,
+                                  'bt_xml_file': bt_xml_file,
+                                  'autostart': 'False',
+                                  'use_remappings': 'True',
+                                  'use_simulator': 'False'}.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'], ' behavior tree xml: ', bt_xml_file])
+        ])
+
+        nav_instances_cmds.append(group)
+
+    # A note on the `remappings` variable defined above and the fact it's passed as a node arg.
+    # A few topics have fully qualified names (have a leading '/'), these need to be remapped
+    # 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
+    # for multi-robot transforms:
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+
+    # 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_params_file_cmd)
+    ld.add_action(declare_bt_xml_cmd)
+
+    # 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

nav2_bringup/bringup/params/nav2_params_namespaced.yaml

@@ -0,0 +1,164 @@
+navigation_namespace:  # this is substituted with the actual robot name during launch
+  amcl:
+    ros__parameters:
+      use_sim_time: True
+      alpha1: 0.2
+      alpha2: 0.2
+      alpha3: 0.2
+      alpha4: 0.2
+      alpha5: 0.2
+      base_frame_id: "base_footprint"
+      beam_skip_distance: 0.5
+      beam_skip_error_threshold: 0.9
+      beam_skip_threshold: 0.3
+      do_beamskip: false
+      global_frame_id: "map"
+      lambda_short: 0.1
+      laser_likelihood_max_dist: 2.0
+      laser_max_range: 100.0
+      laser_min_range: -1.0
+      laser_model_type: "likelihood_field"
+      max_beams: 60
+      max_particles: 2000
+      min_particles: 500
+      odom_frame_id: "odom"
+      pf_err: 0.05
+      pf_z: 0.99
+      recovery_alpha_fast: 0.0
+      recovery_alpha_slow: 0.0
+      resample_interval: 1
+      robot_model_type: "differential"
+      save_pose_rate: 0.5
+      sigma_hit: 0.2
+      tf_broadcast: true
+      transform_tolerance: 1.0
+      update_min_a: 0.2
+      update_min_d: 0.25
+      z_hit: 0.5
+      z_max: 0.05
+      z_rand: 0.5
+      z_short: 0.05
+
+  amcl_map_client:
+    ros__parameters:
+      use_sim_time: True
+
+  amcl_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+  bt_navigator:
+    ros__parameters:
+      use_sim_time: True
+      bt_xml_filename: "bt_navigator.xml"
+
+  dwb_controller:
+    ros__parameters:
+      use_sim_time: True
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      min_x_velocity_threshold: 0.001
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      min_y_velocity_threshold: 0.5
+      min_theta_velocity_threshold: 0.001
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      xy_goal_tolerance: 0.25
+      transform_tolerance: 0.2
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 0.0
+      GoalAlign.scale: 0.0
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+
+  local_costmap:
+    local_costmap:
+      ros__parameters:
+        use_sim_time: True
+        global_frame: odom
+        plugin_names: ["obstacle_layer", "inflation_layer"]
+        plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+        rolling_window: true
+        width: 3
+        height: 3
+        resolution: 0.05
+        robot_radius: 0.22
+        inflation_layer.cost_scaling_factor: 3.0
+        obstacle_layer:
+          enabled: True
+        static_layer:
+          map_subscribe_transient_local: True
+        always_send_full_costmap: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+    local_costmap_client:
+      ros__parameters:
+        use_sim_time: True
+    local_costmap_rclcpp_node:
+      ros__parameters:
+        use_sim_time: True
+
+  global_costmap:
+    global_costmap:
+      ros__parameters:
+        use_sim_time: True
+        robot_radius: 0.22
+        obstacle_layer:
+          enabled: True
+        static_layer:
+          map_subscribe_transient_local: True
+        always_send_full_costmap: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+    global_costmap_client:
+      ros__parameters:
+        use_sim_time: True
+    global_costmap_rclcpp_node:
+      ros__parameters:
+        use_sim_time: True
+
+  map_server:
+    ros__parameters:
+      use_sim_time: True
+      yaml_filename: "turtlebot3_world.yaml"
+
+  navfn_planner:
+    ros__parameters:
+      use_sim_time: True
+      tolerance: 0.0
+      use_astar: false
+
+  navfn_planner_GetCostmap_client:
+    ros__parameters:
+      use_sim_time: True
+
+  robot_state_publisher:
+    ros__parameters:
+      use_sim_time: True

nav2_bringup/bringup/worlds/world_only.model

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>1000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+    <model name="turtlebot3_world">
+      <static>1</static>
+      <include>
+        <uri>model://turtlebot3_world</uri>
+      </include>
+    </model>
+
+  </world>
+</sdf>