Vision Posisition EKF2 Issue with VICON +Mavros+Mavproxy

ROS 1 / ROS 2
1

Hello,

I am working on implementing an indoor position system for a custom build coaxial Hexcopter. I am running into a issue with having the EKF2 actually use the vision position data provided to it.

System Architechture:
Pixhawk Cube Orange
3DR SIK Radio Telemetry @ 57600
Mavrpoxy with 2 links out (1 for QGC, 1 for MAVROS)
MAVROS (ROS Noetic UBUNTU 20.04)
Vicon Bridge
Custom File to convert Vicon Data to /mavros/vision_pose/pose (See Code Below)

I am able to echo the /mavros/vision_pose/pose topic and see vicon data stream there, and further I can see inside QGC the VISION_POSITION_ESTIMATE uORB topic the data is publishing at 30Hz (ive tried throttling down to 10hz and up to 120hz no effect)

I have also attempted to revert to LPE and use MOCAP to no effect as well.

Can anyone provide any insight into what issue would be? It seems the breakdown is in estimator not using the data on pixahawk, could it be format issue or timing issue? How could I debug this

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped, TransformStamped
from sensor_msgs.msg import TimeReference  # Import the TimeReference message type
import tf

# Reference point at Vicon system's origin
REF_X = 0.0
REF_Y = 0.0
REF_Z = -0.5

# Global variable to store the Pixhawk time
pixhawk_time = None

def convert_to_pose(vicon_data):
    global pixhawk_time

    pose_msg = PoseStamped()

    # Use Pixhawk time if available, else use ROS time
    if pixhawk_time is not None:
        pose_msg.header.stamp = pixhawk_time
    else:
        pose_msg.header.stamp = rospy.Time.now()

    pose_msg.header.frame_id = "world"

    # Position
    pose_msg.pose.position.x = REF_X + vicon_data.transform.translation.x
    pose_msg.pose.position.y = REF_Y + vicon_data.transform.translation.y
    pose_msg.pose.position.z = REF_Z + vicon_data.transform.translation.z

    # Orientation
    pose_msg.pose.orientation.x = vicon_data.transform.rotation.x
    pose_msg.pose.orientation.y = vicon_data.transform.rotation.y
    pose_msg.pose.orientation.z = vicon_data.transform.rotation.z
    pose_msg.pose.orientation.w = vicon_data.transform.rotation.w

    return pose_msg

def time_reference_callback(time_ref_msg):
    global pixhawk_time
    pixhawk_time = time_ref_msg.header.stamp

def vicon_callback(data):
    global last_time_published

    current_time = rospy.Time.now()

    if (current_time - last_time_published).to_sec() < 1.0 / 50.0:
        return

    pose_msg = convert_to_pose(data)

    pub.publish(pose_msg)
    #pub2.publish(pose_msg)
    last_time_published = current_time

def vicon_to_px4_vision_pose():
    #global pub, pub2, last_time_published
    global pub, last_time_published
    rospy.init_node('vicon_to_px4_vision_pose_node', anonymous=True)

    # Subscribe to the Pixhawk time topic
    rospy.Subscriber('/mavros/time_reference', TimeReference, time_reference_callback)

    pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
    #pub2 = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10)
    last_time_published = rospy.Time.now()

    rospy.Subscriber('/vicon/RMAL_P_01/RMAL_P_01', TransformStamped, vicon_callback)

    rospy.spin()

if __name__ == '__main__':
    try:
        vicon_to_px4_vision_pose()
    except rospy.ROSInterruptException:
        pass