Sending VISION_POSE_ESTIMATE message via MAVlink

send_vision_pose.py

import numpy as np
import time

from dronekit import connect, VehicleMode

vehicle = connect('udp:127.0.0.1:14551', wait_ready = True, source_system = 255)


def send_vision_position_message():
    x, y, z = np.random.rand(3, ) * 10
    r,p,y1 = 0,0,0

    print("INFO: ", end="")
    print([x,y,z,r,p,y1])

    ns = int(time.time()*1e6)
    msg = vehicle.message_factory.vision_position_estimate_encode(ns,x,y,z,r,p,y1)

    vehicle.send_mavlink(msg)
    vehicle.flush()

if __name__ == "__main__":
    while True:
        send_vision_position_message()
        print("Message sent!")


        rate_hz = 20
        time.sleep(1/rate_hz)