🧭 Navegação com Nav2

O Nav2 é o conjunto de pacotes de navegação do ROS 2. Ele não é um nó só: é uma equipe de servidores, cada um com uma tarefa. Um carrega o mapa, outro descobre onde o robô está, outro traça a rota no mapa inteiro, outro decide a velocidade a cada instante e outro tenta desatolar o robô. Quem coordena essa equipe é o BT Navigator, seguindo uma árvore de comportamento.

Como ler: o objetivo entra no alto e o BT Navigator distribui o trabalho para os três servidores âmbar. As setas ciano tracejadas são os dados de apoio (mapa, posição e custos). A única coisa que sai do Nav2 para o mundo é a velocidade em /cmd_vel.

Objetivo: ver a pilha inteira funcionando no TurtleBot simulado antes de usar o seu robô. Se você já instalou os pacotes no módulo 4.4, pule o 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"

Como verificar: o RViz2 abre com o mapa, o robô e os botões "2D Pose Estimate" e "Nav2 Goal" na barra; a lista de nós mostra planner_server, controller_server, bt_navigator, amcl e behavior_server.

Por que aprender

Quando o robô "não anda", saber qual servidor faz o quê transforma um mistério em uma pergunta objetiva: ele não sabe onde está (AMCL), não acha rota (planner) ou acha rota mas não consegue seguir (controller)?

Conceitos-chave

Com o mapa pronto, o robô não precisa mais de SLAM: ele só precisa descobrir onde está dentro de um mapa que já conhece. Quem faz isso no Nav2 é o AMCL. Ele espalha centenas de "palpites" de posição (partículas) pelo mapa, compara o que o LiDAR deveria ver em cada palpite com o que ele vê de verdade e mantém os palpites que combinam. Conforme o robô anda, a nuvem encolhe em volta da posição real.

Como ler: da esquerda para a direita, o robô anda e o AMCL descarta os palpites que não batem com o LiDAR. O quadro do meio mostra o caso difícil: dois cantos iguais. Andar mais (ou dar a posição inicial) resolve a dúvida.

Objetivo: subir o Nav2 com o seu mapa salvo e informar onde o robô está. No simulado, acrescente use_sim_time:=True.

# terminal 1: Nav2 com o mapa do módulo 4.4 (o robô já publica /scan, /odom e TF)
source /opt/ros/<distro>/setup.bash
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml

# terminal 2: RViz2 com a configuração do Nav2
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}}}}"

Por que aprender

Toda navegação depende de uma localização boa. Se o robô acha que está meio metro à esquerda, ele vai planejar uma rota perfeita para um lugar onde não está, e bater no batente da porta.

Conceitos-chave

O mapa salvo diz só "livre" ou "parede". Para planejar, o Nav2 precisa de mais: quanto custa passar por cada célula. Esse é o mapa de custo (costmap). Ele é feito em camadas somadas: a camada estática vem do mapa salvo, a de obstáculos vem do LiDAR ao vivo (a cadeira que mudou de lugar) e a de inflação cria uma "margem" em volta de tudo, cara perto da parede e barata no meio do corredor.

Como ler: pense em três folhas transparentes empilhadas. A de cima nunca muda; a do meio muda a cada volta do LiDAR; a de baixo "espalha" custo em volta do que as outras marcam. O raio dessa faixa âmbar é o parâmetro que você mais vai ajustar.

Existem dois mapas de custo. O global cobre o mapa inteiro e serve ao planner, que traça a rota do ponto A ao B (algoritmos como NavFn ou Smac). O local é uma janela pequena que anda com o robô e serve ao controller, que escolhe a velocidade a cada instante para seguir a rota sem bater (algoritmos como DWB, MPPI ou Regulated Pure Pursuit).

Como ler: a linha âmbar tracejada é o plano no mapa inteiro, passando pela porta. Dentro do retângulo ciano (o mapa de custo local), apareceu uma caixa nova: o controlador faz a curva ciano em volta dela e volta para a rota, sem precisar replanejar tudo.

Objetivo: criar o seu arquivo de parâmetros a partir do padrão do Nav2 e ajustar o tamanho do robô, a margem e a velocidade máxima. Os nomes abaixo existem no arquivo padrão; a posição exata muda entre versões, então use a busca do editor.

# 1) copie o arquivo padrão para editar
cp $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/params/nav2_params.yaml ~/meu_nav2.yaml

# 2) no ~/meu_nav2.yaml, procure e ajuste (nos DOIS costmaps, global e local):
#    robot_radius: 0.15          raio do robô em metros (meça o seu)
#    inflation_radius: 0.35      margem em volta de paredes e objetos
#    cost_scaling_factor: 3.0    maior = custo cai mais rápido longe da parede
# 3) no velocity_smoother, limite a velocidade [x, y, giro]:
#    max_velocity: [0.2, 0.0, 1.0]

# 4) suba o Nav2 com o seu arquivo
ros2 launch nav2_bringup bringup_launch.py map:=$HOME/mapas/mapa.yaml params_file:=$HOME/meu_nav2.yaml

