Vehicle Tracking

ROS 2 workspace for testing a PX4 fixed-wing external mode that tracks a moving ground vehicle (GV). The local vehicle_tracking package generates a GPS lookahead point on a GV-relative sine sweep path, then a PX4 ROS 2 mode converts that point into fixed-wing course and altitude setpoints.

The repo is set up around PX4 v1.17 beta style messages and the PX4 ROS 2 Interface Library.

Repository Layout

• src/vehicle_tracking/ - local ROS 2 package with the tracking mode, path generator, simulator, and shared math.
• src/px4_msgs/ - ROS 2 message/service definitions that must match the PX4-Autopilot checkout used for SITL.
• src/px4-ros2-interface-lib/ - PX4 ROS 2 interface library used to register the external mode and publish fixed-wing setpoints.
• processing/ - bag-analysis scripts for straight/turning and constant/varying GV trials.
• notes.txt - short run notes used as the current operational checklist.
• manual_px4_msgs_sync.txt - manual message sync commands for this workspace.
• px4_sitl_setup.bash - PX4 home-location environment setup for SITL.

Core Runtime Flow

1. simulated_vehicle_node publishes a synthetic GV on:
   • /gv/odom (nav_msgs/msg/Odometry)
   • /gv/navsat (sensor_msgs/msg/NavSatFix)
2. path_generation_node subscribes to GV state plus PX4 aircraft position:
   • /gv/odom
   • /gv/navsat
   • /fmu/out/vehicle_global_position
   • /fmu/out/vehicle_local_position_v1
