🧭 Navigation with Nav2

O Nav2 is the set of ROS 2 navigation packages. It isn't a single node: it's a team of servers, each with a task. One loads the map, another figures out where the robot is, another plots the route across the whole map, another decides the speed at each moment, and another tries to free the robot if it gets stuck. The team is coordinated by the BT Navigator, following a behavior tree.

How to read this map: the goal comes in at the top, and the BT Navigator assigns work to the three amber servers. The dashed cyan arrows show supporting data (map, position, and costs). The only thing that leaves Nav2 for the outside world is the speed on /cmd_vel.

Goal: see the whole stack working in the simulated TurtleBot before using your robot. If you installed the packages in module 4.4, skip apt.

# <distro> = jazzy ou lyrical
sudo apt install ros-<distro>-navigation2 ros-<distro>-nav2-bringup ros-<distro>-nav2-minimal-tb*
source /opt/ros/<distro>/setup.bash

# Gazebo + RViz2 + Nav2 completo, já com um mapa de exemplo
# (se não existir, liste a pasta launch do nav2_bringup e use o tb3_simulation_launch.py)
ros2 launch nav2_bringup tb4_simulation_launch.py headless:=False

# em outro terminal: os servidores que subiram
ros2 node list | grep -E "planner|controller|bt_navigator|amcl|behavior"

How to check: RViz2 opens with the map, the robot, and the “2D Pose Estimate” and “Nav2 Goal” buttons in the toolbar; the node list shows planner_server, controller_server, bt_navigator, amcl e behavior_server.

Why learn this

When the robot “doesn't move,” knowing what each server does turns a mystery into a clear question: does it not know where it is (AMCL), can't find a route (planner), or can it find a route but not follow it (controller)?

Key concepts

With the map ready, the robot no longer needs SLAM: it only needs to figure out where it is within a map it already knows. In Nav2, the component that does this is AMCL. It spreads hundreds of position “guesses” (particles) across the map, compares what the LiDAR should see at each guessed position with what it actually sees, and keeps the guesses that match. As the robot moves, the cloud shrinks around its real position.

How to read this map: from left to right, the robot moves and AMCL discards the guesses that don't match the LiDAR. The middle panel shows the tricky case: two identical corners. Moving farther (or setting the initial position) resolves the uncertainty.

Goal: start Nav2 with your saved map and tell it where the robot is. In simulation, add use_sim_time:=True.

# terminal 1: Nav2 with the map from module 4.4 (the robot already publishes /scan, /odom and TF)
source /opt/ros/<distro>/setup.bash
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml

# terminal 2: RViz2 with the Nav2 configuration
ros2 launch nav2_bringup rviz_launch.py

# alternativa ao botão "2D Pose Estimate": robô na origem do mapa, virado para x
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: 0.0, y: 0.0}, orientation: {w: 1.0}}}}"

Why learn this

All navigation depends on accurate localization. If the robot thinks it’s half a meter to the left, it will plan a perfect route to somewhere it isn’t—and hit the doorframe.

Key concepts

The saved map only says “free” or “wall.” To plan, Nav2 needs more: the cost of passing through each cell. This is the costmap It’s built from stacked layers: the static layer comes from the saved map, the obstacle layer comes from the live LiDAR (the chair that moved), and the inflation layer creates a “buffer” around everything—expensive near the wall and cheap in the middle of the corridor.

How to read this map: Think of three transparent sheets stacked on top of each other. The top one never changes; the middle one changes with every LiDAR scan; the bottom one “spreads” cost around whatever the others mark. The radius of this amber band is the parameter you’ll adjust most often.

There are two costmaps. The global covers the whole map and is used by the planner, which plots the route from point A to B (algorithms such as NavFn or Smac). The local is a small window that moves with the robot and is used by the controller, which chooses the speed at each moment to follow the route without colliding (algorithms such as DWB, MPPI, or Regulated Pure Pursuit).

