Localização (control/localization)¶

Índice da documentação¶

Papel¶

Integração de navegação externa (external-nav) para voo sem GPS (indoor). Alimenta o FCU com uma pose de Visual SLAM para que seu EKF (ArduPilot EKF3 / PX4 EKF2) possa estimar a posição sem GPS.

Vive sob control porque faz a ponte entre o producer de VSLAM e os transportes MAVROS e MAVLink. O pipeline é dividido em um producer (RealSense + Isaac ROS Visual SLAM, executado no container Isaac) e um consumer (uma bridge de vision-pose que encaminha a pose ao FCU, executada do lado do SDK). Eles se comunicam por host networking com um ROS_DOMAIN_ID compartilhado.

Topologia¶

flowchart LR
  subgraph isaac [Isaac container]
    rs[realsense2_camera infra1/2 + IMU]
    vslam[isaac_ros_visual_slam]
    rs --> vslam
  end
  subgraph sdk [SDK container or host]
    bridge[vision_pose_node backend mavros/mavlink/dds]
    mavros[MAVROS indoor]
  end
  vslam -->|"/visual_slam/tracking/vo_pose_covariance"| bridge
  bridge -->|"mavros: /mavros/vision_pose/pose_cov"| mavros
  bridge -->|"mavlink: VISION_POSITION_ESTIMATE"| fcu[FCU]
  bridge -->|"dds: VehicleOdometry"| fcu
  mavros --> fcu

Componentes¶

Executar¶

Pré-requisito: producer e consumer compartilham o ROS_DOMAIN_ID (padrão 14, veja scripts/lib/config.sh). Jetson, computador de bordo/host e o laptop opcional (RViz) devem usar o mesmo domínio.

1. Produtor (container Isaac)¶

make isaac-run          # or: ./docker/isaac_vslam/run_docker.sh

# inside:

nectar-vslam            # = ros2 launch nectar/launch/isaac_vslam_realsense.launch.py

Confirme que a RealSense é enumerada e que os tópicos do VSLAM publicam. Notas do Docker: Isaac ROS Visual SLAM (Jetson).

2. Consumidor — feeder de vision (manter em execução)¶

Ou: make driver DRONE=mavlink ENV=indoor. Verifique a pose fundida (Procedimento indoor) e só então inicie a missão em um endpoint MAVLink diferente do feeder.

mavlink_url / fcu_url são de formato livre — ajuste à sua linha serial ou ao fan-out do roteador. Layout MAVProxy de exemplo no Jetson da Black Bee (GCS / missão / feeder): 14550 / 14551 / 14552.

Respeite a regra do feeder único: não inicie também um segundo feeder de vision no mesmo link do FCU a partir de uma missão.

3. Missão¶

PoseSource.VISION com auto_vision_feed=False (padrão): a missão assina o tópico do VSLAM para navegação do companion e não envia VISION_*. Ative auto_vision_feed=True para um feed em processo único (nesse caso, não execute também o feeder standalone de mavlink no mesmo endpoint). Velocidade opcional nesse caminho: vision_send_speed=True.

Procedimento indoor¶

Checklist do dia de voo para o stack atual (D435i + cuVSLAM). Os comandos e as tabelas do FCU estão acima e em Configuração do FCU; esta seção trata apenas de ordem e prática.

Montagem (prática Black Bee)¶

VIO / V-SLAM assume que a câmera e a IMU se movem junto com a aeronave. Vibração ou uma fixação solta parece movimento para o estimador.

• Alinhe o eixo óptico da D435i com as extrinsics configuradas (VISO_POS_* / frames do launch do Isaac).
• Fixação soft-mount: elásticos e espuma entre a câmera e o frame; espuma no case do sensor onde ajudar sem bloquear as lentes ou o USB.
• Depois de transportar ou remontar, reinicie o producer + a bridge e refaça as verificações abaixo — não reaproveite a sessão de mapa do dia anterior.

Bring-up¶