Como verificar: no RViz2, ligue as camadas "Global Costmap" e "Local Costmap": a faixa colorida em volta das paredes deve engordar ou afinar conforme o inflation_radius. Com ros2 topic echo /cmd_vel durante uma navegação, o linear.x não passa de 0.2.

Por que aprender

Quase todo comportamento estranho do Nav2 (passar raspando, recusar uma porta, ziguezaguear no corredor) se explica olhando os dois mapas de custo no RViz2. É a "radiografia" da navegação.

Conceitos-chave

O jeito mais rápido de testar é o botão "Nav2 Goal" do RViz2: clique no destino e arraste para indicar para onde o robô deve ficar virado. Para um robô que faz tarefas sozinho, você vai querer o mesmo pedido em código. O pacote nav2_simple_commander traz a classe BasicNavigator, que esconde a ação do ROS 2 atrás de funções simples como goToPose.

Objetivo: um script Python que informa a posição inicial, espera o Nav2 ficar pronto, manda o robô para (1,5; 0,5) virado a 90° e mostra quanto falta. Troque as coordenadas por um ponto livre do seu mapa (passe o mouse no RViz2 para ler x e y).

#!/usr/bin/env python3
# Manda o robô até um ponto do mapa com o 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))   # onde o robô está na largada
    nav.waitUntilNav2Active()                     # espera AMCL e 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'Faltam {fb.distance_remaining:.2f} m')

    resultado = nav.getResult()
    if resultado == TaskResult.SUCCEEDED:
        print('Cheguei.')
    elif resultado == TaskResult.CANCELED:
        print('Cancelado.')
    else:
        print('Falhou: o Nav2 não conseguiu chegar.')
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Para rodar: com o Nav2 já no ar, num terminal com source /opt/ros/<distro>/setup.bash, rode python3 ir_ate_ponto.py. Se faltar o módulo, instale ros-<distro>-nav2-simple-commander com apt.

Por que aprender

Com o objetivo em código, outro programa pode decidir para onde o robô vai: um botão no celular (Trilha 2), um objeto visto pela câmera (Trilha 3) ou um comando de voz interpretado por um modelo de linguagem (Trilha 5).

Conceitos-chave

No mundo real o robô vai travar: uma cadeira fecha o caminho, uma pessoa para na frente, a roda prende no tapete. O Nav2 não desiste na primeira falha. A árvore de comportamento do BT Navigator diz o que tentar: primeiro limpar o mapa de custo (o obstáculo pode ter ido embora), depois girar no lugar para o LiDAR ver melhor, esperar, dar ré um pouco e replanejar. Só depois de algumas tentativas o objetivo é dado como falho.

Como ler: lê-se de cima para baixo e da esquerda para a direita. O ramo âmbar é o caminho normal (planejar e seguir). Se ele falhar, o ramo ciano roda as ações de recuperação em ordem e o topo manda tentar navegar de novo, até esgotar as tentativas.

Objetivo: não deixar o seu script esperar para sempre. Esta versão cancela o objetivo se passar de 60 segundos e você pode testar a recuperação pondo uma caixa (no Gazebo, um bloco) no caminho.

#!/usr/bin/env python3
# Vai até um ponto, mas cancela se demorar demais (rede de segurança do seu 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()            # aqui a posição inicial já foi dada pelo 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('Demorou demais: cancelando.')
        nav.cancelTask()
        cancelado = True

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

Por que aprender

Um robô que só funciona com o caminho perfeito não sai da bancada. Entender a recuperação, e pôr um limite no seu próprio programa, é o que separa uma demonstração de um robô que faz a ronda todos os dias.

Conceitos-chave

Uma ronda é uma lista de waypoints (pontos de passagem): sala, corredor, cozinha, volta à base. O BasicNavigator tem followWaypoints, que visita os pontos em ordem e informa em qual está. Se um ponto falhar (porta fechada), por padrão ele segue para o próximo e, no fim, conta quais falharam.

Como ler: o robô sai da base (B, ciano), passa pelos pontos 1, 2 e 3 na ordem e volta. A área hachurada em vermelho é a escada: nenhum waypoint fica perto dela. No módulo 4.6 você vai bloqueá-la também no mapa.

Objetivo: uma ronda de três pontos com volta à base, repetida duas vezes. Troque as coordenadas pelos pontos do seu mapa.

#!/usr/bin/env python3
# Ronda por waypoints com o Nav2: visita os pontos em ordem e volta à base
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

# (nome, x, y, ângulo em graus) no frame do 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'Volta {volta}: indo para {PONTOS[atual][0]}')
    print(f'Volta {volta} terminou:', nav.getResult())
    if nav.getResult() != TaskResult.SUCCEEDED:
        break
rclpy.shutdown()

Por que aprender

Ronda é a base de robô de vigilância, de entrega entre salas e de inventário. Com uma lista de pontos você já tem um "trabalho" para o robô, que vai virar o projeto do próximo módulo.

Conceitos-chave