cmd_vel_to_ackermann_drive.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Author: christoph.roesmann@tu-dortmund.de
4 
5 import rospy, math
6 from geometry_msgs.msg import Twist
7 from ackermann_msgs.msg import AckermannDriveStamped
8 
9 
10 def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
11  if omega == 0 or v == 0:
12  return 0
13 
14  radius = v / omega
15  return math.atan(wheelbase / radius)
16 
17 
18 def cmd_callback(data):
19  global wheelbase
20  global ackermann_cmd_topic
21  global frame_id
22  global pub
23 
24  v = data.linear.x
25  steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
26 
27  msg = AckermannDriveStamped()
28  msg.header.stamp = rospy.Time.now()
29  msg.header.frame_id = frame_id
30  msg.drive.steering_angle = steering
31  msg.drive.speed = v
32 
33  pub.publish(msg)
34 
35 
36 
37 
38 
39 if __name__ == '__main__':
40  try:
41 
42  rospy.init_node('cmd_vel_to_ackermann_drive')
43 
44  twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel')
45  ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
46  wheelbase = rospy.get_param('~wheelbase', 1.0)
47  frame_id = rospy.get_param('~frame_id', 'odom')
48 
49  rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
50  pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
51 
52  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)
53 
54  rospy.spin()
55 
56  except rospy.ROSInterruptException:
57  pass
58 
def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10