Using nav2

Added in version Jazzy: This section.

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

There is a plethora of online examples of nav2, with varying levels of detail and quality. My approach will top-down, where we will see how illustrative examples and then walk through their contents.

Navigation on a known map

See also

Official documentation: https://docs.nav2.org/tutorials/docs/navigation2_on_real_turtlebot3.html

In this example, our interest is looking at how navigation can be done with built-in tools, using a known map. A map will have many definitions and the literature about it is extensive. For now, we can think of the map as something usual. It points out where things are, for instance, where passable regions (such as roads) are, and possibly hazards, obstacles, and other relevant objects.

In this example, a TurtleBot3 will be used. As part of nav2_bringup, there is a rather complete example that we can utilize, namely tb3_simulation_launch.py. The example can be executed with the following command.

ros2 launch nav2_bringup \
tb3_simulation_launch.py \
use_sim_time:=True \
headless:=False \
sigterm_timeout:=120

Important

Differently from everything else used in this tutorial, this tb3_simulation_launch.py demo does not always finishes cleanly on my machines. It has hanged sometimes, sometimes Gazebo does not show, and sometimes it does not shutdown properly.

It is highly recommended to use a docker container, such as UoMMScRobotics/sfr_gazebo_nav2, to minimise disappointment.

Warning

The sigterm_timeout flag is particularly important. Given that some devices we use might not be powerful enough for all processes to finish cleanly within 5 or 10 seconds, we should add this to prevent a SIGTERM from being sent to the nodes. If nodes are not terminated correctly they can leave connections open, linger indefinitely, and cause extremely difficult-to-debug situations. This can also be dangerous when real robots are used.

We use headless:=False for the launch file to spawn Gazebo. We use use_sim_time:=True to make sure that Gazebo's clock will be used an prevent timing issues with /tf.

This example will create a large number of nodes and open two screens. One of these will be for Gazebo and another for rviz2. In this example, there are two actions expected from the user.

1. Set the initial 2D Pose Estimate. This can be done through rviz2, or ROS2 interfaces.

2. Send one or more Nav2 Goal s.

An example of how to do so is shown in the video below. Please note that although I adjust Gazebo to line up with the rviz2 view, this is for my convenience. As long as your initial estimate is fairly accurate the navigation should work fine.

Warning

The jazzy version of this example shows a number of errors when shutting down. This might cause issues when the example is run repeatedly.

Unpacking the example

Locally, you can navigate to the package using the following command.

cd $(ros2 pkg prefix nav2_bringup --share)

The installed folder will have the following structure.

nav2_bringup/
├── cmake
│   ├── nav2_bringupConfig.cmake
│   └── nav2_bringupConfig-version.cmake
├── environment
│   ├── ament_prefix_path.dsv
│   ├── ament_prefix_path.sh
│   ├── path.dsv
│   └── path.sh
├── launch
│   ├── bringup_launch.py
│   ├── cloned_multi_tb3_simulation_launch.py
│   ├── localization_launch.py
│   ├── navigation_launch.py
│   ├── rviz_launch.py
│   ├── slam_launch.py
│   ├── tb3_loopback_simulation.launch.py
│   ├── tb3_simulation_launch.py
│   ├── tb4_loopback_simulation.launch.py
│   ├── tb4_simulation_launch.py
│   └── unique_multi_tb3_simulation_launch.py
├── local_setup.bash
├── local_setup.dsv
├── local_setup.sh
├── local_setup.zsh
├── maps
│   ├── depot.pgm
│   ├── depot.yaml
│   ├── tb3_sandbox.pgm
│   ├── tb3_sandbox.yaml
│   ├── warehouse.pgm
│   └── warehouse.yaml
├── package.dsv
├── package.xml
├── params
│   ├── nav2_multirobot_params_1.yaml
│   ├── nav2_multirobot_params_2.yaml
│   ├── nav2_multirobot_params_all.yaml
│   └── nav2_params.yaml
└── rviz
    ├── nav2_default_view.rviz
    └── nav2_namespaced_view.rviz

The launch file we executed in this example is shown below. A launch file that is general will tend to have a proportional level of complexity. We are not currently interested in dissecting all elements of this file. Rather, our interest is to take a look at the relevant files used and what they mean. By understanding these, we would be a step closer to be able to modify the example or interact with it for your own purposes.

Contents of tb3_simulation_launch.py

# Copyright (C) 2023 Open Source Robotics Foundation
#
# 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 all-in-one launch script intended for use by nav2 developers."""

import os
import tempfile

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    IncludeLaunchDescription,
    OpaqueFunction,
    RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnShutdown
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression

