Revision history [back]

Thank you so much for your help!

There was a problem with: position = position and dt variable, so I modified it.

I understand that dt = time_current - time_last. It runs but it gives wrong values, any idea? http://pastebin.com/9dqjCs6K

Thank you so much for your help!

There was a problem with: position = position and dt variable, so I modified it.

I understand that dt = time_current - time_last. It runs but it gives wrong values, any idea? ~~http://pastebin.com/9dqjCs6K~~

#!/usr/bin/env python

#ROS
import rospy
import roslib; roslib.load_manifest('ardrone_python')
from ardrone_autonomy.msg import Navdata
import numpy as np


position = np.array([[0], [0]])
last_t = 0;

def rotation_to_world(yaw):
        from math import cos, sin
        return np.array([[cos(yaw), -sin(yaw)], [sin(yaw), cos(yaw)]])


def callback(navdata):
    global position, last_t

    current_t = navdata.header.stamp.to_sec()
    dt = (current_t - last_t)
    position_temp = position + dt * np.dot(rotation_to_world(navdata.rotZ), np.array([[navdata.vx], [navdata.vy]]))
    position = position_temp    
    print("received odometry message: vx=%f vy=%f z=%f yaw=%f"%(navdata.vx,navdata.vy,navdata.altd,navdata.rotZ))
    print(position[0])
    print(position[1])
    last_t = current_t

if __name__ == '__main__':
    rospy.init_node('example_node', anonymous=True)

    # subscribe to navdata (receive from quadrotor)
    rospy.Subscriber("/ardrone/navdata", Navdata, callback)

    rospy.spin()