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
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):
# 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)
# You may add dampening term to steering
# Take current angular velocity and target angular velocity
# Look at the difference between the two, if they are the same do not change the steering.
# If they are very different add some dampening term to the steering
# Low pass filter of steering could also be a solution
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 stpped at a light. Acceleration ~ 1m/s2
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):
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.is_simulation = not self.config["is_site"]
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
if self.is_simulation:
kp = 0.3 #0.3
ki = 0.1 #0.1
kd = 0.0
mn = 0.0 # Minimum throttle value
mx = 0.4 # Maximum throttle value, increase this if desired
else:
kp = 0.15
ki = 0.1
kd = 0.0
mn = 0.0 # Minimum throttle value
mx = 0.15
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.stopped_brake_value = 0.0 #N*m
self.last_throttle_value = 0.0
self.throttle_aim = 0.0
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
current_vel = self.vel_lpf.filt(current_vel)
#rospy.logwarn("Angular velocity: {0}".format(angular_vel))
#rospy.logwarn("Target linear 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(self.vel_lpf.get()))
# **** Possibly add dampening terms below to reduce steering jerk when vehicle is wandering
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)
#rospy.logwarn("Steering: {0}".format(steering))
vel_error = linear_vel - current_vel
#self.last_vel = current_vel
brake = 0.0
throttle = 0.0
#if self.is_simulation:
# Original PID Control Function - Run this for simulator
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)
# else:
# # New PID control function - Run this on Carla?
# if self.last_throttle_value >= self.throttle_aim:
# current_time = rospy.get_time()
# sample_time = current_time - self.last_time
# self.last_time = current_time
# # Run PID Controller
# self.throttle_aim = self.throttle_controller.step(vel_error, sample_time)
# if self.last_throttle_value < self.throttle_aim:
# # Proportionally increase throttle to meet aim
# throttle += self.last_throttle_value + (self.throttle_aim * 0.01) # Increases throttle at 1% per update
# # Clamp throttle to aim value
# throttle = min(throttle, self.throttle_aim)
# else:
# throttle = self.throttle_aim
if linear_vel == 0.0 and current_vel < 1.0:
throttle = 0.0
self.throttle_aim = 0.0
# Changed from 400 Nm to 700 Nm per Udacity's specification of how much torque it would take
# to keep the vehicle stopped in gear with an automatic transmission
# Ramp this value in to prevent jerk/shudder at stop
self.stopped_brake_value += HOLD_STOP_BRAKE_TORQUE * 0.05
brake = min(HOLD_STOP_BRAKE_TORQUE, self.stopped_brake_value) #N*m - to hold the car in place if we are stopped at a light. Acceleration ~ lm/s^2
#brake = self.stopped_brake_value #N*m - to hold the car in place if we are stopped at a light. Acceleration ~ lm/s^2
elif throttle < 0.01 and vel_error < 0.0:
throttle = 0.0
self.throttle_aim = 0.0
decel = max(vel_error, self.decel_limit)
#rospy.logwarn("Vel error: {0}, Decel value {1}".format(vel_error, decel))
brake = abs(decel)*(self.vehicle_mass+self.fuel_capacity*GAS_DENSITY)*self.wheel_radius # Torque N*m
else:
# Reset stopping brake value
self.stopped_brake_value = 0.0
self.last_throttle_value = throttle
return throttle, brake, steering
class TwistController(object):
# def __init__(self, *args, **kwargs):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, decel_limit, accel_limit):
# TODO: Implement
# Use same order of magnitude K values as PID project, these
# are complete guesses for now.
# Order of params is Kp, Ki, Kd
self.accel_pid = PID(2, .01, .1)
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.accel_limit = accel_limit
self.decel_limit = decel_limit
self.lowpass = LowPassFilter(1.0, 1.0)
self.last_time = None
pass
def control(self, linear_v, angular_v, current_v, dbw_enabled):
# def control(self, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.accel_pid.reset()
return 0.0, 0.0, 0.0
if self.last_time is None:
self.last_time = rospy.get_time()
return 0.0, 0.0, 0.0
time = rospy.get_time()
error = linear_v - current_v
v_out = self.accel_pid.step(error, time - self.last_time)
self.last_time = time
steering = self.yaw_controller.get_steering(linear_v, angular_v,
current_v)
# steering = self.lowpass.filt(steering)
if v_out >= 0.0: # Accelerate
accel_out = v_out
brake_out = 0.0
elif v_out > -0.05: # Coast, should put through low pass filter
accel_out = 0.0
brake_out = 0.0
else: # Brake
accel_out = 0.0
#brake_out = min(v_out * 8, 5)
#brake_out = min(- v_out * 8, -5)
#brake_out = min(max(- error * 5, 1), 10)
#brake_out = min(max(- error * 8, 5), 30)
#brake_out = min(max(- error * 8, 5), 30)
brake_out = max(
-50.0 * error,
10.0) # what is the limit of brake value? 0-1 0-30? 0-100?
rospy.logwarn("PID error: {:08.4f}".format(error) +
" v_out {:08.4f}".format(v_out) +
" current_v {:08.4f}".format(current_v) +
" linear_v {:08.4f}".format(linear_v) +
" brake raw {:08.4f}".format(-error * 5) +
" brake {:08.4f}".format(brake_out))
return accel_out, brake_out, 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
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 = 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):
# TODO: Change the arg, kwarg list to suit your needs
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))
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 velocity 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.0 and current_vel < 0.1:
throttle = 0
# N*m - to hold car in place if we are stopped at a light
# Acceleration ~ 1m/s^2
brake = 700
elif throttle < 0.1 and vel_error < 0: # going too fast
throttle = 0
decel = max(vel_error, self.decel_limit)
# Torque N * m
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
# Return throttle, brake, steer
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, 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, 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: 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, 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, *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
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, 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
# Initialization of vehicle properties
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.max_steer_angle = max_steer_angle
# Yaw controller
min_speed = 0.1
self.yaw_controller = YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
# Speed throttle controller
kp = 0.8 # Proportional term
ki = 0.1 # Integral term
kd = 0.1 # Differential term
mn = 0.0 # Min throttle value
mx = 0.8 # Max throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Define low-pass filter settings
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 # Sample time
# Filtering out all high frequency noise in the velocity
self.vel_lpf = LowPassFilter(tau,ts)
# Get current time stamp during initialization
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
# During manual operation, reset throttle controller to avoid false growth of integral term
# Return zero for all controller inputs
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 vel: {0}\n".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.
if ((linear_vel < 0.01) and (current_vel < 0.1)): # Changed linear_vel == 0. to < 0.01
throttle = 0.
brake = 700 # Nm
elif ((throttle < 0.1) and (vel_error < 0.0)):
throttle = 0.
decel = max(vel_error, self.decel_limit)
brake = abs(decel)*self.vehicle_mass*self.wheel_radius # Torque Nm
steering = self.stabilize_steering(steering, vel_error)
#rospy.logwarn("Throttle: {0}".format(throttle))
#rospy.logwarn("Brake: {0}".format(brake))
#rospy.logwarn("Steering: {0}".format(steering))
#rospy.logwarn("Velocity error: {0}".format(vel_error))
return throttle, brake, steering
def stabilize_steering(self, steering, vel_error):
"""
Stabilize the steering angle
"""
if vel_error <= abs(0.001):
if steering > 0:
return steering - abs(vel_error)
else:
return steering + abs(vel_error)
elif vel_error >= abs(1.0) and steering == 0.0:
return steering
else:
if steering > 0:
return steering + abs(vel_error)
else:
return steering - abs(vel_error)
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_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
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, 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):
# hyperparameters for throttle PID controller
kp = 0.3
ki = 0.1
kd = 0.
mn = 0. # minimum throttle value
mx = 0.2 # maximum throttle value
self.throttle_pid = PID(kp, ki, kd, mn, mx)
# hyperparameters for velocity low pass filter
tau = .5 # cutoff frequency (1 / 2 * pi * tau))
ts = .02 # Sample time
self.velocity_lowpass = LowPassFilter(tau, ts)
# steerin controller based on the givven YawController class
self.steering_controller = YawController(
wheel_base,
steer_ratio,
0.1, # TODO: what is a good value for min speed?
max_lat_accel,
max_steer_angle)
# member to holt last call timestamp
self.last_timestamp = rospy.get_time()
# vehicle parameters for calculation
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
def control(self, target_lin_vel, target_ang_vel, current_lin_vel,
current_ang_vel, dbw_enabled):
# TODO: pid to control steering, throttle and maybe brake
# Return throttle, brake, steer
if dbw_enabled and (target_lin_vel
is not None) and (target_ang_vel is not None) and (
current_lin_vel
is not None) and (current_ang_vel is not None):
# init control values before calculation
throttle = 0.
brake = 0.
steering = 0.
# lowpassfilter on velocity to reduce noise
filtered_velocity = self.velocity_lowpass.filt(current_lin_vel)
# calculate sample time to use in PID controller
sample_time = rospy.get_time() - self.last_timestamp
# calculate CTE for PID controller
cte = target_lin_vel - filtered_velocity
# get throttle from PID controlelr
throttle = self.throttle_pid.step(cte, sample_time)
# save current timestamp for next round
last_timestamp = rospy.get_time()
# calculate brake torque
if (target_lin_vel < .01) and (filtered_velocity < .1):
# car is standing still, avoid throttle and brake with enough force to hold car
throttle = 0.
brake = 700. # Nm according to Udacity project information
if (throttle < .1) and ((target_lin_vel - filtered_velocity) < 0):
# we are driving to fast and need to brake
throttle = 0.
# calculate deceleration with respect to the limit parameter for comfort
# first use a linear decrease proportional to the current error
decel = min(self.decel_limit, abs(cte / sample_time))
# calculate the brake torque through decelleration, mass and wheel physics
# TODO: maybe use better mass calculation?
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
# if brake force is below a weak value, cancel braking
if brake < .1:
brake = 0.
steering = self.steering_controller.get_steering(
target_lin_vel, target_ang_vel, filtered_velocity)
else:
# ensure to reset controller to avoid accumulating error
self.throttle_pid.reset()
throttle = 0.
brake = 0.
steering = 0.
return throttle, brake, steering
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):
## *kwargs definition (see dwb_node.py)
##
## controller_args = {'wheel_base': wheel_base,
## 'steer_ratio': steer_ratio,
## 'min_speed': min_speed,
## 'max_lat_accel' : max_lat_accel,
## 'max_steer_angle' : max_steer_angle,
## 'decel_limit': decel_limit,
## 'accel_limit' : accel_limit.
## 'vehicle_mass' : vehicle_mass,
## 'wheel_radius' : wheel_radius,
## 'brake_deadband' : brake_deadband
## }
def __init__(self, *args, **kwargs):
wheel_base = kwargs.get('wheel_base')
self.steer_ratio = kwargs.get('steer_ratio')
self.min_speed = kwargs.get('min_speed')
self.max_speed = rospy.get_param('/waypoint_loader/velocity') / 3.6
max_lat_accel = kwargs.get('max_lat_accel')
self.max_steer_angle = kwargs.get('max_steer_angle')
self.decel_limit = kwargs.get('decel_limit')
self.accel_limit = kwargs.get('accel_limit')
self.vehicle_mass = kwargs.get('vehicle_mass')
self.wheel_radius = kwargs.get('wheel_radius')
self.brake_deadband = kwargs.get('brake_deadband')
self.yawcontroller = YawController(wheel_base, self.steer_ratio, self.min_speed,
max_lat_accel, self.max_steer_angle)
self.throttle_pid = pid.PID(kp=T_kp, ki=T_ki, kd=T_kd, mn=self.decel_limit, mx=self.accel_limit)
self.steer_pid_high = pid.PID(kp=S_kp_high, ki=S_ki_high, kd=S_kd_high, mn=-self.max_steer_angle, mx=self.max_steer_angle)
self.steer_pid_high_cte = pid.PID(kp=S_kp_high, ki=S_ki_high, kd=S_kd_high, mn=-self.max_steer_angle, mx=self.max_steer_angle)
self.last_time = rospy.rostime.get_time()
## *kwargs definition (see dwb_nobe.py)
## control_args = {'trgtv' : self.trgtv, # target linear velocity
## 'currv' : self.currv, # current linear velocity
## 'trgtav' : self.trgtav, # target angular velocity
## 'currav' : self.currav, # current angular velocity
## 'dbw_enabled' : self.dbw_enabled, # dbw status
## 'current_angle' : self.current_angle,
## 'elapsed' : elapsed
## }
def control(self, *args, **kwargs):
trgtv = kwargs.get('trgtv')
currv = kwargs.get('currv')
trgtav = kwargs.get('trgtav')
currav = kwargs.get('currav')
dbw_enabled = kwargs.get('dbw_enabled')
elapsed = kwargs.get('elapsed')
current_angle = kwargs.get('current_angle')
cte = kwargs.get('cte')
# used PID for throttle
# used yawcontroller to get the steering angle
# used two PIDs for steering (yaw and CTE error)
# brake value set to vehicle mass times throttle times wheel radius
if trgtv > self.max_speed: # limit speed
trgtv = self.max_speed
if dbw_enabled:
throttle = self.throttle_pid.step(trgtv - currv, elapsed)
if (throttle > self.accel_limit):
throttle = self.accel_limit
brake = 0.
target_angle = self.yawcontroller.get_steering(trgtv, trgtav, currv)
angle_high_speed = self.steer_pid_high.step(target_angle - current_angle, elapsed)
angle_high_speed_cte = self.steer_pid_high_cte.step(cte, elapsed)
angle = (target_angle + angle_high_speed + angle_high_speed_cte) / 3. # average
# we are in the brake deadband
# or we are decelerating below min_speed
if throttle <= self.brake_deadband or (throttle < 0 and trgtv < self.min_speed):
brake = -(self.vehicle_mass * throttle * self.wheel_radius) # vehicle mass times deceleration time wheel radius
if (brake < self.decel_limit):
brake = self.decel_limit
throttle = 0. # do not throttle while braking
else:
brake = 0. # no braking if the car is traveling
return throttle, brake, angle
else:
self.steer_pid_high.reset()
self.steer_pid_high_cte.reset()
self.throttle_pid.reset()
return 0., 0., 0.
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, *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):
# 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 Controller(object):
def __init__(self, S):
self.yaw_controller = YawController(S.wheel_base, S.steer_ratio, 0.1,
S.max_lat_accel, S.max_steer_angle)
#Initial paramaters
self.S = S
self.driverless_mass = self.S.vehicle_mass + self.S.fuel_capacity * GAS_DENSITY
self.pid_steer = PID(.6, .0004, .01, -S.max_steer_angle,
S.max_steer_angle)
self.last_cte = 0
self.p1 = .0001
self.p2 = .0001
#self.prev_vel_angular = 0
def control(self, twist_cmd, c_v, t_delta, pose, waypoints):
#Create cte from waypoints
cte = initial_cte(pose, waypoints)
cte_delta = (cte - self.last_cte) / t_delta
self.last_cte = cte
# update velocity for linear and angular
vel_linear = abs(twist_cmd.twist.linear.x)
vel_linear = vel_linear / (1 + self.p1 * cte * cte +
self.p2 * cte_delta * cte_delta)
vel_anglular = twist_cmd.twist.angular.z
velocity_error = vel_linear - c_v.twist.linear.x
#FIXME: DEFINITION OF ACCEL_ANGULAR MISSING
accel_angular = vel_anglular
#accel_angular = vel_anglular - self.prev_vel_angular
# Steering prediction
steer = self.yaw_controller.get_steering(vel_linear, accel_angular,
c_v.twist.linear.x)
# Steer correction
#FIXME: INCORRECT NUMBER OF VARIABLES
steer_correction = self.pid_steer.step(steer, cte_delta)
#steer_correction = self.pid_steer.step(cte, t_delta, steer, self.S.max_steer_angle)
steer = steer_correction + steer
# Throttle/brake prediction
# Acceleration (a)
a = velocity_error / t_delta
# Update sensitive to direction
throttle = 0
brake = 0
#positive accel keep under accel limit
if a > 0:
a = min(a, self.S.accel_limit)
# Decel must be kept under decel limit
else:
a = max(a, self.S.decel_limit)
torque = self.driverless_mass * a * self.S.wheel_radius
# accel/decel until limits or ideal state
if a > 0:
throttle = min(
1, torque / (self.driverless_mass * self.S.accel_limit *
self.S.wheel_radius))
else:
brake = min(abs(torque),
(self.driverless_mass * abs(self.S.decel_limit) *
self.S.wheel_radius))
# if deadband, set throttle and brake to 0 (cruise)
if abs(a) < self.S.brake_deadband:
throttle = 0
brake = 0
# Return throttle, brake, steer
return throttle, brake, steer
# Reset for post-manual control/Stop
def reset(self):
self.pid_steer.reset()
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):
# 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.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
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):
# 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
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.0 and current_vel < 0.1:
throttle = 0
brake = 400
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
# return 1., 0., 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: 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,
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, 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, 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, **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()
