teb_local_planner: cmd_vel_to_ackermann

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Author: christoph.roesmann@tu-dortmund.de
00004 
00005 import rospy, math
00006 from geometry_msgs.msg import Twist
00007 from ackermann_msgs.msg import AckermannDriveStamped
00008 
00009 
00010 def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
00011   if omega == 0 or v == 0:
00012     return 0
00013 
00014   radius = v / omega
00015   return math.atan(wheelbase / radius)
00016 
00017 
00018 def cmd_callback(data):
00019   global wheelbase
00020   global ackermann_cmd_topic
00021   global frame_id
00022   global pub
00023   
00024   v = data.linear.x
00025   steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
00026   
00027   msg = AckermannDriveStamped()
00028   msg.header.stamp = rospy.Time.now()
00029   msg.header.frame_id = frame_id
00030   msg.drive.steering_angle = steering
00031   msg.drive.speed = v
00032   
00033   pub.publish(msg)
00034   
00035 
00036 
00037 
00038 
00039 if __name__ == '__main__': 
00040   try:
00041     
00042     rospy.init_node('cmd_vel_to_ackermann_drive')
00043         
00044     twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel') 
00045     ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
00046     wheelbase = rospy.get_param('~wheelbase', 1.0)
00047     frame_id = rospy.get_param('~frame_id', 'odom')
00048     
00049     rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
00050     pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
00051     
00052     rospy.loginfo("Node 'cmd_vel_to_ackermann_drive' started.\nListening to %s, publishing to %s. Frame id: %s, wheelbase: %f", "/cmd_vel", ackermann_cmd_topic, frame_id, wheelbase)
00053     
00054     rospy.spin()
00055     
00056   except rospy.ROSInterruptException:
00057     pass
00058