0
$\begingroup$

Rosanswers logo

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/autonavx_ardrone/blob/master/ardrone_python/src/example1_odometry.py

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

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

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


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

This answer was ACCEPTED on the original site

Post score: 3

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.