1. Produtor — Executar §1.
2. Consumidor — Executar §2.
3. Verificação do pipeline — mova a aeronave manualmente; a pose local fundida do FCU precisa mudar (vision_fcu_check.py e/ou o MAVLink Inspector da GCS). RViz opcional: Visualização.
4. Origem do EKF — apenas quando necessário; veja Origem do EKF. Flag opcional do feeder: set_ekf_origin:=true.
5. Aquecimento (warm-up) — caminhe com o veículo lentamente pelo volume onde ele vai voar (quadrado / círculo / volta pela arena). Movimento suave; evite yaw abrupto e pessoas cruzando o FOV (modos de falha). Isso constrói a cobertura do mapa e permite abortar no chão se o tracking parecer ruim.
6. Sanidade de escala / vertical — levante e translade manualmente; X/Y/Z fundidos devem se mover aproximadamente a distância certa. (O bring-up do T265 historicamente pedia um levantamento de ~1 m para a escala vertical — Legado; com D435i + cuVSLAM trate isso como uma verificação, não uma calibração oculta.)
7. Primeiro voo — Stabilize ou AltHold → movimento suave observando a pose fundida → Loiter somente quando o tracking parecer estável (pronto para reverter imediatamente). Depois, missão / perfis mais exigentes. Padrão do LuckyBird Discourse parte 2.

Após muito tempo ligado, mudanças de fixação/USB/parâmetros, ou avisos estranhos na GCS: reinicie o producer + a bridge (e reinicie o FCU depois de mudanças de parâmetro).

Triagem¶

Velocidade (opcional)¶