from launch_ros.actions import Node


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

    # Create the launch configuration variables
    slam = LaunchConfiguration('slam')
    namespace = LaunchConfiguration('namespace')
    use_namespace = LaunchConfiguration('use_namespace')
    map_yaml_file = LaunchConfiguration('map')
    use_sim_time = LaunchConfiguration('use_sim_time')
    params_file = LaunchConfiguration('params_file')
    autostart = LaunchConfiguration('autostart')
    use_composition = LaunchConfiguration('use_composition')
    use_respawn = LaunchConfiguration('use_respawn')

    # Launch configuration variables specific to simulation
    rviz_config_file = LaunchConfiguration('rviz_config_file')
    use_simulator = LaunchConfiguration('use_simulator')
    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
    use_rviz = LaunchConfiguration('use_rviz')
    headless = LaunchConfiguration('headless')
    world = LaunchConfiguration('world')
    pose = {
        'x': LaunchConfiguration('x_pose', default='-2.00'),
        'y': LaunchConfiguration('y_pose', default='-0.50'),
        'z': LaunchConfiguration('z_pose', default='0.01'),
        'R': LaunchConfiguration('roll', default='0.00'),
        'P': LaunchConfiguration('pitch', default='0.00'),
        'Y': LaunchConfiguration('yaw', default='0.00'),
    }
    robot_name = LaunchConfiguration('robot_name')
    robot_sdf = LaunchConfiguration('robot_sdf')

    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]

    # Declare the launch arguments
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace', default_value='', description='Top-level namespace'
    )

    declare_use_namespace_cmd = DeclareLaunchArgument(
        'use_namespace',
        default_value='false',
        description='Whether to apply a namespace to the navigation stack',
    )

    declare_slam_cmd = DeclareLaunchArgument(
        'slam', default_value='False', description='Whether run a SLAM'
    )

    declare_map_yaml_cmd = DeclareLaunchArgument(
        'map',
        default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
    )

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true',
    )

    declare_params_file_cmd = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
        description='Full path to the ROS2 parameters file to use for all launched nodes',
    )

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

    declare_use_composition_cmd = DeclareLaunchArgument(
        'use_composition',
        default_value='True',
        description='Whether to use composed bringup',
    )

    declare_use_respawn_cmd = DeclareLaunchArgument(
        'use_respawn',
        default_value='False',
        description='Whether to respawn if a node crashes. Applied when composition is disabled.',
    )

    declare_rviz_config_file_cmd = DeclareLaunchArgument(
        'rviz_config_file',
        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
        description='Full path to the RVIZ config file to use',
    )

    declare_use_simulator_cmd = DeclareLaunchArgument(
        'use_simulator',
        default_value='True',
        description='Whether to start the simulator',
    )

    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'
    )

    declare_simulator_cmd = DeclareLaunchArgument(
        'headless', default_value='True', description='Whether to execute gzclient)'
    )

    declare_world_cmd = DeclareLaunchArgument(
        'world',
        default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'),
        description='Full path to world model file to load',
    )

    declare_robot_name_cmd = DeclareLaunchArgument(
        'robot_name', default_value='turtlebot3_waffle', description='name of the robot'
    )

    declare_robot_sdf_cmd = DeclareLaunchArgument(
        'robot_sdf',
        default_value=os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro'),
        description='Full path to robot sdf file to spawn the robot in gazebo',
    )

    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
    with open(urdf, 'r') as infp:
        robot_description = infp.read()

    start_robot_state_publisher_cmd = Node(
        condition=IfCondition(use_robot_state_pub),
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        namespace=namespace,
        output='screen',
        parameters=[
            {'use_sim_time': use_sim_time, 'robot_description': robot_description}
        ],
        remappings=remappings,
    )

    rviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
        condition=IfCondition(use_rviz),
        launch_arguments={
            'namespace': namespace,
            'use_namespace': use_namespace,
            'use_sim_time': use_sim_time,
            'rviz_config': rviz_config_file,
        }.items(),
    )

    bringup_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
        launch_arguments={
            'namespace': namespace,
            'use_namespace': use_namespace,
            'slam': slam,
            'map': map_yaml_file,
            'use_sim_time': use_sim_time,
            'params_file': params_file,
            'autostart': autostart,
            'use_composition': use_composition,
            'use_respawn': use_respawn,
        }.items(),
    )
    # The SDF file for the world is a xacro file because we wanted to
    # conditionally load the SceneBroadcaster plugin based on wheter we're
    # running in headless mode. But currently, the Gazebo command line doesn't
    # take SDF strings for worlds, so the output of xacro needs to be saved into
    # a temporary file and passed to Gazebo.
    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
    world_sdf_xacro = ExecuteProcess(
        cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
    gazebo_server = ExecuteProcess(
        cmd=['gz', 'sim', '-r', '-s', world_sdf],
        output='screen',
        condition=IfCondition(use_simulator)
    )

    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
        on_shutdown=[
            OpaqueFunction(function=lambda _: os.remove(world_sdf))
        ]))

    gazebo_client = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('ros_gz_sim'),
                         'launch',
                         'gz_sim.launch.py')
        ),
        condition=IfCondition(PythonExpression(
            [use_simulator, ' and not ', headless])),
        launch_arguments={'gz_args': ['-v4 -g ']}.items(),
    )

    gz_robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')),
        launch_arguments={'namespace': namespace,
                          'use_sim_time': use_sim_time,
                          'robot_name': robot_name,
                          'robot_sdf': robot_sdf,
                          'x_pose': pose['x'],
                          'y_pose': pose['y'],
                          'z_pose': pose['z'],
                          'roll': pose['R'],
                          'pitch': pose['P'],
                          'yaw': pose['Y']}.items())

    # Create the launch description and populate
    ld = LaunchDescription()

    # Declare the launch options
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_namespace_cmd)
    ld.add_action(declare_slam_cmd)
    ld.add_action(declare_map_yaml_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_use_composition_cmd)

    ld.add_action(declare_rviz_config_file_cmd)
    ld.add_action(declare_use_simulator_cmd)
    ld.add_action(declare_use_robot_state_pub_cmd)
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(declare_simulator_cmd)
    ld.add_action(declare_world_cmd)
    ld.add_action(declare_robot_name_cmd)
    ld.add_action(declare_robot_sdf_cmd)
    ld.add_action(declare_use_respawn_cmd)

    ld.add_action(world_sdf_xacro)
    ld.add_action(remove_temp_sdf_file)
    ld.add_action(gz_robot)
    ld.add_action(gazebo_server)
    ld.add_action(gazebo_client)

    # Add the actions to launch all of the navigation nodes
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(rviz_cmd)
    ld.add_action(bringup_cmd)

    return ld

