feeding vicon data to mav framework

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
Report Content as Inappropriate

feeding vicon data to mav framework


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
import rospy
import numpy
from asctec_hl_comm.msg import mav_ctrl
import time
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
class takeoffDemoFile(object):
    def __init__(self):
        self.pubMavCTRL = rospy.Publisher('/fcu/control',mav_ctrl,queue_size=10)
        self.subVicon = rospy.Subscriber('/quadrotor_orange_raw' , TransformStamped , self.subscribeTopic)

        self.odom = Odometry()

        print 'publish'
        self.controller = mav_ctrl()
        self.controller.type = 3
        self.controller.v_max_z = 0.1
        self.controller.v_max_xy = 0.5
        self.controller = mav_ctrl()

    def subscribeTopic(self,data):
        self.current = numpy.array([data.transform.translation.x , data.transform.translation.y , data.transform.translation.z])
    def publishTopic(self):
       print 'publishing'
    def land(self):
        self.controller.z = 0.0
    def takeoff(self):
        self.controller.z = 0.5

if __name__ ==  '__main__':
    while not rospy.is_shutdown():