def __init__(self, **ros_param):
"""
:param ros_param:
Note:
sample time (sec) is based on the dbw node frequency 50Hz
low_pass filter:
val = w * cur_val + (1 - w) * prev_val
w is 0 ~ 1
"""
# used ros_param
self.vehicle_mass = ros_param['vehicle_mass']
# self.fuel_capacity = ros_param['fuel_capacity']
self.brake_deadband = ros_param['brake_deadband']
self.decel_limit = ros_param['decel_limit']
self.accel_limit = ros_param['accel_limit']
self.wheel_radius = ros_param['wheel_radius']
self.last_time = rospy.get_time()
# low pass filter for velocity
self.vel_lpf = LowPassFilter(0.5, .02)
# Init yaw controller
min_speed = 0.1 # I think min_speed
self.steer_controller = YawController(min_speed, **ros_param)
self.throttle_lpf = LowPassFilter(0.05, 0.02) # w = 0.28
# Init throttle PID controller
# TODO: tweaking
kp = 0.5
ki = 0.005
kd = 0.1
acc_min = 0.
acc_max = 0.5 #self.accel_limit
self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max)
def __init__(self, *args, **kwargs):
# TODO: Implement
self.vehicle_mass = kwargs['vehicle_mass']
self.fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
self.accel_limit = kwargs['accel_limit']
self.wheel_radius = kwargs['wheel_radius']
self.wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
self.max_lat_accel = kwargs['max_lat_accel']
self.max_steer_angle = kwargs['max_steer_angle']
self.min_speed = 0.0
self.yaw_contoller = YawController(self.wheel_base,
self.steer_ratio,
self.min_speed,
self.max_lat_accel,
self.max_steer_angle)
kp = 0.1
ki = 0.007
kd = 0.0
minth = 0.0
maxth = 0.2
self.throttle_contoller = PID(kp, ki, kd, minth, maxth)
tau = 150
ts = 75
self.lpf_velocity = LowPassFilter(tau, ts)
# self.lpf_steering = LowPassFilter(tau, ts)
self.t0 = rospy.get_time()
def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle,
decel_limit, vehicle_mass):
self.yaw_controller = YawController(
wheel_base,
steer_ratio,
0.1, # min_speed
max_lat_accel,
max_steer_angle,
)
# PID coefficients
kp = 0.3
ki = 0.1
kd = 0.0
# For convenience (no real limits on throttle):
mn_th = 0.0 # Minimal throttle
mx_th = 0.2 # Maximal throttle
self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th)
tau = 0.5 # 1 / (2pi*tau) = cutoff frequency
ts = 0.02 # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.wheel_radius = wheel_radius
self.decel_limit = decel_limit
self.vehicle_mass = vehicle_mass
self.last_time = rospy.get_time()
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):
min_speed = 1.0 * ONE_MPH
self.pid = PID(2.0, 0.4, 0.1)
self.lpf = LowPassFilter(0.5, 0.02)
self.yaw = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
self.v_mass = vehicle_mass
self.w_radius = wheel_radius
self.d_limit = decel_limit
self.last_time = None
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):
min_speed = 1.0 * ONE_MPH
self.pid = PID(2.0, 0.4, 0.1)
self.lpf = LowPassFilter(0.5, 0.02)
self.yaw = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
self.v_mass = vehicle_mass
self.w_radius = wheel_radius
self.d_limit = decel_limit
self.last_time = None
def control(self, proposed_v, proposed_omega, current_v):
# Return throttle, brake, steer
if self.last_time is None:
throttle, brake, steer = 1.0, 0.0, 0.0
else:
dt = time.time() - self.last_time
# get throttle, if negative, change it to break
error = proposed_v.x - current_v.x
throttle = self.pid.step(error, time.time() - self.last_time)
throttle = max(0.0, min(1.0, throttle))
if error < 0: # if we need to decelerate
deceleration = abs(error) / dt
if abs(deceleration ) > abs(self.d_limit) * 500:
deceleration = self.d_limit * 500
longitudinal_force = self.v_mass * deceleration
brake = longitudinal_force * self.w_radius
throttle = 0
else:
brake = 0
steer = self.yaw.get_steering(proposed_v.x, proposed_omega.z, current_v.x)
self.last_time = time.time() # update time
# rospy.logwarn('proposed v: {}'.format(proposed_v.x))
# rospy.logwarn('current v: {}'.format(current_v.x))
# rospy.logwarn('Error: {}'.format(proposed_v.x - current_v.x))
# rospy.logwarn('Throttle: {}'.format(throttle))
# rospy.logwarn('Brake: {}'.format(brake))
# rospy.logwarn('Steer: {}'.format(steer))
# rospy.logwarn('Speed: {}'.format(current_v))
# rospy.logwarn('')
return throttle, brake, steer
def __init__(self, *args, **kwargs):
vehicle_mass = kwargs['vehicle_mass']
fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
accel_limit = kwargs['accel_limit']
wheel_radius = kwargs['wheel_radius']
wheel_base = kwargs['wheel_base']
steer_ratio = kwargs['steer_ratio']
max_lat_accel = kwargs['max_lat_accel']
max_steer_angle = kwargs['max_steer_angle']
self.brake_tourque_const = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius
self.current_dbw_enabled = False
yaw_params = [wheel_base, steer_ratio, max_lat_accel, max_steer_angle]
self.yaw_controller = YawController(*yaw_params)
self.linear_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, accel_limit)
self.tau_correction = 0.2
self.ts_correction = 0.1
self.low_pass_filter_correction = LowPassFilter(self.tau_correction, self.ts_correction)
self.previous_time = None
pass
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,
max_throttle):
# Yaw controller to controller the turning
self.vehicle_min_velocity = .1
self.yaw_controller = YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=self.vehicle_min_velocity,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.steering_controller = PID(
kp=.0, ki=.001, kd=.1, min=-.2, max=.2)
# Throttle controller
self.throttle_controller = PID(
kp=.8, ki=.001, kd=.1, min=0, max=max_throttle)
# Remove high frequency noise on the velocity
self.low_pass = LowPassFilter(tau=.5, ts=.02)
# Assume the tank is full
fuel_mass = fuel_capacity * GAS_DENSITY
self.total_mass = vehicle_mass + fuel_mass
self.decel_limit = decel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
class Controller(object):
def __init__(self, *args, **kwargs):
vehicle_mass = kwargs['vehicle_mass']
fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
accel_limit = kwargs['accel_limit']
wheel_radius = kwargs['wheel_radius']
wheel_base = kwargs['wheel_base']
steer_ratio = kwargs['steer_ratio']
max_lat_accel = kwargs['max_lat_accel']
max_steer_angle = kwargs['max_steer_angle']
self.brake_tourque_const = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius
self.current_dbw_enabled = False
yaw_params = [wheel_base, steer_ratio, max_lat_accel, max_steer_angle]
self.yaw_controller = YawController(*yaw_params)
self.linear_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, accel_limit)
self.tau_correction = 0.2
self.ts_correction = 0.1
self.low_pass_filter_correction = LowPassFilter(self.tau_correction, self.ts_correction)
self.previous_time = None
pass
def update_sample_step(self):
current_time = rospy.get_time()
sample_step = current_time - self.previous_time if self.previous_time else 0.05
self.previous_time = current_time
return sample_step
def control(self, linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity, angular_current, dbw_enabled, final_waypoint1, final_waypoint2, current_location):
if (not self.current_dbw_enabled) and dbw_enabled:
self.current_dbw_enabled = True
self.linear_pid.reset()
self.previous_time = None
else:
self.current_dbw_enabled = False
linear_velocity_error = linear_velocity_setpoint - linear_current_velocity
sample_step = self.update_sample_step()
velocity_correction = self.linear_pid.step(linear_velocity_error, sample_step)
velocity_correction = self.low_pass_filter_correction.filt(velocity_correction)
if abs(linear_velocity_setpoint)<0.01 and abs(linear_current_velocity) < 0.3:
velocity_correction = self.decel_limit
throttle = velocity_correction
brake = 0.
if throttle < 0.:
decel = abs(throttle)
#[alexm]NOTE: let engine decelerate the car if required deceleration below brake_deadband
brake = self.brake_tourque_const * decel if decel > self.brake_deadband else 0.
throttle = 0.
#[alexm]::NOTE this lowpass leads to sending both throttle and brake nonzero. Maybe it is better to filter velocity_correction
#brake = self.low_pass_filter_brake.filt(brake)
#steering = self.yaw_controller.get_steering_pid(angular_velocity_setpoint, angular_current, dbw_enabled)
#steering = self.yaw_controller.get_steering_pid_cte(final_waypoint1, final_waypoint2, current_location, dbw_enabled)
#[alexm]::NOTE changed static 10.0 to linear_current_velocity and surprisingly car behave better on low speeds. Need to look close to formulas...
#PID also improves the same with the factor
#moved factor into function because limits are checked in that function
steering = self.yaw_controller.get_steering_calculated(linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity)
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_speed,
max_lat_accel, max_steer_angle, vehicle_mass, fuel_capacity,
brake_deadband, decel_limit, accel_limit, wheel_radius):
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.min_speed = min_speed
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.low_pass_steer_filter = LowPassFilter(0.3, 1.0)
self.acceleration_controller = PID(0.19, 0.00005, 0.017,
self.decel_limit, self.accel_limit)
self.throttle_controller = PID(1.5, 0.00007, 0.01, 0.0, 0.5)
self.steering_controller = PID(2.0, 0.0005, 0.1, -max_steer_angle,
max_steer_angle)
self.throttle_controller.reset()
self.acceleration_controller.reset()
self.lastControlTime = None
self.dbw_status = True
def control(self, linear_velocity, angular_velocity, current_velocity,
dbw_status):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# control is taken by driver
if not dbw_status:
if self.dbw_status:
self.throttle_controller.reset()
self.acceleration_controller.reset()
self.steering_controller.reset()
self.dbw_status = False
return None, None, None
current_time = time.time()
if self.lastControlTime is None:
sample_time = 0.01
else:
sample_time = current_time - self.lastControlTime
self.lastControlTime = current_time
throttle = 0.0
brake = 0.0
steering = self.steering_controller.step(
self.yaw_controller.get_steering(
abs(linear_velocity),
self.low_pass_steer_filter.filt(angular_velocity),
current_velocity), sample_time)
acceleration = self.acceleration_controller.step(
linear_velocity - current_velocity, sample_time)
if (acceleration < -self.brake_deadband):
brake = (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * abs(acceleration) * self.wheel_radius
throttle = self.throttle_controller.step(
linear_velocity - current_velocity, sample_time)
# Holding
if (linear_velocity < STOP_THRESHOLD_VELOCITY
and current_velocity < self.min_speed):
brake = (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * 0.3 * self.wheel_radius
throttle = 0
return throttle, brake, steering
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: change to using *args and **kwargs
# Yaw Controller
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
# PID Throttle controller
kp = 0.3
ki = 0.1
kd = 0.
mn = 0. # Minimum throttle value
mx = 0.25 # Maximum throttle value
tau = 0.5 # 1/(2pi * tau) = cutoff frequency
ts = 0.02 # Sample time
self.throttle_controller = PID(kp, ki, kd, mn, mx)
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn("Angular velocity: {0}".format(angular_vel))
# rospy.logwarn("Target velocity: {0}".format(linear_vel))
# rospy.logwarn("Target angular velocity: {0}".format(angular_vel))
# rospy.logwarn("Current velocity: {0}".format(current_vel))
# rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get()))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_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)
brake = 0
## Special Cases
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. Acceleration - 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
#return 1., 0., 0.
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle,
decel_limit, accel_limit, vehicle_total_mass, brake_deadband, wheel_radius):
# TODO: Implement
# pass
# Variables for Yaw, LowPass and PID controller
min_speed = 0.1
Kp = 1.1
Ki = 0.010
Kd = 0.005
pid_cmd_range = 4
filter_tau = 0.0
filter_ts = 0.8
# YawController
# def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
# LowPass Filter
# def __init__(self, tau, ts)
self.low_pass_filter = LowPassFilter(filter_tau, filter_ts)
# PID Controller for the target velocity
#def __init__(self, kp, ki, kd, mn=MIN_NUM, mx=MAX_NUM)
self.pid_controller = PID(Kp, Ki, Kd, decel_limit, accel_limit)
self.vehicle_total_mass = vehicle_total_mass
self.brake_deadband = brake_deadband
self.wheel_radius = wheel_radius
self.t_start = time.time()
self.dt = 0.0
self.feed_forward_brake_control = True
def control(self, current_velocity, linear_velocity, angular_velocity):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# return 1., 0., 0.
# Check if all required parameters are set
if not (self.vehicle_total_mass and self.brake_deadband and self.wheel_radius):
rospy.logerror('vehicle parameters not set')
# Apply the filter to the angular velocity
angular_velocity = self.low_pass_filter.filt(angular_velocity)
# Compute the steering angle
steer = self.yaw_controller.get_steering(linear_velocity, angular_velocity, current_velocity)
# Compute the throttle command
throttle_cmd = 0
velocity_error = linear_velocity - current_velocity
if self.dt:
throttle_cmd = self.pid_controller.step(velocity_error, self.dt)
self.dt = time.time() - self.t_start
self.t_start += self.dt
throttle = throttle_cmd
brake = 0.0
rospy.logdebug('throttle = %.2f, T = %.2f, B = %.2f, S = %.2f (BAND: %.2f)', throttle_cmd, throttle, brake, steer, self.brake_deadband)
return throttle, brake, steer
def reset(self):
self.pid_controller.reset()
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
mn = 0.
mx = 0.2
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = 0.02
self.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()
# TODO: Implement
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 1000
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
class Controller(object):
def __init__(self, accel_limit, brake_deadband, decel_limit, fuel_capacity,
max_lat_accel, max_steer_angle, steer_ratio, vehicle_mass,
wheel_base, wheel_radius):
#########################################################################
#### PID Controller parameters ####
Kp = 0
Ki = 0
Kd = 0
mn = 0
mx = 0
#### Initalizing the PID controller using Controller gains, minimum and maximum throttle value ####
self.PID_controller = PID(Kp, Ki, Kd, mn, mx)
#### Low Pass Filter parameters ###
tau = 0
ts = 0
#### Intialize the low pass filter using time constant (tau) and sample time ####
self.Low_pass_filter = LowPassFilter(tau, ts)
#### Initialize the yaw controller using the wheel_base, steer_ratio, min_speed, max_lat_accel and max_steer_angle ####
self.Yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
#### Vehicle parameters and contraints ####
self.accel_limit = accel_limit
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
#### Time Initialization ####
previous_time = rospy.get_time()
Last_throttle = 0
#########################################################################
pass
def control(self, linear_velocity, angular_velocity, current_velocity,
dbw_enabled):
#########################################################################
#### Resetting the PID integrator error when the driver controls the vehicle ####
if not dbw_enabled:
self.PID_controller.reset()
#### Filtering the noise in velcoity readings ####
current_velocity = self.Low_pass_filter.filt(current_velocity)
#### Throttle Control ####
## Error calculation ##
Velocity_error = linear_velocity - current_velocity
## Sample time calculation ##
current_time = rospy.get_time()
sample_time = current_time - previous_time
previous_time = current_time
## Throttle value from PID controller ##
Throttle = self.PID_controller.step(Velocity_error, sample_time)
if (Throttle - Last_throttle) > 0.05:
Throttle = Last_throttle + 0.05
Last_throttle = Throttle
#### Steering Control ####
Steering = self.Yaw_controller.get_steering(linear_velocity,
angular_velocity,
current_velocity)
#### Brake Control ####
## For normal case, brake torque applied is equal to 0 ##
Brake = 0
## For stopping the vehicle at low speed ##
if linear_velocity == 0 and current_velocity < 0.1:
Throttle = 0
Brake = MAX_BRAKE
## For releasing throttle when current velocity becomes greater than required velocity ##
elif Throttle > 0.1 and Velocity_error < 0:
Throttle -= 0.01
## For heavy braking when current velocity becomes greater than required velocity and throttle value is small ##
elif Throttle < 0.1 and Velocity_error < 0:
Throttle = 0
Deceleration = abs(max(Velocity_error, self.decel_limit))
Brake = min(MAX_BRAKE,
(Deceleration * vehicle_mass * wheel_radius))
#########################################################################
return Throttle, Brake, Steering
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
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
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# When dbw node is disabled (at traffic light) this will prevent us from accumulating error
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = MAX_BRAKE #N*m - to hold the car in place if we are stopped at a light. Acceleration - lm/s^2
elif throttle < 0.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
return throttle, brake, steering
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.001
kd = 10.
mn_acceleration = 0.
mx_acceleration = 0.2
self.throttle_controller = PID(kp, ki, kd, mn_acceleration,
mx_acceleration)
tau = 0.5
ts = .02
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
#return 1., 0., 0.
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn("angular vel: {0}".format(angular_vel))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
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. Acceleration - 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
return throttle, brake, steering
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
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
#Filtering out the high frequencey "Velocity" noise
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0. #reset controller in manual mode
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
# calculate sample_time for each step of our PID controller
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)
brake = 0
#We are going faster than target velocity, so slow down
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 400 #N*m to hold the car in place if we are stopped at light acceleration
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
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
## Initialize Yaw Controller Object ..
self.yaw_controller = YawController(kwargs['wheel_base'],
kwargs['steer_ratio'],
kwargs['min_speed'],
kwargs['max_lat_accel'],
kwargs['max_steer_angle'])
## Define PID, to be used later for Throttle computation
self.throttle_pid = PID(kp=0.3,
ki=0.0,
kd=0.0,
mn=kwargs['decel_limit'],
mx=kwargs['accel_limit'])
## Minimum Speed placeholder
self.min_speed = kwargs['min_speed']
self.steer_ratio = kwargs['steer_ratio']
## Placeholders for effective breaking ..
self.wheel_base = kwargs['wheel_base']
self.wheel_radius = kwargs['wheel_radius']
self.total_mass = kwargs['vehicle_mass'] + kwargs[
'fuel_capacity'] * GAS_DENSITY # And even air pressure would effect mass distrubution
self.brake_deadband = kwargs['brake_deadband']
## Time Logging Placeholder
self.prev_time = None
### Define low pass filter & apply if needed
self.s_lpf = LowPassFilter(tau=.2, ts=.1)
# control method: for any preprocessing of throttle, brake, yaw
def control(self, *args, **kwargs):
# Retrieve present throttle, brake, steer
target_velocity_linear_x = args[0]
target_velocity_angular_z = args[1]
current_velocity_linear_x = args[2]
current_velocity_angular_z = args[3]
dbw_enabled = args[4]
throttle = 0.0
brake = 0.0
steering = 0.0
## If DBW is interrupted, reset throttle to avoid errors & return ..
if not dbw_enabled:
self.throttle.reset()
self.steer_pid.reset()
return 0, 0, 0
# Compute difference between target and current velocity as CTE for throttle.
diff_velocity = target_velocity_linear_x - current_velocity_linear_x
current_time = rospy.get_time()
dt = 0
# Compute Delta Time from last compute..
if self.prev_time is not None:
dt = current_time - self.prev_time
self.prev_time = current_time
velocity_controller = 0
# Determine if throttle or braking is needed ..
if dt > 0:
velocity_controller = self.throttle_pid.step(diff_velocity, dt)
if velocity_controller > 0: # Throttle
throttle = velocity_controller
elif velocity_controller < 0: # Braking
brake = -velocity_controller
# Check for dead-band defined in config
if brake < self.brake_deadband:
brake = 0.0
# Apply other factors which impact braking intensity like vehcile mass, wheel radius, air pressure ..
brake = brake * self.total_mass * self.wheel_radius * 4
# Define yaw from yaw conrtoller, given target and present linear, angular velocities ..
steering = self.yaw_controller.get_steering(target_velocity_linear_x,
target_velocity_angular_z,
current_velocity_linear_x)
# Apply filter if needed
#steering = self.s_lpf.filt(steering)
return throttle, brake, steering
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, *args, **kwargs):
# TODO: Implement
self.pid = PID(100, 1, 1)
self.yawcontroller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
#kp=0.25
#ki=0.001
#FOPDT(first order plus dead-time model is used.
#Tuning is done using IMC(Internal Model Control)
kp = 0.5
ki = kp / 8.0
kd = 0.00001
mn = 0.
#mx=0.2
mx = 0.8 #well... 1 is possible but... it might not be for the car..
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = .02
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 450 # N*m
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
# current_vel is m/s. therefore 5m/s is around 20 km/h
vel_idx = int(current_vel)
if vel_idx < 5:
limit = throttle_for_slowstart[vel_idx]
throttle = limit if current_vel > limit else current_vel
# throttle=0.2
# brake = 0
return throttle, brake, steering
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
mn = 0.0 # minimum throttle value
mx = 0.2 # maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# filter out high frequency noise in velocity
tau = 0.5 # 1/(2*pi*tau) is the cutoff frequency
ts = 0.02 # sample time
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn("Current velocity is: {}".format(current_vel))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
vel_error = linear_vel - current_vel
print("Velocity error is " + str(vel_error))
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
print("Sample time is " + str(sample_time))
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0.
if linear_vel == 0. and current_vel < 0.1: # the car should stop
throttle = 0.
brake = 700. # Nm to hold the car in place if stopped at traffic light
elif throttle < 0.1 and vel_error < 0.: # the car should brake
throttle = 0.
decel = max(
vel_error, self.decel_limit
) # both are negative, so we take the smaller absolute value
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
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
# implemented based on "DBW Walkthrough"
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.0
mn = 0.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 in Hz
ts = 0.02 # Sample time in seconds
self.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.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.last_time = rospy.get_time()
pass
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# implemented based on "DBW Walkthrough"
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
filtered_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn("Angular vel: {0}".format(angular_vel))
# rospy.logwarn("Target velocity: {0}".format(linear_vel))
# rospy.logwarn("Target angular velocity: {0}\n".format(angular_vel))
# rospy.logwarn("Current velocity: {0}".format(current_vel))
# rospy.logwarn("Filtered velocity: {0}".format(filtered_vel))
steer = self.yaw_controller.get_steering(linear_vel, angular_vel,
filtered_vel)
# rospy.logwarn("steer: {0}".format(steer))
vel_error = linear_vel - filtered_vel
self.last_vel = filtered_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)
brake = 0
if linear_vel == 0.0 and filtered_vel < 0.1:
throttle = 0.0
brake = 800.0 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2
elif throttle < 0.1 and vel_error < 0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius # Torque N*m
return throttle, brake, steer
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
## PID parameters for throttle
Kp = 0.1
Ki = 0.02
Kd = 0.0
mn = 0.0 #min throttle
mx = 0.23 #max throttle
## PID parameters for braking
Kp_b = 60.0
Ki_b = 0.0
Kd_b = 10.0
mn_b = 0.0 #min brake
# setup two PIDs
self.throttle_controller = PID(Kp, Ki, Kd, mn, mx)
self.brake_controller = PID(Kp_b, Ki_b, Kd_b, mn_b, MAX_BRAKE)
tau = 0.5 #1/(2pi*tau) = cutoff freq
ts = 0.02 # sampling time, this should reflect refresh rate from dbw_node
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
self.brake_controller.reset()
return 0.0, 0.0, 0.0
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
#rospy.loginfo('test:%s\n', linear_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)
brake = 0
# when car is supposed to stop and current velocity is small, throw on max brake
if linear_vel < 0.4 and current_vel < 0.4:
throttle = 0
brake = MAX_BRAKE
# when car is going faster than target speed, let go off throttle and apply brake
elif throttle < 0.1 and vel_error < 0:
throttle = 0
brake = self.brake_controller.step(-vel_error, sample_time)
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
self.vehicle_mass = kwargs['vehicle_mass']
self.fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
self.accel_limit = kwargs['accel_limit']
self.wheel_radius = kwargs['wheel_radius']
self.wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
self.max_lat_accel = kwargs['max_lat_accel']
self.max_steer_angle = kwargs['max_steer_angle']
min_speed = 0
self.velocity_pid = PID(kp=0.8,
ki=0,
kd=0.05,
mn=self.decel_limit,
mx=0.5 * self.accel_limit)
self.steering_pid = PID(kp=0.16,
ki=0.001,
kd=0.1,
mn=-self.max_steer_angle,
mx=self.max_steer_angle)
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
min_speed, self.max_lat_accel,
self.max_steer_angle)
def reset(self):
self.velocity_pid.reset()
self.steering_pid.reset()
def control(self, target_linear_vel, target_angular_vel,
current_linear_vel, cross_track_error, sample_time):
# Difference between target velocity and current velocity.
vel_error = target_linear_vel - current_linear_vel
throttle = self.velocity_pid.step(vel_error, sample_time)
# N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2
brake = 0.0
# Stop
if target_linear_vel == 0.0 and current_linear_vel < 0.1:
throttle = 0.0
brake = MAX_BRAKE
# Decelerate
elif throttle < 0:
deceleration = abs(throttle)
if deceleration > self.brake_deadband:
total_weight = self.vehicle_mass + (self.fuel_capacity *
GAS_DENSITY)
brake = total_weight * self.wheel_radius * deceleration
throttle = 0
# without Steering PID car will wiggle around the waypoint path
predictive_steering = self.yaw_controller.get_steering(
target_linear_vel, target_angular_vel, current_linear_vel)
corrective_steering = self.steering_pid.step(cross_track_error,
sample_time)
steering = predictive_steering + corrective_steering
return throttle, brake, steering
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):
# Yaw controller (pre-provided)
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# Throttle PID controller
kp = 0.3
ki = 0.1
kd = 0.0
# rospy.loginfo("PID Kp :{0}".format(kp))
# rospy.loginfo("PID Ki :{0}".format(ki))
# rospy.loginfo("PID Kd :{0}".format(kd))
mn = 0.0 # Min throttle
mx = 0.2 # Max throttle
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Velocity low-pass filter - to filter noise
tau = 0.5 # Time constant
ts = 0.02 # Sampling time
# rospy.loginfo("Low pass filter Time constant :{0}".format(tau))
# rospy.loginfo("Low pass filter Sampling time :{0}".format(ts))
self.vel_lpf = LowPassFilter(tau, ts)
# Other parameters
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, dbw_enabled):
# Return throttle, brake, steer
# In case of drive-bt-wire NOT enabled, reset PID and return 0
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
# Otherwise, first filter velocity
current_vel = self.vel_lpf.filt(current_vel)
# Calculate steering
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
# Calculate throttle
vel_error = linear_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)
# Calculate brake
brake = 0.0
if linear_vel == 0.0 and current_vel < 0.1:
# Vehcile stopped
throttle = 0.0
brake = STOP_TORQUE # N*m - needed to keep the car in place
elif throttle < 0.1 and vel_error < 0.0:
# Stop throttle and calculate brake
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
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
self.yaw_control = YawController(wheel_base, steer_ratio, 0.1,
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_control = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2pi*tau) = cut-off frequency
ts = 0.02 # sample time
# Low pass filter for incoming, noisy velocity message
self.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, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_control.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
# Steering
steer = self.yaw_control.get_steering(linear_vel, angular_vel,
current_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
throttle = self.throttle_control.step(vel_error, sample_time)
# Brake
brake = 0
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 400 # N*m - To hold the car in a place if we are stopped at a light. Acceleration ~ 1 m/s^2
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = self.vehicle_mass * self.wheel_radius * abs(
decel) # Torque - N*m
return throttle, brake, steer
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
Kp = 1.0
Ki = 0.001
Kd = 0
mn = 0. # Minmum 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 = 0.02 # Sample time
self.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()
#pass
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
#rospy.loginfo("dbw_enabled is false !")
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 400 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2
elif throttle < 0.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
return throttle, brake, steering
class Controller(object):
def __init__(self,
vehicle_mass,
fuel_capacity,
brake_deadband,
decel_limit,
accel_limit,
wheel_base,
wheel_radius,
steer_ratio,
max_lat_accel,
max_steer_angle,
min_speed=0.0):
# PID controller for throttle and braking
self.tb_pid = PID(kp=0.4, ki=0.01, kd=0.01, mn=-1.0, mx=accel_limit)
# Low pass filter to smooth out throttle/braking actuations.
self.tb_lpf = LowPassFilter(0.1)
# Constants that are relevant to computing the final throttle/brake value
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
# Compute the braking conversion constant according to the formula `braking_torque = total_mass * max_deceleration * wheel_radius`.
# `self.tb_pid` outputs barking values in `[-1,0]`, which will then be scaled by the constant below.
self.braking_constant = (self.vehicle_mass +
self.fuel_capacity * GAS_DENSITY
) * self.decel_limit * self.wheel_radius
# Record the time of the last control computation.
self.last_time = None
# Controller for steering angle
self.yaw_controller = YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=min_speed,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
def control(self, target_linear_velocity, target_angular_velocity,
current_linear_velocity, dbw_enabled):
if dbw_enabled:
if self.last_time is None:
sample_time = 0.02
self.last_time = time.time()
else:
this_time = time.time()
sample_time = this_time - self.last_time
self.last_time = this_time
# Compute the raw throttle/braking value.
tb = self.tb_pid.step(error=(current_linear_velocity -
target_linear_velocity),
sample_time=sample_time)
# Smoothen it.
tb = self.tb_lpf.filt(tb)
# Scale it.
if tb < -self.brake_deadband: # We're braking.
# Convert the raw braking value to torque in Nm (Newton-meters).
brake = self.braking_constant * tb * TEST_BRAKING_CONST
throttle = 0
elif tb < 0: # We want to slow down, but don't need to brake actively in order to do so.
brake = 0
throttle = 0
else: # We're accelerating.
throttle = tb
brake = 0
# Compute the steering value.
steer = self.yaw_controller.get_steering(
linear_velocity=target_linear_velocity,
angular_velocity=target_angular_velocity,
current_velocity=current_linear_velocity)
rospy.loginfo("tb PID: %s, brake: %s, steer: %s, tv: %s, cv: %s",
tb, brake, steer, target_linear_velocity,
current_linear_velocity)
return throttle, brake, steer
else: # If `dbw_enabled` is False.
self.tb_pid.reset()
return 0, 0, 0
class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
self.yaw_controller = YawController(kwargs['wheel_base'], kwargs['steer_ratio'], MIN_SPEED, kwargs['max_lat_accel'], kwargs['max_steer_angle'])
# Create PID to control throttle
mn=0.0 # minimum throttle value
mx=0.8 # maximum throttle value
self.throttle_controller = PID(kp=0.8, ki=0.001, kd=0.2, mn=mn, mx=mx) #ki=0.15, kd=0.0001
# Create PID to control steering angle
mn=-kwargs['max_steer_angle'] # minimum steering angle
mx=kwargs['max_steer_angle'] # maximum steering angle
self.steer_controller = PID(kp=1.4, ki=0.000001, kd=0.2, mn=mn, mx=mx)
# Set low pass filter parameters: sample_time=1/50hz, cutoff_frecuency=tau
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 # Sample time = 1 / 50hz
self.vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = kwargs['vehicle_mass']
self.fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
self.accel_limit = kwargs['accel_limit']
self.wheel_radius = kwargs['wheel_radius']
# Add the fuel mass to the vehicle mass
self.vehicle_mass = self.vehicle_mass + self.fuel_capacity*GAS_DENSITY
self.last_time = rospy.get_time()
def control(self, current_vel, dbw_enabled, target_linear_vel, target_angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
# Filter current speed of the car
#not_filtered_vel = current_vel
current_vel = self.vel_lpf.filt(current_vel)
#rospy.loginfo("dbw_node: LowPassFilter (not_filtered_speed={0}, filtered_speed={1})".format(not_filtered_vel, current_vel))
current_time = rospy.get_time()
dt = current_time - self.last_time
self.last_time = current_time
# Calculate steering angle using yaw controller and pid controller
steering = self.yaw_controller.get_steering(target_linear_vel, target_angular_vel, current_vel)
steering = self.steer_controller.step(steering, dt)
# Calculate throttle using pid controller
vel_error = target_linear_vel - current_vel
self.last_vel = current_vel
throttle = self.throttle_controller.step(vel_error, dt)
brake = 0
if target_linear_vel == 0.0 and current_vel < 0.1: # Stop the car an activate brake
throttle = 0
brake = 700 #N*m - to hold the car in place if we are stopped at light. Acceleration ~ 1m/s^2
elif throttle < 0.1 and vel_error < 0: # Decrease speed of the car
throttle = 0
decel = max(vel_error/dt, self.decel_limit) #tranform speed in acceleration
brake = abs(decel)*self.vehicle_mass*self.wheel_radius #Torque N*m
return throttle, brake, steering
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.rate = 50 # Hz
self.sample_time = 1.0 / self.rate
self.dbw_enabled = True
self.initialized = False
self.current_linear_velocity = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = None
self.current_pose = None
self.logger = DBWLogger(self, rate=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
yaw_controller = YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=0.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.controller = TwistController(yaw_controller, max_steer_angle,
self.sample_time)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
self.loop()
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.001
mn = 0.0 # Minimum throttle value
mx = 0.4 # Maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 # Sample time
self.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()
self.log_time = rospy.get_time()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0.0
if linear_vel == 0.0 and current_vel < 0.1:
throttle = 0.0
brake = MAX_BRAKE # N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2
elif throttle < 0.1 and vel_error < 0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = min(MAX_BRAKE, (abs(decel) * self.vehicle_mass *
self.wheel_radius)) # Torque N*m
if (current_time - self.log_time) > LOGGING_THROTTLE_FACTOR:
self.log_time = current_time
rospy.logwarn(
"POSE: current_vel={:.2f}, linear_vel={:.2f}, vel_error={:.2f}"
.format(current_vel, linear_vel, vel_error))
rospy.logwarn(
"POSE: throttle={:.2f}, brake={:.2f}, steering={:.2f}".format(
throttle, brake, steering))
return throttle, brake, steering
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# Determining parameters experimentally
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 = 0.02
self.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 # comfort parameter
self.accel_limit = accel_limit # comfort parameter
self.wheel_radius = wheel_radius
self.last_time = self.print_time = rospy.get_time()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
# Filter the high frequency velocities off
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
# Calculate the error for the PID controller
vel_err = 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_err, sample_time)
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
elif throttle < 0.1 and vel_err < 0:
throttle = 0
decel = max(vel_err, self.decel_limit)
brake = abs(
decel
) * self.vehicle_mass * self.wheel_radius # Torque per meter
return throttle, brake, steering
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,
max_throttle):
# Yaw controller to controller the turning
self.vehicle_min_velocity = .1
self.yaw_controller = YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=self.vehicle_min_velocity,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.steering_controller = PID(
kp=.0, ki=.001, kd=.1, min=-.2, max=.2)
# Throttle controller
self.throttle_controller = PID(
kp=.8, ki=.001, kd=.1, min=0, max=max_throttle)
# Remove high frequency noise on the velocity
self.low_pass = LowPassFilter(tau=.5, ts=.02)
# Assume the tank is full
fuel_mass = fuel_capacity * GAS_DENSITY
self.total_mass = vehicle_mass + fuel_mass
self.decel_limit = decel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
def control(
self,
current_velocity,
linear_velocity,
angular_velocity,
cte):
current_velocity = self.low_pass.filter(current_velocity)
# Calculate the velocity error
dv = linear_velocity - current_velocity
# Update time
current_time = rospy.get_time()
dt = current_time - self.last_time
self.last_time = current_time
# Calculate the predictive path and correct for the cross track error
steering = self.yaw_controller.get_steering(
linear_velocity,
angular_velocity,
current_velocity)
steering -= self.steering_controller.step(cte, dt)
# Calculate the throttle and brake
throttle = self.throttle_controller.step(dv, dt)
brake = 0.
if linear_velocity < 0. and current_velocity < self.vehicle_min_velocity:
# Keep car in place when it is stopped
throttle = 0.
brake = 700.
elif throttle < .1 and dv < 0.:
throttle = 0.
decel = max(dv, self.decel_limit)
brake = abs(decel) * self.total_mass * self.wheel_radius
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
self.vehicle_mass = kwargs['vehicle_mass']
self.fuel_capacity = kwargs['fuel_capacity']
self.brake_deadband = kwargs['brake_deadband']
self.decel_limit = kwargs['decel_limit']
self.accel_limit = kwargs['accel_limit']
self.wheel_radius = kwargs['wheel_radius']
self.wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
self.max_lat_accel = kwargs['max_lat_accel']
self.max_steer_angle = kwargs['max_steer_angle']
self.min_speed = 0.0
self.yaw_contoller = YawController(self.wheel_base,
self.steer_ratio,
self.min_speed,
self.max_lat_accel,
self.max_steer_angle)
kp = 0.1
ki = 0.007
kd = 0.0
minth = 0.0
maxth = 0.2
self.throttle_contoller = PID(kp, ki, kd, minth, maxth)
tau = 150
ts = 75
self.lpf_velocity = LowPassFilter(tau, ts)
# self.lpf_steering = LowPassFilter(tau, ts)
self.t0 = rospy.get_time()
def control(self, proposed_twist, current_twist, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_contoller.reset()
return 0.0, 0.0, 0.0
proposed_linear_velocity = proposed_twist.twist.linear.x
proposed_angular_velocity = proposed_twist.twist.angular.z
current_linear_velocity = current_twist.twist.linear.x
current_angular_velocity = current_twist.twist.angular.z
current_linear_velocity = self.lpf_velocity.filt(current_linear_velocity) # Filter current linear velocity
err_linear_velocity = proposed_linear_velocity - current_linear_velocity
t1 = rospy.get_time()
sample_time = t1 - self.t0
self.t0 = t1
throttle = self.throttle_contoller.step(err_linear_velocity, sample_time)
brake = 0.0
if proposed_linear_velocity == 0.0 and current_linear_velocity < 0.1:
throttle = 0.0
brake = 400 # from walkthrough
elif throttle < 0.1 and err_linear_velocity < 0:
throttle = 0.0
decel = max(err_linear_velocity, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
steering = self.yaw_contoller.get_steering(proposed_linear_velocity,
proposed_angular_velocity,
current_linear_velocity)
return throttle, brake, steering
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
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 = 0.02 # Sample time
self.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, current_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
rospy.loginfo("current v is : %f", current_vel)
current_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn("Angular vel: {0}".format(angular_vel))
# rospy.logwarn("Target vel: {0}".format(linear_vel))
# rospy.logwarn("Target angular vel: {0}".format(angular_vel))
# rospy.logwarn("Current vel: {0}".format(current_vel))
# rospy.logwarn("Filtered vel: {0}".format(self.vel_lpf.get()))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_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)
brake = 0
if linear_vel == 0 and current_vel == 0.1:
throttle = 0
brake = 700 # N*m to hold the car in place when at a light
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 # N*m
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle,
decel_limit, vehicle_mass):
self.yaw_controller = YawController(
wheel_base,
steer_ratio,
0.1, # min_speed
max_lat_accel,
max_steer_angle,
)
# PID coefficients
kp = 0.3
ki = 0.1
kd = 0.0
# For convenience (no real limits on throttle):
mn_th = 0.0 # Minimal throttle
mx_th = 0.2 # Maximal throttle
self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th)
tau = 0.5 # 1 / (2pi*tau) = cutoff frequency
ts = 0.02 # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.wheel_radius = wheel_radius
self.decel_limit = decel_limit
self.vehicle_mass = vehicle_mass
self.last_time = rospy.get_time()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
filtered_vel = self.vel_lpf.filt(current_vel)
# rospy.logwarn(
# '\nAngular vel: {}\n'
# 'Target velocity: {}\n'
# 'Current velocity: {}\n'
# 'Filtered velocity: {}\n'
# .format(angular_vel, linear_vel, current_vel, filtered_vel)
# )
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel)
vel_error = linear_vel - filtered_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)
brake = 0
if linear_vel == 0 and filtered_vel < 0.1:
throttle = 0
brake = 400 # [N*m] -- to hold the car in place if we are stopped
# at a light. Acceleration ~ 1 m/s/s
elif throttle < 0.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]
return throttle, brake, steering
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
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.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.last_time = None
# setup the yaw controller for steering
self.yaw_controller = YawController(wheel_base = wheel_base, steer_ratio = steer_ratio, min_speed = ONE_MPH,
max_lat_accel = max_lat_accel, max_steer_angle = max_steer_angle)
# setup the PID controller parameters for the accelerator/brake
self.accel_pid = PID(kp=1., ki=0., kd=0., mn=decel_limit, mx=accel_limit)
# setup the low pass filter for steering
self.steer_lpf = LowPassFilter(tau = 3., ts = 1.)
# setup the low pass filter for acceleration
self.accel_lpf = LowPassFilter(tau = 3., ts = 1.)
#pass
def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# only proceed if dbw is enabled; else error may accumulate in control loops
if not dbw_enabled:
return 0., 0., 0.
# don't send commands if this is first time through loop
if self.last_time is None:
self.last_time = rospy.get_time()
return 0.0, 0.0, 0.0
# calculate delta time for the PID controller
delta_time = rospy.get_time() - self.last_time
rospy.loginfo("proposed_linear_velocity %f, current_linear_velocity %f", proposed_linear_velocity, current_linear_velocity)
rospy.loginfo("proposed_angular_velocity %f", proposed_angular_velocity)
velocity_error = proposed_linear_velocity - current_linear_velocity
steering = self.yaw_controller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity)
steer_cmd = self.steer_lpf.filt(steering)
acceleration = self.accel_pid.step(velocity_error, delta_time)
acceleration = self.accel_lpf.filt(acceleration)
if acceleration > 0.0:
throttle_cmd = acceleration
brake_cmd = 0.0
else:
throttle_cmd = 0.0
brake_cmd = abs(acceleration)
# only brake if commanded braking higher than deadband
if brake_cmd < self.brake_deadband:
brake_cmd = 0.0
# translate percentage of braking to braking force desired
brake_cmd = brake_cmd * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius
#return 1., 0., 0.
return throttle_cmd, brake_cmd, steer_cmd
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.0
mn = 0.0
mx = 0.9
self.throttle_controler = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = 0.02
self.vel_lpf = LPF(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.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.last_time = rospy.get_time()
def control(self, curr_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controler.reset()
return 0.0, 0.0, 0.0
curr_vel = self.vel_lpf.filt(curr_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
curr_vel)
vel_error = linear_vel - curr_vel
self.last_vel = curr_vel
curr_time = rospy.get_time()
sample_time = curr_time - self.last_time
self.last_time = curr_time
throttle = self.throttle_controler.step(vel_error, sample_time)
brake = 0.0
if linear_vel < 0.05 and curr_vel < 0.05:
throttle = 0.0
brake = 700.0
elif throttle < 0.1 and vel_error < 0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
#decel = self.decel_limit
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
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
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.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.throttle_controller = PID(1.5, 0.001, 0.0, 0, 1)
self.steer_controller = YawController(self.wheel_base,
self.steer_ratio, 0.001,
self.max_lat_accel,
self.max_steer_angle)
self.previous_time = 0
self.lpfilter = LowPassFilter(0.2, 0.1)
self.sim_scale_factor = 15 # set to 1 for Carla???
def control(self, cmd_linear_x, cmd_angular_z, current_linear_x,
current_angular_z, reset_dt):
if reset_dt:
self.previous_time = rospy.get_time()
throttle = 0.0
steer = 0.0
brake = 0.0
else:
delta_t = rospy.get_time() - self.previous_time
speed_error = cmd_linear_x - current_linear_x
steer = self.steer_controller.get_steering(cmd_linear_x,
cmd_angular_z,
current_linear_x)
steer = self.lpfilter.filt(steer)
if speed_error >= 0.0:
brake = 0.0
speed_error = self.sim_scale_factor * min(
speed_error, self.accel_limit * delta_t)
throttle = self.throttle_controller.step(speed_error, delta_t)
else:
throttle = 0.0
decel = max(self.decel_limit, speed_error / delta_t)
if abs(decel) < self.brake_deadband:
brake = 0.0
else:
mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY
brake = abs(decel) * mass * self.wheel_radius
rospy.loginfo('speed_error is %f', speed_error)
rospy.loginfo('pid result is %f', throttle)
self.previous_time = rospy.get_time()
return throttle, brake, steer
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.pid = PID(-0.9, -0.001, -0.07, decel_limit, accel_limit)
self.lowpass_filter = LowPassFilter(
1.0, 2.0) # the less the value, the more smooth
#self.yaw_lowpass = LowPassFilter(1.0, 1.0)
self.yaw_controller = YawController(wheel_base, steer_ratio, 0,
max_lat_accel, max_steer_angle)
self.brake_factor = vehicle_mass * wheel_radius
self.brake_deadband = brake_deadband
# TODO: Subscribe to all the topics you need to
# init the vars
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False # For debugging, use true
self.last_timestamp = rospy.get_time()
# subscribe
rospy.Subscriber("/current_velocity", TwistStamped,
self.current_velocity_cb)
rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cmd_cb)
rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_enabled_cb)
rospy.loginfo("### Final submission with dbw")
rospy.loginfo("### Controller initialized Yeah 3x Speed")
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# throttle, brake, steering = self.controller.control(<proposed linear velocity>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
# init the vars
ref_v = 0.
cur_v = 0.
err_v = 0.
ref_yaw = 0.
dt = 0.01
thro_cmd = 0.
brake_cmd = 0.
steer_cmd = 0.
if self.twist_cmd and self.current_velocity:
ref_v = self.twist_cmd.twist.linear.x
# play sth!
rospy.loginfo("RefV B4: %f" % ref_v)
#ref_v = ref_v * 3
rospy.loginfo("RefV Afta: %f" % ref_v)
ref_yaw = self.twist_cmd.twist.angular.z
cur_v = self.current_velocity.twist.linear.x
err_v = cur_v - ref_v
dt = rospy.get_time() - self.last_timestamp
self.last_timestamp = rospy.get_time()
rospy.loginfo("CurV: %f, RefV: %f" % (cur_v, ref_v))
rospy.loginfo("Change the errors: " + str(err_v))
if self.dbw_enabled:
acc = self.pid.step(err_v, dt) # get the acc
if acc >= 0:
thro_cmd = self.lowpass_filter.filt(acc)
brake_cmd = 0.
else:
thro_cmd = 0.
brake_cmd = self.brake_factor * (
-acc) + self.brake_deadband
steer_cmd = self.yaw_controller.get_steering(
ref_v, ref_yaw, cur_v)
#steer_cmd = self.yaw_lowpass.filt(steer_cmd)
self.publish(thro_cmd, brake_cmd, steer_cmd)
rospy.loginfo("Publish:" + str(thro_cmd) + " " +
str(steer_cmd))
rospy.loginfo("Time used: %.3f" % dt)
else:
self.pid.reset()
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
rospy.loginfo("Get twist cmd:" + str(self.twist_cmd.twist))
def current_velocity_cb(self, msg):
self.current_velocity = msg
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
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
'''
yaw controller initialize
'''
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
self.last_time = rospy.get_time()
'''
throttle control initialize
'''
kp = 0.3
ki = 0.1
kd = 0.0
mn = 0.0 # Min throttle value
mx = 0.4 # Max throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
'''
Low pass filter initialize
To address velocity msg noise
'''
tau = 0.5
ts = 0.02
self.vel_lpf = LowPassFilter(tau, ts)
'''
store vehicle parameters
'''
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
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
'''
reset controller while dbw is disable, in case integral component accumulating error
'''
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
'''
drive by wire enable
'''
# filtering current velocity
current_vel = self.vel_lpf.filt(current_vel)
# steering control
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)
# throttle control
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)
# brake control
brake = 0
if linear_vel == 0.0 and current_vel < 0.1:
throttle = 0.0
brake = 700 #Nm
elif throttle <0.1 and vel_error < 0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
class Controller(object):
#def __init__(self, *args, **kwargs):
def __init__(self, vehicle_mass, fuel_capacity, brake_demand, decel_limit, accel_limit, wheel_radius,
wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_demand = brake_demand
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
kp = 0.25
ki = 0.0
kd = 0.15
mn = 0. # minimum throttle value
mx = 0.5 # maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = 0.02 # sample time
self.vel_lpf = LowPassFilter(tau, ts)
# -----------------------------------------------------------------------------------------------------
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_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)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 400 # Nm - to hold car in place if stopped at light Acceleration - 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
return throttle, brake, steering
class Controller(object):
def __init__(self, **ros_param):
"""
:param ros_param:
Note:
sample time (sec) is based on the dbw node frequency 50Hz
low_pass filter:
val = w * cur_val + (1 - w) * prev_val
w is 0 ~ 1
"""
# used ros_param
self.vehicle_mass = ros_param['vehicle_mass']
# self.fuel_capacity = ros_param['fuel_capacity']
self.brake_deadband = ros_param['brake_deadband']
self.decel_limit = ros_param['decel_limit']
self.accel_limit = ros_param['accel_limit']
self.wheel_radius = ros_param['wheel_radius']
self.last_time = rospy.get_time()
# low pass filter for velocity
self.vel_lpf = LowPassFilter(0.5, .02)
# Init yaw controller
min_speed = 0.1 # I think min_speed
self.steer_controller = YawController(min_speed, **ros_param)
self.throttle_lpf = LowPassFilter(0.05, 0.02) # w = 0.28
# Init throttle PID controller
# TODO: tweaking
kp = 0.5
ki = 0.005
kd = 0.1
acc_min = 0.
acc_max = 0.5 #self.accel_limit
self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max)
def control(self, target_linear_velocity, target_angular_velocity,
cur_linear_velocity, dbw_status):
# Check input info is ready
if not dbw_status:
self.throttle_controller.reset()
return 0., 0., 0.
# dbw enabled: control!
cur_linear_velocity = self.vel_lpf.filt(cur_linear_velocity)
# get steer value
steering = self.steer_controller.get_steering(target_linear_velocity,
target_angular_velocity,
cur_linear_velocity)
# get throttle (could be < 0 so it will be updated by `get brake` as well)
vel_err = target_linear_velocity - cur_linear_velocity
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_err, sample_time)
# get brake
brake = 0
if target_linear_velocity == 0. and cur_linear_velocity < 0.1:
throttle = 0
brake = 400 # N * m - hold the car in place for the red traffic light
elif throttle < .1 and vel_err < 0.:
throttle = 0.
decel_velocity = max(vel_err, self.decel_limit) # attention this value is < 0
# if less than brake_deaband, we don't need to add brake
# The car will deceleration by friction just release peddle
if abs(decel_velocity) > self.brake_deadband:
brake = abs(decel_velocity) * self.vehicle_mass * self.wheel_radius
else:
brake = 0
return throttle, brake, steering
def reset(self):
self.throttle_controller.reset()
