examples: add vision odometry example

Add a practical example showing how to send visual odometry data
to PX4 via the Mocap plugin's Odometry message. Demonstrates
connecting to PX4 SITL, configuring EKF2 for vision fusion
(EKF2_EV_CTRL, EKF2_HGT_REF), sending ODOMETRY at 30 Hz, and
monitoring health to verify fusion is active.

Author: alireza787b

examples/vision_odometry.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+
+"""
+Send visual odometry data to PX4 via MAVSDK's Mocap plugin.
+
+This example demonstrates how to send ODOMETRY messages to PX4 for
+vision-based navigation (VIO, visual odometry, motion capture, etc.).
+
+It connects to PX4 SITL, configures EKF2 for external vision fusion,
+sends simulated odometry at 30 Hz, and monitors health to verify fusion.
+
+Prerequisites:
+    - PX4 SITL running (e.g. make px4_sitl gz_x500)
+    - pip install mavsdk
+"""
+
+import asyncio
+import math
+import time
+
+from mavsdk import System
+from mavsdk.mocap import (
+    Odometry,
+    PositionBody,
+    Quaternion,
+    SpeedBody,
+    AngularVelocityBody,
+    Covariance,
+)
+
+
+async def run():
+    drone = System()
+    await drone.connect(system_address="udp://:14540")
+
+    print("Waiting for drone to connect...")
+    async for state in drone.core.connection_state():
+        if state.is_connected:
+            print("-- Connected to drone!")
+            break
+
+    # Configure EKF2 for vision fusion
+    print("Configuring EKF2 for vision fusion...")
+    await drone.param.set_param_int("EKF2_EV_CTRL", 15)
+    await drone.param.set_param_int("EKF2_HGT_REF", 3)
+    print("-- EKF2 configured (EV_CTRL=15, HGT_REF=3)")
+
+    # Start sending odometry and monitoring health in parallel
+    await asyncio.gather(
+        send_odometry(drone),
+        monitor_health(drone),
+    )
+
+
+async def monitor_health(drone):
+    """Print health status until vision fusion is active."""
+    async for health in drone.telemetry.health():
+        status = "OK" if health.is_local_position_ok else "WAITING"
+        print(f"  Local position: {status}")
+        if health.is_local_position_ok:
+            print("  --> EKF2 is fusing vision data!")
+            break
+
+
+async def send_odometry(drone):
+    """Send simulated vision odometry at 30 Hz for 20 seconds."""
+    nan_cov = Covariance([float("nan")])
+    start_time = time.monotonic()
+
+    print("Sending vision odometry at 30 Hz...")
+    for i in range(600):
+        elapsed = time.monotonic() - start_time
+
+        # Simulated position: gentle circle at 0.5 m radius
+        x = 0.5 * math.cos(elapsed * 0.5)
+        y = 0.5 * math.sin(elapsed * 0.5)
+        z = 0.0
+
+        # Simulated velocity
+        vx = -0.25 * math.sin(elapsed * 0.5)
+        vy = 0.25 * math.cos(elapsed * 0.5)
+
+        odom = Odometry(
+            time_usec=0,
+            frame_id=Odometry.MavFrame.MOCAP_NED,
+            position_body=PositionBody(x, y, z),
+            q=Quaternion(1.0, 0.0, 0.0, 0.0),
+            speed_body=SpeedBody(vx, vy, 0.0),
+            angular_velocity_body=AngularVelocityBody(0.0, 0.0, 0.0),
+            pose_covariance=nan_cov,
+            velocity_covariance=nan_cov,
+        )
+        await drone.mocap.set_odometry(odom)
+
+        if i % 90 == 0:
+            print(f"  t={elapsed:.1f}s  pos=({x:.2f}, {y:.2f}, {z:.2f})")
+
+        await asyncio.sleep(1.0 / 30)
+
+    print("-- Done sending odometry")
+
+
+if __name__ == "__main__":
+    asyncio.run(run())