Advertisement
Guest User

ok

a guest
Jul 13th, 2015
501
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.95 KB | None | 0 0
  1. #!/usr/bin/env python
  2.  
  3. import roslib
  4. import rospy
  5. import serial
  6. from math import pi
  7. import math
  8. import tf
  9. import time
  10. import geometry_msgs
  11. import nav_msgs
  12. from nav_msgs.msg import Odometry
  13. from geometry_msgs.msg import Twist
  14.  
  15.  
  16. odom_frame = "odom"
  17. base_frame = "base_link"
  18. lin_x = 0.0
  19. ang_z = 0.0
  20.  
  21.  
  22.  
  23. def get_vel(data):
  24. global lin_x
  25. global ang_z
  26. lin_x = data.linear.x
  27. ang_z = data.angular.z
  28.  
  29.  
  30. if __name__=='__main__':
  31. try:
  32. rospy.init_node('odom_send')
  33.  
  34. odom_pub = rospy.Publisher('odom', Odometry, queue_size = 50)
  35. rospy.loginfo("Starting odom node")
  36.  
  37. tf_listener = tf.TransformListener()
  38.  
  39. odom = nav_msgs.msg.Odometry()
  40. current_time = rospy.Time.now()
  41.  
  42. odom.header.frame_id = odom_frame
  43. odom.child_frame_id = base_frame
  44.  
  45. rospy.Subscriber("turtle1/cmd_vel", Twist, get_vel)
  46.  
  47. rate = rospy.get_param('~rate', 20)
  48. r = rospy.Rate(rate)
  49. while not rospy.is_shutdown():
  50. try:
  51. current_time = rospy.Time.now()
  52. trans,rot = tf_listener.lookupTransform(odom_frame, base_frame, rospy.Time() )
  53. odom.header.stamp = current_time
  54. odom.pose.pose.position.x = trans[0]
  55. print trans[0]
  56. odom.pose.pose.position.y = trans[1]
  57. odom.pose.pose.position.z = 0.0
  58. print rot
  59. odom.pose.pose.orientation.x = rot[0]
  60. odom.pose.pose.orientation.y = rot[1]
  61. odom.pose.pose.orientation.z = rot[2]
  62. odom.pose.pose.orientation.w = rot[3]
  63.  
  64. odom.twist.twist.linear.x = lin_x
  65. print lin_x
  66. odom.twist.twist.linear.y = 0.0
  67. odom.twist.twist.linear.z = 0.0
  68. odom.twist.twist.angular.x = 0.0
  69. odom.twist.twist.angular.y = 0.0
  70. odom.twist.twist.angular.z = ang_z
  71. print ang_z
  72. #print odom
  73.  
  74. odom_pub.publish(odom)
  75. rospy.loginfo("Public!")
  76. r.sleep()
  77. except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  78. continue
  79.  
  80. except rospy.ROSInterruptException as e:
  81. print e
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement