ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# 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
from ardrone_autonomy.msg import Navdata
import numpy as np

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

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 close merge delete

Sort by » oldest newest most voted

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([, ])

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

more

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
from ardrone_autonomy.msg import Navdata
import numpy as np

position = np.array([, ])
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

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)
print(position)
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()

more

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?

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