diff --git a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py new file mode 100644 index 00000000000..80c80286f07 --- /dev/null +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_params_namespaced.yaml b/nav2_bringup/bringup/params/nav2_params_namespaced.yaml new file mode 100644 index 00000000000..2eb1fe1e14d --- /dev/null +++ b/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 diff --git a/nav2_bringup/bringup/worlds/world_only.model b/nav2_bringup/bringup/worlds/world_only.model new file mode 100644 index 00000000000..4c45d4e2f94 --- /dev/null +++ b/nav2_bringup/bringup/worlds/world_only.model @@ -0,0 +1,54 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + 1 + + model://turtlebot3_world + + + + +