Newbie python script, odometry

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:

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
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)

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


Originally posted by green96 on ROS Answers with karma: 115 on 2014-11-10

Post score: 1

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
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)

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

Originally posted by Gary Servin with karma: 962 on 2014-11-10

This answer was ACCEPTED on the original site

Post score: 3