🧭 Navegación con Nav2

O Nav2 es el conjunto de paquetes de navegación de ROS 2. No es un solo nodo: es un equipo de servidores, cada uno con una tarea. Uno carga el mapa, otro descubre dónde está el robot, otro traza la ruta en todo el mapa, otro decide la velocidad en cada instante y otro intenta destrabar el robot. Quien coordina al equipo es el BT Navigator, que sigue un árbol de comportamiento.

Cómo leer: el objetivo entra por arriba y el BT Navigator distribuye el trabajo entre los tres servidores ámbar. Las flechas cian discontinuas representan los datos de apoyo (mapa, posición y costos). Lo único que sale de Nav2 hacia el mundo es la velocidad en /cmd_vel.

Objetivo: ver toda la pila en funcionamiento en el TurtleBot simulado antes de usar tu robot. Si ya instalaste los paquetes en el módulo 4.4, omite el 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, ya con um mapa de ejemplo
# (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"

Cómo verificar: RViz2 se abre con el mapa, el robot y los botones «2D Pose Estimate» y «Nav2 Goal» en la barra; la lista de nodos muestra planner_server, controller_server, bt_navigator, amcl e behavior_server.

Por qué aprender

Cuando el robot «no se mueve», saber qué hace cada servidor transforma un misterio en una pregunta concreta: ¿no sabe dónde está (AMCL), no encuentra una ruta (planner) o encuentra una ruta, pero no puede seguirla (controller)?

Conceptos clave

Con el mapa listo, el robot ya no necesita SLAM: solo necesita descubrir dónde está dentro de un mapa que ya conoce. Quien hace esto en Nav2 es AMCL. Distribuye cientos de «posibles» posiciones (partículas) por el mapa, compara lo que el LiDAR debería ver en cada una con lo que realmente ve y conserva las que coinciden. A medida que el robot avanza, la nube se concentra alrededor de la posición real.

Cómo leer: de izquierda a derecha, el robot avanza y AMCL descarta las posibles posiciones que no coinciden con el LiDAR. El cuadro del medio muestra el caso difícil: dos esquinas iguales. Avanzar un poco más (o indicar la posición inicial) resuelve la duda.

Objetivo: iniciar Nav2 con el mapa guardado e indicar dónde está el robot. En la simulación, agrega use_sim_time:=True.

# terminal 1: Nav2 con el mapa del módulo 4.4 (el robot ya publica /scan, /odom y TF)
source /opt/ros/<distro>/setup.bash
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml

# terminal 2: RViz2 con la configuración de Nav2
ros2 launch nav2_bringup rviz_launch.py

# alternativa al botón "2D Pose Estimate": robot en el origen del mapa, orientado hacia 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}}}}"

Por qué aprender

Toda navegación depende de una buena localización. Si el robot cree que está medio metro a la izquierda, planeará una ruta perfecta hacia un lugar donde no está y chocará con el marco de la puerta.

Conceptos clave

El mapa guardado solo indica "libre" o "pared". Para planificar, Nav2 necesita algo más: cuánto cuesta pasar por cada celda. Este es el mapa de costos (costmap). Se compone de capas sumadas: la capa estática proviene del mapa guardado, la de obstáculos proviene del LiDAR en tiempo real (la silla que cambió de lugar) y la de inflación crea un "margen" alrededor de todo, con un costo alto cerca de la pared y bajo en medio del pasillo.

Cómo leer: Imagina tres hojas transparentes apiladas. La de arriba nunca cambia; la del medio cambia con cada vuelta del LiDAR; la de abajo "distribuye" el costo alrededor de lo que marcan las otras. El radio de esa franja ámbar es el parámetro que más vas a ajustar.

Hay dos mapas de costos. El global cubre todo el mapa y sirve al planner, que traza la ruta del punto A al B (algoritmos como NavFn o Smac). El local es una ventana pequeña que se mueve con el robot y sirve al controller, que elige la velocidad en cada instante para seguir la ruta sin chocar (algoritmos como DWB, MPPI o Regulated Pure Pursuit).

Cómo leer: la línea ámbar discontinua es el plan en todo el mapa, que pasa por la puerta. Dentro del rectángulo cian (el mapa de costos local), apareció una caja nueva: el controlador traza la curva cian alrededor de ella y vuelve a la ruta sin necesidad de recalcular todo.

Objetivo: crear tu archivo de parámetros a partir del archivo predeterminado de Nav2 y ajustar el tamaño del robot, el margen y la velocidad máxima. Los nombres de abajo están en el archivo predeterminado; la ubicación exacta cambia según la versión, así que usa la búsqueda del editor.

# 1) copia el archivo predeterminado para editarlo
cp $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/params/nav2_params.yaml ~/meu_nav2.yaml

# 2) en ~/mi_nav2.yaml, busca y ajusta (en AMBOS costmaps, global y local):
#    robot_radius: 0.15          radio del robot en metros (mide el tuyo)
#    inflation_radius: 0.35      margen alrededor de paredes y objetos
#    cost_scaling_factor: 3.0    un valor mayor = el costo disminuye más rápido al alejarse de la pared
# 3) en velocity_smoother, limita la velocidad [x, y, giro]:
#    max_velocity: [0.2, 0.0, 1.0]

# 4) inicia Nav2 con tu archivo
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml params_file:=$HOME/meu_nav2.yaml

Cómo verificar: en RViz2, activa las capas "Global Costmap" y "Local Costmap": la franja de color alrededor de las paredes debe ensancharse o estrecharse según el inflation_radius. Con ros2 topic echo /cmd_vel durante una navegación, el linear.x no supera 0.2.

Por qué aprender

Casi cualquier comportamiento extraño de Nav2 (pasar rozando, rechazar una puerta, zigzaguear en el pasillo) se explica al mirar los dos mapas de costos en RViz2. Es la "radiografía" de la navegación.

Conceptos clave

La forma más rápida de probar es usar el botón "Nav2 Goal" de RViz2: haz clic en el destino y arrastra para indicar hacia dónde debe quedar orientado el robot. Para un robot que realiza tareas por su cuenta, querrás hacer la misma solicitud mediante código. El paquete nav2_simple_commander incluye la clase BasicNavigator, que oculta la acción de ROS 2 detrás de funciones sencillas como goToPose.

Objetivo: un script de Python que indica la posición inicial, espera a que Nav2 esté listo, envía el robot a (1,5; 0,5) orientado a 90° y muestra cuánto falta. Cambia las coordenadas por un punto libre de tu mapa (pasa el mouse sobre RViz2 para leer x e y).

#!/usr/bin/env python3
# Envía el robot a un punto del mapa con 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))   # dónde está el robot al inicio
    nav.waitUntilNav2Active()                     # espera a AMCL y a los servidores

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

    resultado = nav.getResult()
    if resultado == TaskResult.SUCCEEDED:
        print('Cheguei.')
    elif resultado == TaskResult.CANCELED:
        print('Cancelado.')
    else:
        print('Falló: Nav2 no pudo llegar.')
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Para ejecutar: con Nav2 ya en funcionamiento, en una terminal con source /opt/ros/<distro>/setup.bash, ejecuta python3 ir_ate_ponto.py. Si falta el módulo, instala ros-<distro>-nav2-simple-commander con apt.

Por qué aprender

Con el objetivo en código, otro programa puede decidir adónde va el robot: un botón en el celular (Trilha 2), un objeto detectado por la cámara (Trilha 3) o un comando de voz interpretado por un modelo de lenguaje (Trilha 5).

Conceptos clave

En el mundo real, el robot se va a atascar: una silla bloquea el camino, una persona se detiene enfrente, una rueda se queda atrapada en la alfombra. Nav2 no se rinde ante el primer fallo. La árbol de comportamiento del BT Navigator indica qué intentar: primero limpiar el mapa de costos (puede que el obstáculo ya no esté), después girar en el lugar para que el LiDAR vea mejor, esperar, retroceder un poco y volver a planificar. Solo después de varios intentos se considera que el objetivo falló.

Cómo leer: se lee de arriba abajo y de izquierda a derecha. La rama ámbar es el camino normal (planificar y seguir). Si falla, la rama cian ejecuta las acciones de recuperación en orden y la parte superior indica que se vuelva a intentar navegar, hasta agotar los intentos.

Objetivo: evitar que tu script espere para siempre. Esta versión cancela el objetivo si pasan más de 60 segundos, y puedes probar la recuperación poniendo una caja (un bloque en Gazebo) en el camino.

#!/usr/bin/env python3
# Va hasta un punto, pero cancela si tarda demasiado (red de seguridad de tu programa)
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()            # aquí RViz2 ya proporcionó la posición inicial

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('Tardó demasiado: cancelando.')
        nav.cancelTask()
        cancelado = True

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

Por qué aprender

Un robot que solo funciona cuando el camino está perfecto no sale del banco de trabajo. Entender la recuperación y poner un límite en tu propio programa es lo que distingue una demostración de un robot que patrulla todos los días.

Conceptos clave

Una ronda es una lista de waypoints (puntos de paso): sala, pasillo, cocina y regreso a la base. Nav2 BasicNavigator tiene followWaypoints, que visita los puntos en orden e informa en cuál está. Si falla un punto (puerta cerrada), de forma predeterminada continúa al siguiente y, al final, indica cuáles fallaron.

Cómo leer: el robot sale de la base (B, cian), pasa por los puntos 1, 2 y 3 en orden y regresa. El área rayada en rojo es la escalera: ningún waypoint queda cerca de ella. En el módulo 4.6 también la vas a bloquear en el mapa.

Objetivo: una ronda de tres puntos con regreso a la base, repetida dos veces. Cambia las coordenadas por los puntos de tu mapa.

#!/usr/bin/env python3
# Ronda por waypoints con Nav2: visita los puntos en orden y vuelve a la base
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

# (nombre, x, y, ángulo en grados) en el frame del mapa
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'Vuelta {volta}: yendo a {PONTOS[atual][0]}')
    print(f'Vuelta {volta} terminó:', nav.getResult())
    if nav.getResult() != TaskResult.SUCCEEDED:
        break
rclpy.shutdown()

Por qué aprender

Las rondas son la base de los robots de vigilancia, de entrega entre habitaciones y de inventario. Con una lista de puntos ya tienes un "trabajo" para el robot, que se convertirá en el proyecto del próximo módulo.

Conceptos clave