Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/usr/bin/env python
- import roslib
- import rospy
- import serial
- from math import pi
- import math
- import tf
- import time
- import geometry_msgs
- import nav_msgs
- from nav_msgs.msg import Odometry
- from geometry_msgs.msg import Twist
- odom_frame = "odom"
- base_frame = "base_link"
- lin_x = 0.0
- ang_z = 0.0
- def get_vel(data):
- global lin_x
- global ang_z
- lin_x = data.linear.x
- ang_z = data.angular.z
- if __name__=='__main__':
- try:
- rospy.init_node('odom_send')
- odom_pub = rospy.Publisher('odom', Odometry, queue_size = 50)
- rospy.loginfo("Starting odom node")
- tf_listener = tf.TransformListener()
- odom = nav_msgs.msg.Odometry()
- current_time = rospy.Time.now()
- odom.header.frame_id = odom_frame
- odom.child_frame_id = base_frame
- rospy.Subscriber("turtle1/cmd_vel", Twist, get_vel)
- rate = rospy.get_param('~rate', 20)
- r = rospy.Rate(rate)
- while not rospy.is_shutdown():
- try:
- current_time = rospy.Time.now()
- trans,rot = tf_listener.lookupTransform(odom_frame, base_frame, rospy.Time() )
- odom.header.stamp = current_time
- odom.pose.pose.position.x = trans[0]
- odom.pose.pose.position.y = trans[1]
- odom.pose.pose.position.z = 0.0
- odom.pose.pose.orientation = rot
- odom.twist.twist.linear.x = lin_x
- odom.twist.twist.linear.y = 0.0
- odom.twist.twist.linear.z = 0.0
- odom.twist.twist.angular.x = 0.0
- odom.twist.twist.angular.y = 0.0
- odom.twist.twist.angular.z = ang_z
- print odom
- odom_pub.publish(odom)
- rospy.loginfo("Public!")
- r.sleep()
- except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
- continue
- except rospy.ROSInterruptException as e:
- print e
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement