Ask Your Question
1

Newbie python script, odometry

asked 2014-11-10 10:02:30 -0500

green96 gravatar image

updated 2014-11-10 10:03:23 -0500

I was doing one online course where on page was special frame to run python script. My task in this exercise was to compute the odometry, velocities are given.

This script on page looks: http://snag.gy/NTJGz.jpg

Now I would like to do the same using ROS: there is nearly the same exercise but in ROS:

clear code looks: https://github.com/tum-vision/autonav...

There is information that I should add code from this online_course version to function callback, I try, but it doesn't work.

My code:

#!/usr/bin/env python

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

def __init__(self):
    self.position = np.array([[0], [0]])


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


def callback(self, t, dt, navdata):
        self.position = self.position + dt * np.dot(self.rotation_to_world(navdata.rotZ), np.array([[navdata.vx], [navdata.vy]]))      
        print("received odometry message: vx=%f vy=%f z=%f yaw=%f"%(navdata.vx,navdata.vy,navdata.altd,navdata.rotZ))
        print(self.position)


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

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

    rospy.spin()

Please correct me, I am totally newbie to python.

Now I get message:

Traceback (most recent call last): File "./example1_odometry.py", line 28, in rospy.Subscriber("/ardrone/navdata", Navdata, callback(self, t, dt, navdata)) NameError: name 'self' is not defined
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
3

answered 2014-11-10 10:49:08 -0500

updated 2014-11-10 13:30:50 -0500

You can take a look here about what self means in python.

The issue you're having is that the callback, rotation and __init__ methods and the position variable were originally part of a class. You need to remove the __init__ method, "declare" the position variable and remove all references to self from your code.

#!/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]])


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


def callback(t, dt, navdata):
        position = position + dt * np.dot(rotation_to_world(navdata.rotZ), np.array([[navdata.vx], [navdata.vy]]))      
        print("received odometry message: vx=%f vy=%f z=%f yaw=%f"%(navdata.vx,navdata.vy,navdata.altd,navdata.rotZ))
        print(position)


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

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

    rospy.spin()

I haven't tested if this code actually works, but it us just to give you an idea on how to solve your issue.

Hope it helps

edit flag offensive delete link more
0

answered 2014-11-10 18:14:57 -0500

green96 gravatar image

updated 2014-11-10 18:21:59 -0500

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?

#!/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()
edit flag offensive delete link more

Comments

Good to hear that it worked, can you try to avoid creating a new answer and instead just comment on the existing answer as it is more complicated to follow the flow of the question->answer. Also, please mark the answer as correct if it helped you fix the issue.

What values do you get for example?

Gary Servin gravatar image Gary Servin  ( 2014-11-10 21:01:04 -0500 )edit

Sorry, answer marked. It gives strange values which are growing, Once I run script, I don't change pose of the quadrotor and it gives values: x, y: [ 6085.4490633 ] [ -45.54289464]] Next I terminate script and run again and start values are: [[ 6475.65030521] [ -52.30088198]]

green96 gravatar image green96  ( 2014-11-11 04:12:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-11-10 10:02:30 -0500

Seen: 1,341 times

Last updated: Nov 10 '14