-
Notifications
You must be signed in to change notification settings - Fork 5
/
yaw_controller.py
executable file
·30 lines (22 loc) · 1.21 KB
/
yaw_controller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
from math import atan
import rospy
class YawController(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle):
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.min_speed = min_speed
self.max_lat_accel = max_lat_accel
self.min_angle = -max_steer_angle
self.max_angle = max_steer_angle
def get_angle(self, radius):
angle = atan(self.wheel_base / radius) * self.steer_ratio
#rospy.loginfo('Angle: %f', angle)
return max(self.min_angle, min(self.max_angle, angle))
def get_steering(self, linear_velocity, angular_velocity, current_velocity):
angular_velocity = current_velocity * angular_velocity / linear_velocity if abs(linear_velocity) > 0. else 0.
if abs(current_velocity) > 0.1:
max_yaw_rate = abs(self.max_lat_accel / current_velocity);
#rospy.loginfo('Max yaw rate: %f', max_yaw_rate)
#rospy.loginfo('Angular velocity %f', angular_velocity)
angular_velocity = max(-max_yaw_rate, min(max_yaw_rate, angular_velocity))
return self.get_angle(max(current_velocity, self.min_speed) / angular_velocity) if abs(angular_velocity) > 0. else 0.0;