3. path_generation_node builds a GV-relative local frame, computes a sine-wave lookahead target, and publishes:
   • /path/lookahead_navsat
   • debug topics under /path/debug/*
   • optional Foxglove GeoJSON on /path/debug/sine_wave_geojson
4. vehicle_tracking_node registers the Vehicle Tracking PX4 external mode and sends FwLateralLongitudinalSetpointType course/altitude commands toward /path/lookahead_navsat.

Code Explanation

include/vehicle_tracking/path_math.hpp

Defines small 2-D vector/matrix helpers and the GvFrame transform. The GV frame has local x along the GV heading and local y to the left of the GV path. buildGvFrame(), globalToGvLocal(), and gvLocalToGlobal() keep the path-generation math independent from WGS84 coordinates.

src/path_math.cpp

Contains:

• quatToYaw() for extracting planar yaw from GV odometry quaternions.
• radToDeg() for debug output.
• solveAmplitude(speed_ratio) for finding the dimensionless sine-wave amplitude that makes the path-length ratio match the aircraft/GV speed ratio. It uses Boost elliptic integrals and a bracketed TOMS748 root solve.

src/path_generation_node.cpp

This is the main path-planning node. It:

• waits until GV GPS, GV odometry, PX4 global position, and PX4 local position are available;
• uses GeographicLib::LocalCartesian to convert WGS84 lat/lon into a local ENU plane;
• expresses the aircraft position in the GV-relative frame;
• computes sine wavelength, wavenumber, speed ratio, and amplitude;
• integrates a moving target point along the sine curve;
• resets the target if stale state gets too far from the aircraft;
• converts the target back to WGS84 and publishes /path/lookahead_navsat;
• publishes debug scalar topics and Foxglove GeoJSON for plots and live inspection.

Important parameters:

• rate_hz - path update frequency, default 20.0.
• spatial_wavelength_m - sine wavelength, default 180.0.
• cruise_speed_mps - assumed aircraft path speed, default 15.0.
• sine_geojson_half_span_m - visible Foxglove line half-span, default 200.0.
• sine_geojson_step_m - Foxglove line sample spacing, default 5.0.
• target_state_reset_dist_m - safety reset distance, default 1000.0.

include/vehicle_tracking/mode.hpp

Defines the VehicleTracking PX4 external mode. It subscribes to /path/lookahead_navsat and /fmu/out/vehicle_global_position, computes the initial geodetic bearing from the aircraft to the lookahead point, wraps it to [-pi, pi], and sends it to PX4 with updateWithAltitude(desired_alt_m, desired_course_rad).

Important parameter:

• desired_alt_m - commanded fixed-wing altitude, default 100.0.

src/main.cpp

Wraps VehicleTracking in px4_ros2::NodeWithMode, which handles registration with PX4 through the PX4 ROS 2 Interface Library.

src/simulated_vehicle_node.cpp

Simulates the GV for repeatable tests. It publishes odometry in ENU meters and NavSatFix coordinates through GeographicLib.

path_mode options:

• 1 - straight, constant 3 m/s.
• 2 - straight for 60 s, then a 600 m radius turn.
• 3 - straight with 3 to 5 to 3 m/s speed profile.
• 4 - mode 3 speed profile plus mode 2 turning behavior.

processing/*.py

Offline ROS bag analysis scripts. They read /gv/navsat, /path/debug/plane_navsat, and /path/lookahead_navsat, convert geodetic tracks to local ENU, compute GV-target and GV-UAV crossover errors, estimate coverage from 180 m x 180 m UAV footprints, write CSV results, and save coverage plots.

Build

Prerequisites expected by this workspace:

• ROS 2 with colcon.
• PX4-Autopilot checked out at ~/PX4-Autopilot for SITL and message sync.
• Micro XRCE-DDS Agent available as MicroXRCEAgent.
• GeographicLib development files.
• foxglove_msgs and foxglove_bridge if using visualization.

Build from the workspace root:

cd ~/vehicle_tracking
git submodule update --init --recursive
source /opt/ros/$ROS_DISTRO/setup.bash
colcon build --symlink-install
source install/setup.bash

If $ROS_DISTRO is not set, replace it with your distro name, for example humble or jazzy.

Run With PX4 SITL

Use separate terminals so each process stays visible.

Terminal 1, PX4 SITL:

cd ~/PX4-Autopilot
source ~/vehicle_tracking/px4_sitl_setup.bash
HEADLESS=1 make px4_sitl gz_rc_cessna

Terminal 2, Micro XRCE-DDS Agent:

MicroXRCEAgent udp4 -p 8888

Terminal 3, ROS path generation:

cd ~/vehicle_tracking
source install/setup.bash
ros2 run vehicle_tracking path_generation_node

Terminal 4, PX4 external tracking mode:

cd ~/vehicle_tracking
source install/setup.bash
ros2 run vehicle_tracking vehicle_tracking_node

Terminal 5, simulated GV:

cd ~/vehicle_tracking
source install/setup.bash
ros2 run vehicle_tracking simulated_vehicle_node --ros-args -p path_mode:=3

Terminal 6, QGroundControl:

Open QGroundControl, connect to the SITL vehicle, and take off normally from QGroundControl first. After the aircraft is flying and the ROS nodes are publishing a valid lookahead point, use the QGroundControl flight-mode selector to switch into Vehicle Tracking.

Optional Foxglove bridge:

cd ~/vehicle_tracking
source install/setup.bash
ros2 launch foxglove_bridge foxglove_bridge_launch.xml

Useful checks:

ros2 topic echo /path/lookahead_navsat
ros2 topic echo /path/debug/base_amplitude_m
ros2 topic list | grep '^/fmu/out'

Manual PX4 Message Sync

PX4 ROS 2 requires the ROS-side px4_msgs definitions to match the PX4 firmware message definitions used by the uXRCE-DDS client. If PX4 messages change, run the commands in manual_px4_msgs_sync.txt from the appropriate directory:

rm -f ~/vehicle_tracking/src/px4_msgs/msg/*.msg
rm -f ~/vehicle_tracking/src/px4_msgs/srv/*.srv
cp ~/PX4-Autopilot/msg/*.msg ~/vehicle_tracking/src/px4_msgs/msg/
cp ~/PX4-Autopilot/msg/versioned/*.msg ~/vehicle_tracking/src/px4_msgs/msg/
cp ~/PX4-Autopilot/srv/*.srv ~/vehicle_tracking/src/px4_msgs/srv/

cd ~/vehicle_tracking/src/px4-ros2-interface-lib
./scripts/check-message-compatibility.py -v ~/vehicle_tracking/src/px4_msgs/ ~/PX4-Autopilot/

Then rebuild the workspace:

cd ~/vehicle_tracking
colcon build --symlink-install
source install/setup.bash

Analyze Bags

Record the key topics:

ros2 bag record -s mcap \
  /gv/navsat \
  /path/debug/plane_navsat \
  /path/lookahead_navsat

Run an analysis script against a bag directory:

python3 processing/straight_constant_analysis.py --bag bags/straight_constant
python3 processing/straight_varying_analysis.py --bag bags/straight_varying
python3 processing/turning_constant_analysis.py --bag bags/turning_constant
python3 processing/turning_varying_analysis.py --bag bags/turning_varying

Outputs are written under processing/output/....

Reference Links

• PX4 ROS 2 User Guide: https://docs.px4.io/main/en/ros2/user_guide
• PX4 uXRCE-DDS bridge: https://docs.px4.io/main/en/middleware/uxrce_dds
• PX4 DDS topics exposed to ROS 2: https://docs.px4.io/main/en/middleware/dds_topics
• PX4 ROS 2 Interface Library overview: https://docs.px4.io/main/en/ros2/px4_ros2_interface_lib
• PX4 ROS 2 Control Interface and FwLateralLongitudinalSetpointType: https://docs.px4.io/main/en/ros2/px4_ros2_control_interface
• PX4 Gazebo SITL: https://docs.px4.io/main/en/sim_gazebo_gz/
• px4_msgs upstream: https://github.com/PX4/px4_msgs
• PX4 ROS 2 Interface Library API docs: https://auterion.github.io/px4-ros2-interface-lib/topics.html
• PX4 ROS 2 Interface Library Python docs: https://auterion.github.io/px4-ros2-interface-lib/python/px4_ros2_py.html