How to read this map: The dashed amber line is the plan across the whole map, passing through the door. A new box has appeared inside the cyan rectangle (the local costmap): the controller takes the cyan detour around it and returns to the route without having to replan everything.

Goal: Create your parameter file from the Nav2 default and adjust the robot size, buffer, and maximum speed. The names below are in the default file; their exact location varies between versions, so use your editor’s search.

# 1) copy the default file to edit
cp $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/params/nav2_params.yaml ~/meu_nav2.yaml

# 2) in ~/meu_nav2.yaml, find and adjust (in BOTH costmaps, global and local):
#    robot_radius: 0.15          robot radius in meters (measure yours)
#    inflation_radius: 0.35      buffer around walls and objects
#    cost_scaling_factor: 3.0    higher = cost drops faster farther from the wall
# 3) in velocity_smoother, limit the speed [x, y, rotation]:
#    max_velocity: [0.2, 0.0, 1.0]

# 4) start Nav2 with your file
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml params_file:=$HOME/meu_nav2.yaml

How to check: In RViz2, enable the “Global Costmap” and “Local Costmap” layers: the colored band around the walls should grow or shrink as you change inflation_radius. With ros2 topic echo /cmd_vel during navigation, the linear.x does not exceed 0.2.

Why learn this

Almost every strange Nav2 behavior (scraping past things, refusing a doorway, zigzagging down a corridor) can be explained by looking at the two costmaps in RViz2. They’re the navigation “X-ray.”

Key concepts

The quickest way to test is the “Nav2 Goal” button in RViz2: click the destination and drag to indicate which way the robot should face. For a robot that performs tasks on its own, you’ll want to send the same request in code. The package nav2_simple_commander provides the BasicNavigatorclass, which wraps the ROS 2 action in simple functions such as goToPose.

Goal: A Python script that sets the initial position, waits for Nav2 to be ready, sends the robot to (1.5; 0.5) facing 90°, and shows how far it has left to go. Replace the coordinates with a free point on your map (hover over it in RViz2 to read x and y).

#!/usr/bin/env python3
# Send the robot to a point on the map with Nav2 (nav2_simple_commander)
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult


def pose(nav, x, y, angulo_graus=0.0):
    p = PoseStamped()
    p.header.frame_id = 'map'
    p.header.stamp = nav.get_clock().now().to_msg()
    p.pose.position.x = x
    p.pose.position.y = y
    a = math.radians(angulo_graus)
    p.pose.orientation.z = math.sin(a / 2)
    p.pose.orientation.w = math.cos(a / 2)
    return p


def main():
    rclpy.init()
    nav = BasicNavigator()
    nav.setInitialPose(pose(nav, 0.0, 0.0, 0.0))   # where the robot is at the starting point
    nav.waitUntilNav2Active()                     # wait for AMCL and servers

    nav.goToPose(pose(nav, 1.5, 0.5, 90.0))        # destination: x, y, angle
    while not nav.isTaskComplete():
        fb = nav.getFeedback()
        if fb:
            print(f'Remaining {fb.distance_remaining:.2f} m')

    resultado = nav.getResult()
    if resultado == TaskResult.SUCCEEDED:
        print('Cheguei.')
    elif resultado == TaskResult.CANCELED:
        print('Cancelado.')
    else:
        print('Failed: Nav2 could not reach the destination.')
    rclpy.shutdown()


if __name__ == '__main__':
    main()

To run: with Nav2 already running, in a terminal with source /opt/ros/<distro>/setup.bash, run python3 ir_ate_ponto.py. If the module is missing, install ros-<distro>-nav2-simple-commander with apt.

Why learn this

With the goal in code, another program can decide where the robot goes: a button on your phone (Track 2), an object seen by the camera (Track 3), or a voice command interpreted by a language model (Track 5).

Key concepts

