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

Christoph-UNM
Hi

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.x=0.000
        self.controller.y=0.000
 
        self.takeoff()
 
        self.publishTopic()
        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'
        self.pubMavCTRL.publish(self.controller)
    def land(self):
        self.controller.z = 0.0
    def takeoff(self):
        self.controller.z = 0.5

if __name__ ==  '__main__':
    rospy.init_node('blahDemoNode',anonymous=True)
    while not rospy.is_shutdown():
        takeoffDemoFile()
 
Loading...