The highlighted lines point out to these important files. These will guide the upcoming discussion.

• params/nav2_params.yaml

• maps/tb3_sandbox.yaml

• worlds/tb3_sandbox.sdf.xacro

• urdf/gz_waffle.sdf.xacro

• urdf/turtlebot3_waffle.urdf

Navigation

See also

Official documentation: https://docs.nav2.org/configuration/index.html

In the example, the navigation is defined in the following configuration file.

Contents of nav2_params.yaml

amcl:
  ros__parameters:
    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: "nav2_amcl::DifferentialMotionModel"
    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
    scan_topic: scan

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.

    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
    # Built-in plugins are added automatically
    # plugin_lib_names: []

    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    costmap_update_timeout: 0.30
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      ax_max: 3.0
      ax_min: -3.0
      ay_max: 3.0
      ay_min: -3.0
      az_max: 3.5
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: -0.35
      vy_max: 0.5
      wz_max: 1.9
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.2
      critics: [
        "ConstraintCritic", "CostCritic", "GoalCritic",
        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
        "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        near_collision_cost: 253
        critical_cost: 300.0
        consider_footprint: false
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.70
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.7
      always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
# map_server:
#   ros__parameters:
#     yaml_filename: ""

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    costmap_update_timeout: 1.0
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

route_server:
  ros__parameters:
    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
    # file & provide full path to map below. If graph config or launch default is provided, it is used
    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
    boundary_radius_to_achieve_node: 1.0
    radius_to_achieve_node: 2.0
    smooth_corners: true
    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
    ReroutingService:
      plugin: "nav2_route::ReroutingService"
    AdjustSpeedLimit:
      plugin: "nav2_route::AdjustSpeedLimit"
    CollisionMonitor:
      plugin: "nav2_route::CollisionMonitor"
      max_collision_dist: 3.0
    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
    DistanceScorer:
      plugin: "nav2_route::DistanceScorer"
    CostmapScorer:
      plugin: "nav2_route::CostmapScorer"

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    stamp_smoothed_velocity_with_smoothing_time: False
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.5, 0.0, 2.0]
    min_velocity: [-0.5, 0.0, -2.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

collision_monitor:
  ros__parameters:
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.2
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: False
      enabled: True
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True

docking_server:
  ros__parameters:
    controller_frequency: 50.0
    initial_perception_timeout: 5.0
    wait_charge_timeout: 5.0
    dock_approach_timeout: 30.0
    undock_linear_tolerance: 0.05
    undock_angular_tolerance: 0.1
    max_retries: 3
    base_frame: "base_link"
    fixed_frame: "odom"
    dock_backwards: false
    dock_prestaging_tolerance: 0.5

    # Types of docks
    dock_plugins: ['simple_charging_dock']
    simple_charging_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
      docking_threshold: 0.05
      staging_x_offset: -0.7
      use_external_detection_pose: true
      use_battery_status: false # true
      use_stall_detection: false # true

      external_detection_timeout: 1.0
      external_detection_translation_x: -0.18
      external_detection_translation_y: 0.0
      external_detection_rotation_roll: -1.57
      external_detection_rotation_pitch: -1.57
      external_detection_rotation_yaw: 0.0
      filter_coef: 0.1

    # Dock instances
    # The following example illustrates configuring dock instances.
    # docks: ['home_dock']  # Input your docks here
    # home_dock:
    #   type: 'simple_charging_dock'
    #   frame: map
    #   pose: [0.0, 0.0, 0.0]

    controller:
      k_phi: 3.0
      k_delta: 2.0
      v_linear_min: 0.15
      v_linear_max: 0.15
      use_collision_detection: true
      costmap_topic: "local_costmap/costmap_raw"
      footprint_topic: "local_costmap/published_footprint"
      transform_tolerance: 0.1
      projection_time: 5.0
      simulation_step: 0.1
      dock_collision_threshold: 0.3

loopback_simulator:
  ros__parameters:
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    map_frame_id: "map"
    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
    update_duration: 0.02
    scan_range_min: 0.05
    scan_range_max: 30.0
    scan_angle_min: -3.1415
    scan_angle_max: 3.1415
    scan_angle_increment: 0.02617
    scan_use_inf: true

Once more, there is a trade-off between generality and the complexity of the configuration file. We can unpack the major elements of the configuration in the table below.

In nav2, each functionality is divided into a so-called server. Each server will have its own set of parameters and will communicate with other servers. In this topology, ideally one functionality can be modified without affecting other servers. These are not all servers available in nav2, but all the servers used in this example.

Server	TL;DR	Link
amcl	Adaptive Monte-Carlo Localiser	docs
bt_navigator	Behavior tree navigator	docs
controller_server	Controller server	docs
local_costmap	A 2D costmap	docs
global_costmap	A 2D costmap	docs
planner_server	Calculates path to goal	docs
smoother_server	Keeps path smooth	docs
behavior_server	Defines basic robot behaviors	docs
route_server	Handles pre-defined routes	docs
velocity_smoother	Smooth velocities sent to robots	docs
collision_monitor	Extra layer of safety	docs

Each server can be attached to one or more nav2 plugins. The plugins will define important aspects of the server and plugins will have their own parameters.

See also

Official documentation: https://docs.nav2.org/plugins/index.html

In nav2, mainly, there will be a global planner (in this case nav2_navfn_planner::NavfnPlanner) and a local controller (in this case nav2_mppi_controller::MPPIController). In general terms, the global planner can find solutions when the goal is far and create a trajectory. The controller will be responsible for dealing with following the trajectory in the short term. This split is common in robotics. Both will use costmaps, described in a following section.

The trajectories generated by the global planner might not be smooth owing to several factors, including the parameters used to adjust execution speed. This possible jaggedness can be countered with a smoother_server. Analogously, the velocity calculated by the controller might not be smooth and, if applied directly, cause the robot to vibrate, destabilise, wear, or break. The velocity_smoother should help to address this.

Lastly, the behavior_server will describe basic robot behaviours such as Wait, or Move Straight for a given robot. The bt_navigator will utilise these, among other things, to decide higher level behaviour beyond the planner. A behaviour tree can be seen as a more general state machine. Thence, as the name implies, each behaviour can branch into different outcomes defined for each behaviour. For instance, if the robot fails reaching a goal, it can be set to do another task or attempt to replan to another location.

map

The contents of tb3_sandbox.yaml are shown below.

image: tb3_sandbox.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

The map will have several parameters. The global costmap is defined in tb3_sandbox.pgm.

A costmap in nav2 is a 2D grid in which each cell is assigned a value, similarly to an image. Using the image analogy, each costmap (image) will be made of costs (pixels). The value in each pixel will define how costly it is to move through that pixel.

Suppose that we have the small costmap below, for illustrative purposes. Suppose that we use F for free space, U for unknown, and O for occupied. We can see this as a top-view image of the path that the robot can traverse. The planner and controller will use this information to define how and where to navigate.

O	O	O	O	U
O	F	F	O	U
O	O	F	O	U
O	F	F	O	U
O	O	O	O	U

For larger maps, and to consolidate the image analogy, this is one possible representation of tb3_sandbox.pgm. In this representation, the dark pixels represent occupied regions, such as walls. The white pixel represents free movement. The grey pixels represent unknown regions. This will be part of the so-called nav2_costmap_2d::StaticLayer, representing these three states.

Below is a representation of tb3_sandbox.pgm when opened in an image viewer.

Given that we are unable to fully entrust the safety of the robot to our carefully-drawn maps, nav2 also allows the use of a nav2_costmap_2d::ObstacleLayer. This will be tied to one of the robot sensors, for instance, a laser scanner. This will allow the robot to avoid obstacles when the map itself was inaccurate, for instance, if an obstacle has not been accounted for, or account for inaccuracies in the localisation itself. The nav2_costmap_2d::VoxelLayer is used in this example for a similar purpose, with the difference that the sensor relays 3D information that is then transformed into relevant 2D information.

Lastly, there will be the nav2_costmap_2d::InflationLayer. Given that the maps hold only three possible states for each pixel, the changes are sudden. The inflation layer will behave similarly to a smoother. The region around each occupied cell in the grid will be modified to show that, although that is indeed free space, the robot should not be going through there. By assigning these costs when approaching an obstacle, planners and controllers can devise better trajectories.

The navigation file also had a local costmap, which works similarly, but can have different parameters and accept data from different sources. The reason for this is that it is more convenient to have individual parameters for the planner and the controller.

World and robot definition files

As you might remember from the .sdf contents, they are XML files. These files, by default, do not take advantage of programming concepts, such as macros and conditionals. This becomes an issue when you want to make a robot description file that is slightly more general and can be easily modified with specific parameters. Therefore you are likely to see xacro being used in many ROS examples.

For instance, the tb3_sandbox.sdf.xacro file’s contents can be modified by the parameter headless. Where the parameter is defined and where it’s used are highlighted in the file below. In this example, this might be a quick patch given that Gazebo does not seem to support it and xacro will pre-process the file to output a compliant .sdf based on the parameter value.

The contents of tb3_sandbox.sdf.xacro.

<?xml version="1.0"?>
<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:arg name="headless" default="true"/>

  <world name="default">
    <plugin
      filename="gz-sim-physics-system"
      name="gz::sim::systems::Physics">
    </plugin>
    <plugin
      filename="gz-sim-user-commands-system"
      name="gz::sim::systems::UserCommands">
    </plugin>
  <xacro:unless value="$(arg headless)">
    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>
  </xacro:unless>
    <plugin
      filename="gz-sim-sensors-system"
      name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin
      filename="gz-sim-imu-system"
      name="gz::sim::systems::Imu">
    </plugin>

    <light name='sun' type='directional'>
        <cast_shadows>0</cast_shadows>
        <pose>0 0 10 0 0 0</pose>
        <diffuse>0.8 0.8 0.8 1</diffuse>
        <specular>0.8 0.8 0.8 1</specular>
        <attenuation>
            <range>1000</range>
            <constant>0.9</constant>
            <linear>0.01</linear>
            <quadratic>0.001</quadratic>
        </attenuation>
        <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name='ground_plane'>
        <static>1</static>
        <link name='link'>
            <collision name='collision'>
                <geometry>
                    <plane>
                        <normal>0 0 1</normal>
                        <size>100 100</size>
                    </plane>
                </geometry>
                <max_contacts>10</max_contacts>
            </collision>
            <visual name='visual'>
                <geometry>
                    <plane>
                        <normal>0 0 1</normal>
                        <size>100 100</size>
                    </plane>
                </geometry>
                <material>
                    <ambient>0.8 0.8 0.8 1</ambient>
                    <diffuse>0.8 0.8 0.8 1</diffuse>
                    <specular>0.8 0.8 0.8 1</specular>
                </material>
            </visual>
        </link>
    </model>

    <scene>
      <shadows>0</shadows>
    </scene>

    <physics name='3ms' type='ode'>
        <max_step_size>0.003</max_step_size>
        <real_time_factor>1</real_time_factor>
    </physics>

    <model name="turtlebot3_world">
      <static>1</static>
      <include>
        <uri>model://turtlebot3_world</uri>
      </include>
    </model>

  </world>
</sdf>

Something similar is done to allow Gazebo topics to have a different namespace. This is helpful when multiple robots are added to the same Gazebo world.

The contents of gz_waffle.sdf.xacro.

<?xml version="1.0"?>
<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:arg name="namespace" default=""/>

    <model name="turtlebot3_waffle"> 
      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>

      <link name="base_footprint"/>

      <link name="base_link">

        <inertial> 
          <pose>-0.064 0 0.048 0 0 0</pose>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.001</iyy>
            <iyz>0.000</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>

        <collision name="base_collision">
          <pose>-0.064 0 0.048 0 0 0</pose>
          <geometry>
            <box>
              <size>0.265 0.265 0.089</size>
            </box>
          </geometry>
        </collision>

        <visual name="base_visual">
          <pose>-0.064 0 0 0 0 0</pose>
          <geometry>
            <mesh>
              <uri>package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/waffle_base.dae</uri>
              <scale>0.001 0.001 0.001</scale>
            </mesh>
          </geometry>
          <material>
            <diffuse>1 1 1</diffuse>
          </material>
        </visual>
      </link>

      <link name="imu_link">
        <sensor name="tb3_imu" type="imu">
          <always_on>true</always_on>
          <update_rate>200</update_rate>
          <topic>$(arg namespace)/imu</topic>
          <gz_frame_id>imu_link</gz_frame_id>
          <imu>
            <angular_velocity>
              <x>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>2e-4</stddev>
                </noise>
              </x>
              <y>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>2e-4</stddev>
                </noise>
              </y>
              <z>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>2e-4</stddev>
                </noise>
              </z>
            </angular_velocity>
            <linear_acceleration>
              <x>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>1.7e-2</stddev>
                </noise>
              </x>
              <y>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>1.7e-2</stddev>
                </noise>
              </y>
              <z>
                <noise type="gaussian">
                  <mean>0.0</mean>
                  <stddev>1.7e-2</stddev>
                </noise>
              </z>
            </linear_acceleration>
          </imu>
        </sensor>
      </link>

      <link name="base_scan">
        <inertial>
          <pose>-0.064 0 0.121 0 0 0</pose>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.001</iyy>
            <iyz>0.000</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.125</mass>
        </inertial>

        <collision name="lidar_sensor_collision">
          <pose>-0.052 0 0.111 0 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.0508</radius>
              <length>0.055</length>
            </cylinder>
          </geometry>
        </collision>

        <visual name="lidar_sensor_visual">
          <pose>-0.064 0 0.121 0 0 0</pose>
          <geometry>
            <mesh>
              <uri>package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/lds.dae</uri>
              <scale>0.001 0.001 0.001</scale>
            </mesh>
          </geometry>
          <material>
            <diffuse>1 1 1</diffuse>
          </material>
        </visual>

        <sensor name="hls_lfcd_lds" type="gpu_lidar">
          <always_on>true</always_on>
          <visualize>true</visualize>
          <pose>-0.064 0 0.15 0 0 0</pose>
          <update_rate>5</update_rate>
          <topic>$(arg namespace)/scan</topic>
          <gz_frame_id>base_scan</gz_frame_id>
          <ray>
            <scan>
              <horizontal>
                <samples>360</samples>
                <resolution>1.000000</resolution>
                <min_angle>0.000000</min_angle>
                <max_angle>6.280000</max_angle>
              </horizontal>
            </scan>
            <range>
              <min>0.00001</min>
              <max>20.0</max>
              <resolution>0.015000</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.01</stddev>
            </noise>
          </ray>
        </sensor>
      </link>

      <link name="wheel_left_link">

        <inertial>
          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.001</iyy>
            <iyz>0.000</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>

        <collision name="wheel_left_collision">
          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.033</radius>
              <length>0.018</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
            <contact>
              <ode>
                <soft_cfm>0</soft_cfm>
                <soft_erp>0.2</soft_erp>
                <kp>1e+5</kp>
                <kd>1</kd>
                <max_vel>0.01</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
          </surface>
        </collision>

        <visual name="wheel_left_visual">
          <pose>0.0 0.144 0.023 0 0 0</pose>
          <geometry>
            <mesh>
              <uri>package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae</uri>
              <scale>0.001 0.001 0.001</scale>
            </mesh>
          </geometry>
          <material>
            <diffuse>1 1 1</diffuse>
          </material>
        </visual>
      </link>

      <link name="wheel_right_link">

        <inertial>
          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.001</iyy>
            <iyz>0.000</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>
      
        <collision name="wheel_right_collision">
          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.033</radius>
              <length>0.018</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
            <contact>
              <ode>
                <soft_cfm>0</soft_cfm>
                <soft_erp>0.2</soft_erp>
                <kp>1e+5</kp>
                <kd>1</kd>
                <max_vel>0.01</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
          </surface>
        </collision>

        <visual name="wheel_right_visual">
          <pose>0.0 -0.144 0.023 0 0 0</pose>
          <geometry>
            <mesh>
              <uri>package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae</uri>
              <scale>0.001 0.001 0.001</scale>
            </mesh>
          </geometry>
          <material>
            <diffuse>1 1 1</diffuse>
          </material>
        </visual>
      </link>

      <link name='caster_back_right_link'>
        <pose>-0.177 -0.064 -0.004 0 0 0</pose>
        <inertial>
          <mass>0.001</mass>
          <inertia>
            <ixx>0.00001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.00001</iyy>
            <iyz>0.000</iyz>
            <izz>0.00001</izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.006000</radius>
            </sphere>
          </geometry>
          <surface>
            <contact>
              <ode>
                <soft_cfm>0</soft_cfm>
                <soft_erp>0.2</soft_erp>
                <kp>1e+5</kp>
                <kd>1</kd>
                <max_vel>0.01</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
          </surface>
        </collision>
      </link>

      <link name='caster_back_left_link'>
        <pose>-0.177 0.064 -0.004 0 0 0</pose>
        <inertial>
          <mass>0.001</mass>
          <inertia>
            <ixx>0.00001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.00001</iyy>
            <iyz>0.000</iyz>
            <izz>0.00001</izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.006000</radius>
            </sphere>
          </geometry>
          <surface>
            <contact>
              <ode>
                <soft_cfm>0</soft_cfm>
                <soft_erp>0.2</soft_erp>
                <kp>1e+5</kp>
                <kd>1</kd>
                <max_vel>0.01</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
          </surface>
        </collision>
      </link>

      <link name="camera_link">
        <inertial>
          <pose>0.069 -0.047 0.107 0 0 0</pose>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>0.001</iyy>
            <iyz>0.000</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.035</mass>
        </inertial>
        <collision name="collision">
          <pose>0 0.047 -0.005 0 0 0</pose>
          <geometry>
            <box>
              <size>0.008 0.130 0.022</size>
            </box>
          </geometry>
        </collision>

        <pose>0.069 -0.047 0.107 0 0 0</pose>

        <sensor name="intel_realsense_r200_depth" type="depth">
          <always_on>1</always_on>
          <update_rate>5</update_rate>
          <pose>0.064 -0.047 0.107 0 0 0</pose>
          <gz_frame_id>camera_depth_frame</gz_frame_id>
          <camera name="realsense_depth_camera">
            <horizontal_fov>1.047</horizontal_fov>
            <image>
              <width>320</width>
              <height>240</height>
            </image>
            <lens>
              <projection>
                <!-- focal_length = fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
                <!-- tx = hackBaseline * focal_length -->
                <tx>19.4</tx>
              </projection>
            </lens>
            <depth_camera>
              <clip>
                <near>0.001</near>
                <far>5.0</far>
              </clip>
            </depth_camera>
          </camera>
        </sensor>
      </link>

      <joint name="base_joint" type="fixed">
        <parent>base_footprint</parent>
        <child>base_link</child>
        <pose>0.0 0.0 0.010 0 0 0</pose>
      </joint>

      <joint name="wheel_left_joint" type="revolute">
        <parent>base_link</parent>
        <child>wheel_left_link</child>
        <pose>0.0 0.144 0.023 -1.57 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>

      <joint name="wheel_right_joint" type="revolute">
        <parent>base_link</parent>
        <child>wheel_right_link</child>
        <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>

      <joint name='caster_back_right_joint' type='ball'>
        <parent>base_link</parent>
        <child>caster_back_right_link</child>
      </joint>

      <joint name='caster_back_left_joint' type='ball'>
        <parent>base_link</parent>
        <child>caster_back_left_link</child>
      </joint>

      <joint name="lidar_joint" type="fixed">
        <parent>base_link</parent>
        <child>base_scan</child>
        <pose>-0.064 0 0.121 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>

      <joint name="camera_joint" type="fixed">
        <parent>base_link</parent>
        <child>camera_link</child>
        <pose>0.064 -0.065 0.094 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>

      <joint name="imu_joint" type="fixed">
        <parent>base_link</parent>
        <child>imu_link</child>
        <pose>0.0 0 0.068 0 0 0</pose>
      </joint>


      <plugin
        filename="gz-sim-diff-drive-system"
        name="gz::sim::systems::DiffDrive">
        <left_joint>wheel_left_joint</left_joint>
        <right_joint>wheel_right_joint</right_joint>
        <wheel_separation>0.287</wheel_separation>
        <wheel_radius>0.033</wheel_radius>
        <max_linear_acceleration>1</max_linear_acceleration>
        <min_linear_acceleration>-1</min_linear_acceleration>
        <max_angular_acceleration>2</max_angular_acceleration>
        <min_angular_acceleration>-2</min_angular_acceleration>
        <max_linear_velocity>0.46</max_linear_velocity>
        <min_linear_velocity>-0.46</min_linear_velocity>
        <max_angular_velocity>1.9</max_angular_velocity>
        <min_angular_velocity>-1.9</min_angular_velocity>
        <topic>$(arg namespace)/cmd_vel</topic>
        <odom_topic>$(arg namespace)/odom</odom_topic>
        <tf_topic>$(arg namespace)/tf</tf_topic>
        <frame_id>odom</frame_id>
        <child_frame_id>base_footprint</child_frame_id>
        <odom_publish_frequency>30</odom_publish_frequency>
      </plugin>

      <plugin
        filename="gz-sim-joint-state-publisher-system"
        name="gz::sim::systems::JointStatePublisher">
        <joint_name>wheel_left_joint</joint_name>
        <joint_name>wheel_right_joint</joint_name>
        <topic>$(arg namespace)/joint_states</topic>
        <update_rate>30</update_rate>
      </plugin>

    </model>
</sdf>

Although .sdf files are meant to replace, or supersede, .urdf files in the long term, the process is still ongoing or under discussion. It is likely that you will see .urdf files heavily used in many ROS2 packages. Nonetheless, .urdf are close in syntax to .sdf files and should be no surprise.

The contents of turtlebot3_waffle.urdf.

<?xml version="1.0" ?>
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/>

  <xacro:property name="r200_cam_rgb_px" value="0.005"/>
  <xacro:property name="r200_cam_rgb_py" value="0.018"/>
  <xacro:property name="r200_cam_rgb_pz" value="0.013"/>
  <xacro:property name="r200_cam_depth_offset" value="0.01"/> -->

  <!-- Init colour -->
  <material name="black">
      <color rgba="0.0 0.0 0.0 1.0"/>
  </material>

  <material name="dark">
    <color rgba="0.3 0.3 0.3 1.0"/>
  </material>

  <material name="light_black">
    <color rgba="0.4 0.4 0.4 1.0"/>
  </material>

  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>

  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>

  <material name="grey">
    <color rgba="0.5 0.5 0.5 1.0"/>
  </material>

  <material name="orange">
    <color rgba="1.0 0.4235 0.0392 1.0"/>
  </material>

  <material name="brown">
    <color rgba="0.8706 0.8118 0.7647 1.0"/>
  </material>

  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>

  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>

  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link" />
    <origin xyz="0 0 0.010" rpy="0 0 0"/>
  </joint>

  <link name="base_link">
    <visual>
      <origin xyz="-0.064 0 0.0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/waffle_base.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="light_black"/>
    </visual>

    <collision>
      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
      <geometry>
        <box size="0.266 0.266 0.094"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1.3729096e+00"/>
      <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
               iyy="8.6195418e-03" iyz="-3.5422299e-06"
               izz="1.4612727e-02" />
    </inertial>
  </link>

  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left_link"/>
    <origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_left_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
      </inertial>
  </link>

  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right_link"/>
    <origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_right_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
      </inertial>
  </link>

  <joint name="caster_back_right_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_right_link"/>
    <origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_right_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="caster_back_left_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_left_link"/>
    <origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_left_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.0 0 0.068" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>

  <joint name="scan_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_scan"/>
    <origin xyz="-0.064 0 0.122" rpy="0 0 0"/>
  </joint>

  <link name="base_scan">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/lds.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.0315" radius="0.055"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.114" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="camera_joint" type="fixed">
    <origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <link name="camera_link">
    <visual>
     <origin xyz="0 0 0" rpy="1.57 0 1.57"/>
      <geometry>
        <mesh filename="package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/r200.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.003 0.065 0.007" rpy="0 0 0"/>
      <geometry>
        <box size="0.012 0.132 0.020"/>
      </geometry>
    </collision>

    <!-- This inertial field needs doesn't contain reliable data!! -->
<!--   <inertial>
      <mass value="0.564" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
               iyy="0.000498940" iyz="0.0"
               izz="0.003879257" />
    </inertial>-->
  </link>

  <joint name="camera_rgb_joint" type="fixed">
    <origin xyz="0.005 0.018 0.013" rpy="0 0 0"/>
    <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
    <parent link="camera_link"/>
    <child link="camera_rgb_frame"/>
  </joint>
  <link name="camera_rgb_frame"/>

  <joint name="camera_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
    <parent link="camera_rgb_frame"/>
    <child link="camera_rgb_optical_frame"/>
  </joint>
  <link name="camera_rgb_optical_frame"/>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="0.005 0.028 0.013" rpy="-1.57 0 -1.57"/>
    <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
    <parent link="camera_link"/>
    <child link="camera_depth_frame"/>
  </joint>
  <link name="camera_depth_frame"/>

  <joint name="camera_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="camera_depth_frame"/>
    <child link="camera_depth_optical_frame"/>
  </joint>
  <link name="camera_depth_optical_frame"/>

</robot>

So what?

After looking through this example you have probably noticed the complexity involved into setting up nav2 for even relatively simple projects. Having a highly configurable system increases the number of possibilities, which is great when applying it to other settings, but might be daunting at first.

Here are some examples of what you could modify in this example and what would be the estimated difficulty of doing so.

• Changing the map. Not as trivial as it might sound, because the map needs to match the Gazebo simulation scene. Some community tools might help with that.

• Changing the Gazebo scene. Same problem as above, although you might benefit from SLAM, it is not likely to create a perfect map.

• Changing the robot. Online tutorials and official repositories might say that it’s a matter of “changing the urdf file”. Unfortunately, this might mean lots of other things will change, including the navigation parameters depending, for instance, on sensors and footprint of your robot.

• Changing planners or controllers in nav2. That is going to be easier generally, but the parameters of one planner or controller might not map exactly to another one. It might take some tinkering to get it to work.

In conclusion, using this section, you have learned the basics of nav2 from an official example. We went through the importance of each file and now you should have a conceptual knowledge beyond the basics for this navigation stack.

Navigation with SLAM

See also

Official documentation: https://docs.nav2.org/tutorials/docs/navigation2_with_slam.html#navigation2-with-slam

In the previous example, we looked into navigating through a completely known map. This is a common problem to solve in robotics, but definitely not the only issue. Commonly, we have partial or no knowledge of the map through which a robot has to traverse. This is where SLAM kicks in. It becomes a problem of navigating through an incomplete and dynamic map.

In this example, they utilise the slam toolbox. Going into the details of SLAM is beyond the scope of this tutorial, but there are some interesting aspects to learn from this example even with the basic concepts of navigation.

We can run the demo with SLAM using the code below. Please note that the only difference is the flag slam:=True.

ros2 launch nav2_bringup \
tb3_simulation_launch.py \
use_sim_time:=True \
headless:=False \
sigterm_timeout:=120 \
slam:=True

You can behold my incredible map completion skills in the video below. As you will notice it will start empty and be completed as we move around it. Using our knowledge of the environment we can know where to go to complete the map and have good coverage. In practice, this can be challenging when the environment is completely unknown and has more complex topology.

Running the same demonstration a second time, we can see how to save an incomplete map. The map will be saved in the folder in which you executed ros2 launch.

The maps will be saved into two files, compliant with nav2 use. The first one we saved will be the .yaml file.

image: incomplete.pgm
mode: trinary
resolution: 0.050
origin: [-0.934, -2.059, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

The second one will be the map. When opened in a image viewer, this is how it looks like, showing its incomplete state.

This example is illustrative of the capacity to create maps using SLAM. This might not look like much, at first glance. However, please take this time to reflect that this mapping is happening with simulated data coming from Gazebo. Therefore, it is as representative of reality as the programs we have used so far allow us to be at this point.