getOdom2D.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
3 
4 import roslib; roslib.load_manifest('kobuki_node')
5 import rospy
6 
7 from tf.transformations import euler_from_quaternion
8 from math import degrees
9 
10 from nav_msgs.msg import Odometry
11 from geometry_msgs.msg import Quaternion
12 from geometry_msgs.msg import Pose2D
13 
14 class Converter(object):
15  def __init__(self):
16  rospy.init_node("getOdom2D", anonymous=True)
17  self.sub = rospy.Subscriber("odom", Odometry, self.OdomCallback)
18  self.pub = rospy.Publisher("pose2d", Pose2D, queue_size=10)
19 
20  def OdomCallback(self,data):
21  px = data.pose.pose.position.x
22  py = data.pose.pose.position.y
23  quat = data.pose.pose.orientation
24  q = [quat.x, quat.y, quat.z, quat.w]
25  roll, pitch, yaw = euler_from_quaternion(q)
26 
27  vx = data.twist.twist.linear.x
28  vy = data.twist.twist.linear.y
29  yaw_rate = data.twist.twist.angular.z
30  print "pose: x: {0:+2.5f}".format(px) + ", y: {0:+2.5f}".format(py)\
31  + ", th: {0:+.4f}".format(yaw) + " rad; "\
32  + "{0:+.2f}".format(degrees(yaw)) + " deg"
33  print "rate: x: {0:+2.5f}".format(vx) + ", y: {0:+2.5f}".format(vy)\
34  + ", th: {0:+.2f}".format(yaw_rate) + " rad/s; "\
35  + "{0:+.2f}".format(degrees(yaw_rate)) + " deg/s"
36  print '---'
37  pose2d = Pose2D()
38  pose2d.x = px
39  pose2d.y = py
40  pose2d.theta = yaw
41  self.pub.publish(pose2d)
42 
43 if __name__ == '__main__':
44  try:
45  instance = Converter()
46  print
47  print "It prints x, y, theta values from Odom message of mobile base."
48  print
49  rospy.spin()
50  except rospy.ROSInterruptException: pass
def OdomCallback(self, data)
Definition: getOdom2D.py:20
def __init__(self)
Definition: getOdom2D.py:15


kobuki_node
Author(s): Daniel Stonier, Younghun Ju, Jorge Santos Simon
autogenerated on Mon Jun 10 2019 13:45:13