MoCap4ROS2

ROS 2
C++
Robotics
PX4
Motion Capture
Production-ready ROS 2 C++ packages for streaming Vicon and OptiTrack motion-capture data into the PX4 EKF for indoor quadrotor experiments.

Vicon4PX4 on GitHub OptiTrack4PX4 on GitHub GaTech IFL Codebase

Running indoor flight experiments means solving the same infrastructure problem every time: getting motion-capture position data into the drone’s state estimator reliably.

Vicon4PX4 and OptiTrack4PX4 are my ROS 2 C++ packages that handle the entire pipeline — coordinate frame conversion, EKF fusion, and full-state relay for both of the major motion-capture systems in most research labs.

Vicon4PX4 has been adopted and showcased by Georgia Tech’s Indoor Flight Lab (IFL) as part of its official codebase.

The Problem

PX4’s EKF expects external-vision data in a very specific format: NED coordinates, a particular quaternion component ordering, microsecond timestamps, and a VehicleOdometry message on the right topic. Every lab doing indoor flight ends up writing bespoke glue code to get there — and it breaks whenever someone upgrades PX4, switches motion-capture vendors, or moves to a new ROS 2 distro. These packages solve that once and for all.

The Solution

My three-stage pipeline:

MoCap System (ENU, mm)
  → Client Node (ENU→NED, mm→m, publishes PoseStamped)
    → Visual Odometry Relay (quat reorder, µs timestamps, 35 Hz)
      → /fmu/in/vehicle_visual_odometry → PX4 EKF2
        → Full-State Relay (merges EKF outputs, 40 Hz)
          → /merge_odom_localpos/full_state_relay

The client node connects to the motion-capture system, converts ENU measurements to NED, and publishes pose data as both quaternions and Euler angles under the rigid body name defined in the MoCap software.

The visual odometry relay takes the processed poses, reorders quaternion components to match PX4’s convention, applies microsecond timestamps, and publishes VehicleOdometry messages at 35 Hz on /fmu/in/vehicle_visual_odometry for direct EKF external-vision fusion.

The optional full-state relay merges the fused EKF output from /fmu/out/vehicle_odometry and /fmu/out/vehicle_local_position into a single topic at 40 Hz, giving controllers and loggers convenient access to all pose data including higher-order derivatives from the EKF.

Key Features

  • ENU → NED coordinate conversion with millimeters-to-meters scaling
  • Pose published as both quaternions and Euler angles under the rigid body name
  • PX4-compatible VehicleOdometry messages with correct quaternion ordering and timestamps
  • Full-state relay merging fused EKF outputs into a single topic for control and logging
  • Multiple launch modes: client only, client + relay, or all nodes including full-state merger
  • Configurable networking: set the MoCap system IP and the bridge handles the rest
  • Vendored SDKs: Vicon DataStream SDK and NatNet SDK are bundled per-architecture — no system installs

Compatibility

Vicon4PX4 OptiTrack4PX4
ROS 2 Jazzy / Humble Jazzy / Humble
Ubuntu 22.04 / 24.04 22.04 / 24.04
Architecture x86_64, ARM64 x86_64
SDK Vicon DataStream 1.12 (vendored) NatNet 1.12 (vendored)
Build C++17, ament_cmake C++17, ament_cmake
License GPL-3.0 GPL-3.0

Built with: ROS 2 (Jazzy / Humble) · C++17 · PX4 · Ubuntu 22.04 / 24.04 · x86_64 & ARM64