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