I want to use vicon instead of gps. I was able to get the mav framework running and I am able to get raw vicon data to the quad. it gives me xyz position and orientation in quaternion. I am trying to send that data to the mav framework. I am having problems with flying the quadrotor. When I echo /fcu/current_pose (I think thats how the rostopic is called) I get numbers that are off by a lot. From my understanding this rostopic should give me the position mav framework is seeing. I am using python to subsrcibe and publish my topics as followed. Is there a mistake in the topic I publish too?
#! /usr/bin/env python
from asctec_hl_comm.msg import mav_ctrl
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry