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,146 @@
+#!/usr/bin/env python3
+
+"""
+Send visual odometry to PX4 and verify EKF2 fusion.
+
+Demonstrates how to feed external position estimates (from a VIO pipeline,
+motion capture system, or visual odometry) to PX4 using the Mocap plugin's
+set_odometry() API.
+
+What this example does:
+    1. Connects to PX4 and configures EKF2 for external vision fusion
+    2. Sends simulated ODOMETRY messages at 30 Hz (a slow circle)
+    3. Reads back PX4's position estimate to verify fusion is working
+    4. Prints both SENT and RECEIVED positions side-by-side
+
+What this example does NOT do:
+    - It does not arm or fly the drone
+    - The simulated positions are fake (in a real setup, they come from
+      a camera/VIO pipeline like T265, ORB-SLAM, OpenCV VO, etc.)
+
+Prerequisites:
+    - PX4 simulator running (e.g. make px4_sitl gz_x500)
+    - pip install mavsdk
+
+Usage:
+    python3 vision_odometry.py
+
+After running, you should see PX4's estimated position converge to match
+the sent odometry within a few seconds. This confirms EKF2 is fusing
+your external vision data.
+"""
+
+import asyncio
+import math
+import time
+
+from mavsdk import System
+from mavsdk.mocap import (
+    Odometry,
+    PositionBody,
+    Quaternion,
+    SpeedBody,
+    AngularVelocityBody,
+    Covariance,
+)
+
+# How long to run (seconds) and send rate (Hz)
+DURATION_S = 20
+SEND_RATE_HZ = 30
+
+
+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 to fuse external vision for position + velocity + yaw
+    print("Configuring EKF2 for vision fusion...")
+    await drone.param.set_param_int("EKF2_EV_CTRL", 15)  # pos_xy + pos_z + vel + yaw
+    await drone.param.set_param_int("EKF2_HGT_REF", 3)  # vision as height reference
+    print("  EKF2_EV_CTRL = 15 (fuse pos_xy + pos_z + vel + yaw)")
+    print("  EKF2_HGT_REF = 3  (vision height reference)")
+
+    # Run all tasks in parallel
+    print()
+    print("Sending odometry and monitoring EKF2 output...")
+    print("-" * 70)
+    await asyncio.gather(
+        send_odometry(drone),
+        print_ekf2_position(drone),
+        wait_for_fusion(drone),
+    )
+
+
+async def wait_for_fusion(drone):
+    """Wait until EKF2 reports local position is OK (fusion active)."""
+    async for health in drone.telemetry.health():
+        if health.is_local_position_ok:
+            print("[HEALTH] EKF2 fusion active -- local position OK!")
+            return
+
+
+async def print_ekf2_position(drone):
+    """Read back PX4's EKF2 estimated position to verify fusion."""
+    start = time.monotonic()
+    async for pos_vel in drone.telemetry.position_velocity_ned():
+        elapsed = time.monotonic() - start
+        if elapsed > DURATION_S:
+            break
+        n = pos_vel.position.north_m
+        e = pos_vel.position.east_m
+        d = pos_vel.position.down_m
+        print(f"  [EKF2 ] t={elapsed:5.1f}s  pos=({n:+7.2f}, {e:+7.2f}, {d:+7.2f})")
+        await asyncio.sleep(3.0)
+
+
+async def send_odometry(drone):
+    """Send simulated vision odometry at SEND_RATE_HZ for DURATION_S."""
+    nan_cov = Covariance([float("nan")])  # unknown covariance
+    start_time = time.monotonic()
+    total_frames = DURATION_S * SEND_RATE_HZ
+
+    for i in range(total_frames):
+        elapsed = time.monotonic() - start_time
+
+        # Simulated trajectory: slow circle, 0.5 m radius, ~12.6 s period
+        x = 0.5 * math.cos(elapsed * 0.5)
+        y = 0.5 * math.sin(elapsed * 0.5)
+        z = 0.0  # NED: 0 means same altitude as start
+
+        vx = -0.25 * math.sin(elapsed * 0.5)
+        vy = 0.25 * math.cos(elapsed * 0.5)
+
+        odom = Odometry(
+            time_usec=0,  # 0 = let autopilot timestamp it
+            frame_id=Odometry.MavFrame.MOCAP_NED,
+            position_body=PositionBody(x, y, z),
+            q=Quaternion(1.0, 0.0, 0.0, 0.0),  # identity = no rotation
+            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)
+
+        # Print sent position every 3 seconds
+        if i % (SEND_RATE_HZ * 3) == 0:
+            print(f"  [SENT ] t={elapsed:5.1f}s  pos=({x:+7.2f}, {y:+7.2f}, {z:+7.2f})")
+
+        await asyncio.sleep(1.0 / SEND_RATE_HZ)
+
+    print()
+    print(
+        "-- Done. Sent {} frames over {:.0f}s at {} Hz".format(
+            total_frames, DURATION_S, SEND_RATE_HZ
+        )
+    )
+
+
+if __name__ == "__main__":
+    asyncio.run(run())