getOdom2D.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
00003 
00004 import roslib; roslib.load_manifest('kobuki_node')
00005 import rospy
00006 
00007 from tf.transformations import euler_from_quaternion
00008 from math import degrees
00009 
00010 from nav_msgs.msg import Odometry
00011 from geometry_msgs.msg import Quaternion
00012 from geometry_msgs.msg import Pose2D 
00013 
00014 class Converter(object):
00015         def __init__(self):
00016                 rospy.init_node("getOdom2D", anonymous=True)
00017                 self.sub = rospy.Subscriber("odom", Odometry, self.OdomCallback)
00018                 self.pub = rospy.Publisher("pose2d", Pose2D, queue_size=10)
00019                 
00020         def OdomCallback(self,data):
00021                 px = data.pose.pose.position.x
00022                 py = data.pose.pose.position.y
00023                 quat = data.pose.pose.orientation
00024                 q = [quat.x, quat.y, quat.z, quat.w]
00025                 roll, pitch, yaw = euler_from_quaternion(q)
00026         
00027                 vx = data.twist.twist.linear.x
00028                 vy = data.twist.twist.linear.y
00029                 yaw_rate = data.twist.twist.angular.z
00030                 print "pose: x: {0:+2.5f}".format(px) + ", y: {0:+2.5f}".format(py)\
00031                 + ", th: {0:+.4f}".format(yaw) + " rad; "\
00032                 + "{0:+.2f}".format(degrees(yaw)) + " deg"
00033                 print "rate: x: {0:+2.5f}".format(vx) + ", y: {0:+2.5f}".format(vy)\
00034                 + ", th: {0:+.2f}".format(yaw_rate) + " rad/s; "\
00035                 + "{0:+.2f}".format(degrees(yaw_rate)) + " deg/s"
00036                 print '---'
00037                 pose2d = Pose2D()
00038                 pose2d.x = px
00039                 pose2d.y = py
00040                 pose2d.theta = yaw
00041                 self.pub.publish(pose2d)
00042 
00043 if __name__ == '__main__':      
00044         try:
00045                 instance = Converter()
00046                 print 
00047                 print "It prints x, y, theta values from Odom message of mobile base."
00048                 print
00049                 rospy.spin()
00050         except rospy.ROSInterruptException: pass


kobuki_node
Author(s): Daniel Stonier, Younghun Ju, Jorge Santos Simon
autogenerated on Thu Jun 6 2019 17:37:58