Por padrão, os feeders enviam apenas posição. send_speed:=true em vision_pose.launch.py (ou vision_send_speed=True com auto_vision_feed) também envia a velocidade do VSLAM; a pose continua sendo enviada normalmente. O EKF3 não inicializa apenas com velocidade (ardupilot#23485).

ros2 launch nectar vision_pose.launch.py backend:=mavros send_speed:=true
ros2 launch nectar vision_pose.launch.py backend:=mavlink send_speed:=true mavlink_url:=…

Argumentos: speed_topic (padrão /visual_slam/tracking/odometry); o node também tem speed_output_topic, speed_timeout_s.

O que o cuVSLAM realmente fornece¶

O cuVSLAM estima pose e covariância da pose; ele não tem estado de velocidade. O twist em /visual_slam/tracking/odometry é produzido pelo wrapper ROS como uma diferença finita sobre as últimas 10 poses, dp = pose(t0)⁻¹ · pose(t1) (PoseCache::GetVelocity). Portanto, ele é derivado da mesma pose que já enviamos, não é uma medição independente. A aceleração é uma entrada (IMU da RealSense, tracking_mode:=1), não uma saída.

Essa forma de dp torna o twist no frame do corpo (child_frame_id, FLU), como o nav_msgs/Odometry exige. O VISION_SPEED_ESTIMATE não carrega atitude, então o FCU não pode rotacioná-lo: as bridges rotacionam de corpo para mundo usando a atitude da mesma amostra de odometria antes da troca ENU -> NED (frames.py). O ODOMETRY é o contraexemplo — ele carrega o quaternion e o próprio ArduPilot rotaciona a velocidade body-FRD.

Configuração do FCU para velocidade¶

O ArduPilot lista a velocidade como opcional, junto com a posição (Non-GPS Position Estimation). Ela só é fundida depois que a fonte é selecionada:

• EK3_SRC1_VELXY = 6, EK3_SRC1_VELZ = 6 (ExternalNav). Com 0 as mensagens são recebidas e logadas, mas não fundidas.
• VISO_VEL_M_NSE define o ruído de medição da velocidade. O ArduPilot ignora o campo de covariância do VISION_SPEED_ESTIMATE e sempre usa este parâmetro (AP_VisualOdom_MAV.cpp), então é o único ajuste para o quanto o EKF confia nessa entrada. Como o twist é derivado da posição que já enviamos, começar de forma conservadora (o padrão de 0.1 m/s) evita contar em dobro uma única medição como se fossem duas independentes.
• PX4: o bit 2 de EKF2_EV_CTRL habilita a fusão de velocidade 3D.

Verifique as inovações de XKFS/XKF3 nos logs antes e depois de habilitar.

Configuração do FCU¶

A bridge apenas entrega a pose — o estimador do FCU ainda precisa ser configurado para fundi-la. ArduPilot (EKF3) e PX4 (EKF2) usam parâmetros diferentes e uma taxa mínima diferente. Os dois backends MAVLink enviam o VISION_POSITION_ESTIMATE (#102); o EKF o funde uma vez configurado. As regras de origem variam por firmware — veja Origem do EKF.

Nossos voos indoor em hardware até hoje são em ArduPilot 4.6.x / 4.8-dev. O conjunto de PX4 abaixo é baseado na documentação do PX4 e corresponde ao pipeline de referência (o mesmo relay MAVROS) usado pelo tutorial VSLAM-UAV no PX4 v1.15.4. O SITL já exercita esse caminho (ENV=indoor → indoor_room_px4 + gz_vision_source); valide no seu Pixhawk antes de usar em competição.

ArduPilot (EKF3)¶

Taxa ≥ 4 Hz. Ajuste (tuning): VISO_POS_M_NSE, VISO_YAW_M_NSE, VISO_DELAY_MS, VISO_QUAL_MIN. Referências: EKF source selection, Non-GPS position estimation, VISO_TYPE, EK3_SRC*.

PX4 (EKF2)¶

Taxa de 30–50 Hz — o PX4 rejeita external vision quando a taxa é baixa demais (muito mais estrito que o ArduPilot); o cuVSLAM a ~90 Hz atende esse requisito. Ajuste: EKF2_EV_NOISE_MD, EKF2_EVP_NOISE, EKF2_EVA_NOISE, EKF2_EV_QMIN. Referências: External position estimation, VIO, EKF2 tuning, params_external_vision.yaml, padrão de EKF2_EV_CTRL (#24298).

Os backends mavros/mavlink entregam essa estimativa como VISION_POSITION_ESTIMATE; o backend nativo dds publica px4_msgs/VehicleOdometry em /fmu/in/vehicle_visual_odometry no lugar (mesmos parâmetros do EKF2, sem MAVROS/MAVLink). Veja Backends.

Origem do EKF¶

A origem do EKF é o lat/lon/alt global que define o NED local (0,0,0). Ela não é o Home (ponto de retorno do RTL) e não é a origem do mapa do VSLAM. O Home permanece com o FCU: o ArduPilot o inicializa depois da origem (e novamente ao armar, para o RTL do Copter). A bridge nunca envia SET_HOME_POSITION.

Opt-in a partir do vision feeder (desligado por padrão):

ros2 launch nectar vision_pose.launch.py backend:=mavlink set_ekf_origin:=true

# optional overrides:

#   origin_lat:=… origin_lon:=… origin_alt_m:=… origin_timeout_s:=2.0

Os padrões são o lat/lon do laboratório da Black Bee (-22.41434308754571, -45.44843145453864) com origin_alt_m:=0.0 (defina a AMSL do local se a altura no mapa da GCS precisar ficar correta). Se GPS_GLOBAL_ORIGIN já estiver presente, o envio é ignorado. Suportado em mavros, mavlink e dds.

Alternativas manuais: Set EKF Origin Here do Mission Planner, SET_GPS_GLOBAL_ORIGIN, ou ahrs-set-origin.lua.

ArduPilot (Non-GPS Position Estimation, Home / origin):

• Se nenhum GPS estiver fornecendo fix, defina a origem antes que o EKF possa estimar a posição (necessário para Loiter / Guided / Auto e para publicar a pose local).
• Os valores de lat/lon só precisam ser um WGS84 válido; são uma referência, não um levantamento topográfico.
• Uma vez definida, a origem não pode ser movida até o próximo reboot.
• Se um GPS estiver presente e obtiver fix, o ArduPilot normalmente define a origem por conta própria (Non-GPS Navigation).
• A partir da 4.7+, AHRS_OPTIONS bit 3 (RecordOrigin) + bit 4 (UseRecordedOriginForNonGPS) podem salvar/restaurar a origem entre ciclos de energia, então não é preciso defini-la a cada boot.
• O ícone do veículo no mapa do Mission Planner aparece quando existe uma origem (wiki do T265, LuckyBird); isso é visualização da GCS, não uma etapa de calibração separada.

Stabilize / AltHold não precisam de uma estimativa de posição. Se você "simplesmente rodar" sem definir a origem e o Loiter ainda funcionar, a origem já estava definida (fix de GPS, origem gravada na 4.7+, ou uma definição anterior via GCS/script/set_ekf_origin).

PX4 (External position estimation):

• A fusão EV local (Position / OFFBOARD local) usa o frame local vindo da visão; o guia de setup do VSLAM-UAV não exige definir uma origem para esse caminho.
• SET_GPS_GLOBAL_ORIGIN serve para construir uma estimativa global a partir da pose local, para que modos automáticos que precisam de posição global (Mission, Return, …) possam rodar indoor (set_ekf_origin:=true nos backends dds / mavros cobre esse caso).

Nota histórica: o caminho ROS do LuckyBird e o set_origin.py / clique no MP eram explícitos. O t265_to_mavlink.py upstream pode opcionalmente enviar SET_GPS_GLOBAL_ORIGIN. O vision_to_mavros (ROS 2) da Black Bee apenas republica a pose. Veja Legado.

Fonte de altura¶

Não usamos o barômetro indoor — ele deriva perto do solo e no fluxo de ar das hélices. Escolhemos o POSZ entre duas fontes e mantemos o resto do conjunto (POSXY / VELXY / YAW) na visão:

• Vision (EK3_SRC1_POSZ=6 / EKF2_HGT_REF=Vision): a altitude é relativa à origem do VSLAM, sem terrain following — o drone mantém uma altura constante ao cruzar plataformas ou obstáculos na arena. Este é o nosso padrão e tem sido útil em missões com plataformas elevadas.
• Rangefinder voltado para baixo (EK3_SRC1_POSZ=2 / EKF2_HGT_REF=Range + EKF2_RNG_CTRL): a altitude acompanha o terreno. O ArduPilot avisa que isso é "só apropriado ... onde o piso é plano, sem obstruções no solo" (EKF sources); o PX4 observa que "a origem NED local vai subir e descer com o nível do solo" (EKF2 tuning). Sobre obstáculos fixos o veículo então sobe para compensar, então mascaramos as quedas abruptas upstream com o ObstacleMaskFilter antes que o FCU chegue a vê-las.

Backends¶

• mavros: o MavrosVisionRelay republica o PoseWithCovarianceStamped do VSLAM (ENU) em /mavros/vision_pose/pose_cov; o MAVROS converte para NED para o FCU.
• mavlink: nectar.control.mavlink.VisionPoseBridge converte ENU->NED e envia VISION_POSITION_ESTIMATE por um link pymavlink dedicado.
• dds: nectar.control.px4.Px4VisionOdometryBridge converte ENU->NED e publica px4_msgs/VehicleOdometry em /fmu/in/vehicle_visual_odometry (uXRCE-DDS nativo do PX4). Precisa de um MicroXRCEAgent em execução e de px4_msgs; defina px4_namespace para corresponder a um cliente com namespace.

Cada backend ganha um caminho de velocidade com send_speed:=true (Velocidade); o caminho de pose acima não é afetado.

Regra do feeder único¶

Exatamente um processo pode enviar vision externa para o FCU:

Opt-in auto_vision_feed=True: a missão envia VISION_* (opcional vision_send_speed). Nunca execute também o feeder standalone de mavlink no mesmo endpoint.

SITL¶

Gazebo indoor: ground-truth → tópicos canônicos do VSLAM → mesmos backends → EKF3 / EKF2.

Arena compartilhada: o ArduPilot compõe nectar_indoor; o PX4 usa indoor_room_px4 + x500_nectar (PosePublisher ~50 Hz). Parâmetros: ArduPilot indoor.parm; PX4 px4_indoor.env. O x500_vision padrão do PX4 só via override explícito. Matriz completa: README de simulação.

Para exercitar a origem opcional do EKF no ArduPilot indoor:

make sim-start  FIRMWARE=ardupilot ENV=indoor
make sim-bridge FIRMWARE=ardupilot ENV=indoor ARGS='set_ekf_origin:=true'

# vision_pose_node log: "EKF origin: sent …" or "already set, skip send"

Observação: o SITL padrão do ArduPilot ainda fixa uma origem SIM home (Camberra) mesmo com GPS1_TYPE=0, então uma sobrescrita tardia geralmente não se mantém — use a linha de log para confirmar o caminho do feeder; valide lat/lon em hardware sem GPS (ou depois de um FCU limpo, sem origem).

ros2 topic hz /visual_slam/tracking/vo_pose_covariance   # ~50 Hz
ros2 topic echo /mavros/vision_pose/pose_cov --once        # PROTOCOL=mavros

Visualização¶

Os overlays de trajetória mostram a estimativa de SLAM / odometria para que você possa avaliar a qualidade do tracking e o loop closure — eles não são uma etapa de calibração. Use-os no bring-up para verificar: a trajetória acompanha o movimento manual, o ruído é aceitável para o Loiter, e a trajetória verde do SLAM se ajusta ao revisitar um local (loop closure).

A NVIDIA recomenda rodar o RViz em um PC remoto, não no Jetson (tutorial RealSense).

Dois perfis (rviz/vslam_light.rviz, rviz/vslam_full.rviz):

O perfil light desenha duas trajetórias sempre publicadas: a trajetória verde do SLAM (/visual_slam/tracking/slam_path, corrigida por loop closure) e a trajetória roxa de VO (/visual_slam/tracking/vo_path, odometria bruta). Quando um loop se fecha, a trajetória verde se ajusta em relação à roxa — esse é o loop closure, visível sem custo no producer. A nuvem de pontos roxa literal do loop closure vive no full (é um tópico /visual_slam/vis/*, publicado apenas com enable_visualization:=true).

As duas trajetórias light são mostradas como um buffer deslizante (últimos 15 s por padrão) para que a janela não se preencha com a trajetória inteira. O vslam_rviz.launch.py executa um relay path_window_node junto com o RViz (sem custo no Jetson) que republica /visual_slam/tracking/{slam,vo}_path recortado para os últimos window_seconds em tópicos *_windowed, que o perfil light assina. Defina window_seconds como 0 para o histórico completo.

Laptop (nectar buildado):

ros2 launch nectar vslam_rviz.launch.py profile:=light            # or profile:=full
ros2 launch nectar vslam_rviz.launch.py window_seconds:=30        # longer buffer

Laptop (apenas o repo, sem build) — execute o relay também, senão as trajetórias light ficam vazias:

ros2 run nectar path_window_node.py    # if built; otherwise: python3 .../nodes/path_window_node.py
rviz2 -d src/nectar-sdk/nectar/nectar/control/localization/rviz/vslam_light.rviz

O perfil full exige que o producer publique os tópicos /visual_slam/vis/*, que ficam desligados por padrão para manter o Jetson leve:

nectar-vslam enable_visualization:=true

Notas de hardware¶

Atual: Intel RealSense D435i + Isaac ROS Visual SLAM (cuVSLAM) em um Jetson Orin Nano; pose a ~90 Hz. Producer / consumer: Executar. Dia do voo: Procedimento indoor.

Legado / fallback: RealSense T265 + vision_to_mavros (VISO_TYPE=2). Histórico completo, versões e bring-up: Legado T265. Comparação conceitual: Conceitos.

Referências¶

Teoria do módulo e uma bibliografia mais completa: Conceitos → Referências.

• Isaac ROS Visual SLAM (isaac_ros_visual_slam)
• cuVSLAM
• Isaac ROS Development Environment
• ArduPilot: Non-GPS Position Estimation
• ArduPilot: Setting Home and/or EKF origin
• ArduPilot: Non-GPS Navigation
• PX4: External Position Estimation