class Controller(object):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
min_speed = 0.1
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
mn = 0. # Minimum throttle value
mx = 0.2 # Maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = .02 # Sample time (50 hertz)
# ts = .033 # Sample time (30 hertz)
self.vel_lpf = LowPassFilter(tau, ts)
self.ang_vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
def control(self, linear_vel, angular_vel, current_vel, current_ang_vel, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
if not dbw_enabled:
# Reset throttle and steering
self.throttle_controller.reset()
self.yaw_controller.reset()
return 0., 0., 0.
# rospy.logwarn("Curr ang vel: {0}".format(current_ang_vel))
current_vel = self.vel_lpf.filt(current_vel)
current_ang_vel = self.ang_vel_lpf.filt(current_ang_vel)
# rospy.logwarn("Angular vel: {0}".format(angular_vel))
# rospy.logwarn("Target vel: {0}".format(linear_vel))
# rospy.logwarn("Current vel: {0}".format(current_vel))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel, current_ang_vel)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
# Throttle to steering ratio function Reduce throttle for larger steerng
# if steering is not None:
# throttle = -0.0015625 * steering * steering + throttle
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 700 #N*m - to hold the car in place if we are stopped at a light. Accl - 1m/s^2
elif throttle < .1 and vel_error <0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(decel)*self.vehicle_mass*self.wheel_radius # Torque N*m
# rospy.logwarn("Throttle: {0}".format(throttle))
# rospy.logwarn("Brake: {0}".format(brake))
# rospy.logwarn("Steering: {0}".format(steering))
# Return throttle, brake, steer
# return 1., 0., 0.
return throttle, brake, steering
class Controller(object):
def __init__(
self,
dbw_parameters, # Set of base parameters
):
self.is_initialized = False
self.last_time = rospy.get_time()
self.dbw_parameters = dbw_parameters
self.steering_controller = YawController(
wheel_base = self.dbw_parameters['wheel_base'],
steer_ratio = self.dbw_parameters['steer_ratio'],
min_speed = self.dbw_parameters['min_vehicle_speed'],
max_lat_accel = self.dbw_parameters['max_lat_accel'],
max_steer_angle = self.dbw_parameters['max_steer_angle'])
self.low_pass_filter_vel = LowPassFilter(samples_num = LOW_PASS_FREQ_VEL * self.dbw_parameters['dbw_rate'])
self.low_pass_filter_yaw = LowPassFilter(samples_num = LOW_PASS_FREQ_YAW * self.dbw_parameters['dbw_rate'])
self.throttle_controller = PIDController(
kp = 0.3,
ki = 0.01,
kd = 0.01,
mn = self.dbw_parameters['vehicle_min_throttle_val'],
mx = self.dbw_parameters['vehicle_max_throttle_val'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
self.steering_controller2 = PIDController(
kp = 3.0,
ki = 0.0,
kd = 1.0,
mn = -self.dbw_parameters['max_steer_angle'],
mx = self.dbw_parameters['max_steer_angle'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
def control(
self,
target_dx,
target_dyaw,
current_dx,
current_dyaw,
dbw_status = True):
throttle = 0
brake = 0
steering = 0
correct_data = False
cur_time = rospy.get_time()
current_dx_smooth = self.low_pass_filter_vel.filt(current_dx)
target_dyaw_smooth = self.low_pass_filter_yaw.filt(target_dyaw)
if not dbw_status:
self.is_initialized = False
else:
if not self.is_initialized:
self.reset()
self.is_initialized = True
dv = target_dx - current_dx_smooth
dt = cur_time - self.last_time
if dt >= MIN_TIME_DELTA:
steering = self.steering_controller.get_steering(target_dx, target_dyaw_smooth, current_dx_smooth) + self.steering_controller2.step(target_dyaw_smooth, dt)
steering = max(min(steering, self.dbw_parameters['max_steer_angle']), -self.dbw_parameters['max_steer_angle'])
throttle = self.throttle_controller.step(dv, dt)
if target_dx <= current_dx_smooth and current_dx_smooth <= self.dbw_parameters['min_vehicle_speed']:
throttle = 0.0
brake = self.dbw_parameters['min_vehicle_break_torque']
elif throttle <= self.dbw_parameters['vehicle_throttle_dead_zone']:
deceleration = abs(self.dbw_parameters['decel_limit'] * (throttle - self.dbw_parameters['vehicle_throttle_dead_zone']) / (self.dbw_parameters['vehicle_throttle_dead_zone'] - self.dbw_parameters['vehicle_min_throttle_val']))
throttle = 0.0
brake = self.dbw_parameters['vehicle_mass'] * self.dbw_parameters['wheel_radius'] * deceleration
correct_data = True
self.last_time = cur_time
return throttle, brake, steering, correct_data
def reset(self):
self.steering_controller.reset()
self.steering_controller2.reset()
self.throttle_controller.reset()