In the real world, the robot will get stuck: a chair blocks its path, someone stops in front of it, or a wheel catches on the rug. Nav2 doesn't give up after the first failure. The behavior tree of the BT Navigator says what to try: first clear the costmap (the obstacle may have gone away), then spin in place so the LiDAR can get a better view, wait, back up a little, and replan. Only after a few attempts is the goal marked as failed.

How to read this map: Read from top to bottom and left to right. The amber branch is the normal path (plan and follow). If it fails, the cyan branch runs the recovery actions in order, and the top node tells it to try navigating again until it runs out of attempts.

Goal: don't let your script wait forever. This version cancels the goal if more than 60 seconds pass. You can test recovery by putting a box (or a block in Gazebo) in the way.

#!/usr/bin/env python3
# Go to a point, but cancel if it takes too long (your program's safety net)
import rclpy
from rclpy.duration import Duration
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

LIMITE_S = 60.0

rclpy.init()
nav = BasicNavigator()
nav.waitUntilNav2Active()            # the initial pose was already set in RViz2

alvo = PoseStamped()
alvo.header.frame_id = 'map'
alvo.header.stamp = nav.get_clock().now().to_msg()
alvo.pose.position.x = 2.0
alvo.pose.position.y = 0.0
alvo.pose.orientation.w = 1.0

inicio = nav.get_clock().now()
cancelado = False
nav.goToPose(alvo)
while not nav.isTaskComplete():
    if not cancelado and nav.get_clock().now() - inicio > Duration(seconds=LIMITE_S):
        print('Took too long: canceling.')
        nav.cancelTask()
        cancelado = True

print('Resultado:', nav.getResult())
rclpy.shutdown()

Why learn this

A robot that only works when the path is perfect never leaves the workbench. Understanding recovery and setting a limit in your own program are what make the difference between a demo and a robot that makes its rounds every day.

Key concepts

A round is a list of waypoints (stops along the way): living room, hallway, kitchen, back to base. Nav2 BasicNavigator has followWaypoints, which visits the points in order and reports which one it's at. If a point fails (a closed door, for example), it moves on to the next one by default and reports which points failed at the end.

How to read this map: the robot leaves the base (B, cyan), passes points 1, 2, and 3 in order, and returns. The red hatched area is the stairs: no waypoint is near them. In module 4.6, you'll also block them on the map.

Goal: a round through three points and back to base, repeated twice. Replace the coordinates with points on your map.

#!/usr/bin/env python3
# Nav2 waypoint round: visits the points in order and returns to base
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

# (name, x, y, angle in degrees) in the map frame
PONTOS = [
    ('corredor', 2.0, 0.0, 0.0),
    ('cozinha', 3.0, 1.5, 90.0),
    ('quarto', 0.5, -1.5, 180.0),
    ('base', 0.0, 0.0, 0.0),
]
VOLTAS = 2


def pose(nav, x, y, graus):
    p = PoseStamped()
    p.header.frame_id = 'map'
    p.header.stamp = nav.get_clock().now().to_msg()
    p.pose.position.x, p.pose.position.y = x, y
    p.pose.orientation.z = math.sin(math.radians(graus) / 2)
    p.pose.orientation.w = math.cos(math.radians(graus) / 2)
    return p


rclpy.init()
nav = BasicNavigator()
nav.waitUntilNav2Active()
for volta in range(1, VOLTAS + 1):
    nav.followWaypoints([pose(nav, x, y, g) for _, x, y, g in PONTOS])
    atual = -1
    while not nav.isTaskComplete():
        fb = nav.getFeedback()
        if fb and fb.current_waypoint != atual:
            atual = fb.current_waypoint
            print(f'Round {volta}: going to {PONTOS[atual][0]}')
    print(f'Round {volta} finished:', nav.getResult())
    if nav.getResult() != TaskResult.SUCCEEDED:
        break
rclpy.shutdown()

Why learn this

Rounds are the basis for security robots, deliveries between rooms, and inventory checks. With a list of points, you already have a "job" for the robot, which will become the project in the next module.

Key concepts