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 = 1
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = .02
self.vel_lpf = LowPassFilter(tau, ts)
tau = 0.5
ts = .02
self.ang_vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
# TODO: Implement
pass
def control(self, current_vel, curr_ang_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)
#curr_ang_vel = self.vel_lpf.filt(curr_ang_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel, curr_ang_vel)
#rospy.logwarn("Angular Vel: {0}".format(angular_vel))
#rospy.logwarn("Current Angular Vel: {0}".format(curr_ang_vel))
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 700
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
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.1
kd = 0.006
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.last_vel = self.vel_lpf
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)
# 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(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 = 400 # N*m - to hold the car in place if we are stopped at a light. Acceleratoin ~ 1 m/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.
mx = 0.2
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2pi*tau) = cut_off frequency
ts = 0.02 # sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.wheel_base = wheel_base
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, angular_vel, linear_vel, dbw_enabled):
# 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)
debug_messages("Angular vel: ", angular_vel)
debug_messages("Linear vel: ", linear_vel)
debug_messages("Target Angular vel: ", angular_vel)
debug_messages("Current vel: ", current_vel)
debug_messages("Filtered vel: ", 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 # torque required at standstill as per updated walkthrough
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 Nm
# rospy.logwarn("braking at {0} Nm".format(brake))
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):
# *args, **kwargs):
# TODO: Implement
#pass
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 values
mx = 0.2 # Maximum throttle values
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)
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 # N8m - tp hold the car in place if we are stopped
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*map
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
rospy.logwarn(
"[Controller Init] vehicle_mass: {0}".format(vehicle_mass))
rospy.logwarn("[Controller Init] decel_limit: {0}".format(decel_limit))
rospy.logwarn("[Controller Init] accel_limit: {0}".format(accel_limit))
rospy.logwarn(
"[Controller Init] wheel_radius: {0}".format(wheel_radius))
min_speed = 0.1
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
#original
#original
#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)
#0530, add steering and throttle controller
###################
# Throttle
###################
#highway ok
#testing lot ok, need use 10kms
throttle_kp = 0.3
throttle_ki = 0.1
throttle_kd = 0.005
#testing lot ok, need use 10kms
#throttle_kp = 0.8
#throttle_ki = 0.002
#throttle_kd = 0.3
#not good in testing log
#throttle_kp = 0.8
#throttle_ki = 0.00
#throttle_kd = 0.06
min_throttle = 0.0 # Minimum throttle value
#max_throttle = 0.2 # Maximum throttle value (should be OK for simulation, not for Carla!)
#max_throttle = 0.5*accel_limit
max_throttle = accel_limit
#self.throttle_controller =PID(2.0, 0.4, 0.1, min_throttle, max_throttle)
self.throttle_controller = PID(throttle_kp, throttle_ki, throttle_kd,
min_throttle, max_throttle)
###################
# Steering
###################
#highway ok
#testing lot ok, need use 10kms
steer_kp = 0.4
steer_ki = 0.1
steer_kd = 0.005
#testing lot ok, need use 10kms
#steer_kp = 1.15
#steer_ki = 0.001
#steer_kd = 0.1
#not good in testing log
#steer_kp = 0.5
#steer_ki = 0.00
#steer_kd = 0.2
min_steer = -max_steer_angle
max_steer = max_steer_angle
self.steering_controller = PID(steer_kp, steer_ki, steer_kd, min_steer,
max_steer)
#is for filter out the high frequency noise of volcity
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 #Sample time
self.vel_lpf = LowPassFilter(tau, ts)
# catch the input
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
# this fun will be call 50Hz, from dbw_node
def control(self, current_vel, curr_ang_vel, dbw_enabled, target_vel,
target_ang_vel, get_first_image):
# TODO: Change the arg, kwarg list to suit your needs
if not dbw_enabled:
rospy.logwarn(
"dbw_enabled: {0}, reset controller".format(dbw_enabled))
#we also need to reset the PID controller to prevent
#the error accumulating.
# If we don't reset that, when we put dbw_enable back on , the PID will
# use the wrong error to do something wrong behavior. ## because there is a integral term in PID.
self.steering_controller.reset()
self.throttle_controller.reset()
return 0., 0., 0.
if get_first_image == False:
rospy.logwarn("get_first_image: {0}, return 0.,750.,0.".format(
get_first_image))
return 0., 750., 0.
#so, if dbw is enable
current_vel = self.vel_lpf.filt(current_vel)
rospy.logwarn("Target vel: {0}".format(target_vel))
rospy.logwarn("Target angular vel: {0}\n".format(target_ang_vel))
rospy.logwarn("current_vel: {0}".format(current_vel))
rospy.logwarn("current target_ang_vel: {0}".format(curr_ang_vel))
rospy.logwarn("filtered vel: {0}".format(self.vel_lpf.get()))
##############
# time
##############
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
############
# steering
############
#use Yaw controller to get the steering
steering_base = self.yaw_controller.get_steering(
target_vel, target_ang_vel, current_vel)
#rospy.logwarn("steering: {0}\n".format(steering))
#0530
target_ang_vel_error = target_ang_vel - curr_ang_vel
steering_correction = self.steering_controller.step(
target_ang_vel_error, sample_time)
steering_total = steering_base + steering_correction
steering = max(min(self.max_steer_angle, steering_total),
-self.max_steer_angle)
##############
# throttle
##############
#get the vel diff erro between the the vel we want to be (target_vel) v.s. the current_vel
# the target_vel comes from twist_cmd, which is the waypoint_follower published, is the target vel
# from waypoint updater
vel_error = target_vel - current_vel
self.last_vel = current_vel
#use PID controller to get the proper throttle under the vel_error and the sample time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
################
#some constrain
################
rospy.logwarn("[some constrain] target_vel: {0}".format(target_vel))
rospy.logwarn("[some constrain] current_vel: {0}".format(current_vel))
rospy.logwarn("[some constrain] vel_error: {0}".format(vel_error))
rospy.logwarn("[some constrain] throttle: {0}".format(throttle))
# if the target vel is 0, and we are very slow <0.1, set the throttle to 0 directly to stop
if target_vel == 0. and current_vel < 0.1:
rospy.logwarn(
"[twist_controller] target vel =0, and current vel is really small, directly STOP"
)
throttle = 0
brake = 750 #N*m to hold the car in place if we are stopped at a light. Acceleration 1m/s^2
rospy.logwarn("[directly STOP] throttle: {0}".format(throttle))
rospy.logwarn("[directly STOP] brake: {0}".format(brake))
rospy.logwarn("[directly STOP] steering: {0}".format(steering))
# vel_erro < 0 means the current car vel too fast than we want, and we still put small throttle
# we directly stop to add throttle, and get the real brake, because this time
# the car still moving, we need the real brake result from the vel, and mass, whell radius
#elif throttle < 0.1 and vel_error <0:
elif vel_error < 0:
rospy.logwarn("[twist_controller] slowing down")
throttle = 0
#decel is our desired deacceleration
decel = max(vel_error, self.decel_limit)
rospy.logwarn("[slowing down] self.decel_limit: {0}".format(
self.decel_limit))
rospy.logwarn("[slowing down] vel_error: {0}".format(vel_error))
rospy.logwarn("[slowing down] decel: {0}".format(decel))
# vehicle_mass in kilograms, wheel_radius in meters
# use abs is because the simluator, the de-acceleration should be negative, but
# the simulator get the positive number as the brake value
#brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque N*m
brake = abs(decel) * (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * self.wheel_radius
rospy.logwarn("[slowing down] self.wheel_radius: {0}".format(
self.wheel_radius))
rospy.logwarn("[slowing down] self.vehicle_mass: {0}".format(
self.vehicle_mass))
rospy.logwarn("[slowing down] throttle: {0}".format(throttle))
rospy.logwarn("[slowing down] brake: {0}".format(brake))
rospy.logwarn("[slowing down] steering: {0}".format(steering))
else:
rospy.logwarn("[donothing] throttle: {0}".format(throttle))
rospy.logwarn("[donothing] brake: {0}".format(brake))
rospy.logwarn("[donothing] steering: {0}".format(steering))
# Return throttle(min is 0.1 m/s), brake, steer
#hard code to keep car move stright
#throttle = 1.0
#brake = 0
#steering = 0
rospy.logwarn("[output] throttle: {0}".format(throttle))
rospy.logwarn("[output] brake: {0}".format(brake))
rospy.logwarn("[output] steering: {0}".format(steering))
return throttle, brake, steering
class Controller(object):
def __init__(self, update_rate_hz, vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
self.update_rate_hz = update_rate_hz
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
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
# THROTTLE
kp = 0.3 # proportional
ki = 0.08 # integral (i.e. bias)
kd = 0. # derivative (i.e. low-pass adjustment for kp)
mn = 0. # min throttle
mx = 1.0 # max throttle
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# STEERING
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
# SMOOTH OUT COMMANDED VELOCITY
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 1.0/update_rate_hz # 0.02 # sample time
self.vel_lpf = LowPassFilter(tau, ts)
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() # Start it over from scratch (should we keep the integral??)
return 0.,0.,0. # return nothingness
current_vel = self.vel_lpf.filt(current_vel)
# Get steering and slow down if we're going to fast to make the turn
steering, new_vel = self.yaw_controller.get_steering(linear_vel,angular_vel,current_vel)
#rospy.loginfo ("Linear Vel = %s Current Vel = %s New Vel = %s" % (linear_vel,current_vel,new_vel))
overshoot_error = current_vel - new_vel # will be positive if going too fast on turn
# penalize velocity error, but not if we need to slow down for the turn
vel_error = linear_vel - current_vel - overshoot_error
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 = RESTING_BRAKE_Nm
#elif throttle < .1 and vel_error < 0:
elif vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
if linear_vel == 0.:
brake = max(RESTING_BRAKE_Nm,brake)
#brake += RESTING_BRAKE_Nm
#rospy.loginfo ("Throttle = %s Brake = %s Steering = %s" % (throttle,brake,steering))
return throttle, brake, steering
# Return throttle, brake, steer
#return 0.5, 0., 0.
return 0., RESTING_BRAKE_Nm, 0.
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
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# PID controller
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()
self.last_vel = None
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)
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
# control brake here
# if we want to stop the car
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
# if we are going faster than we want
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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
'''
# experimental parameters
kp = 0.3
ki = 0.1
kd = 0.02
mn = 0. # minimum throttle
mx = 0.2 # maximum throttle
self.throttle_controller = PID(kp,ki,kd,mn,mx)
'''
kp = 0.5
ki = 0.0001
kd = 0.15
mn = decel_limit # minimum throttle
mx = 0.5 # maximum throttle
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# to control the noisy messages in current_velocity via a provided low pass filter
tau = 0.05
ts = 0.02
self.velocity_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
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.last_throttle = 0
self.last_brake = 100
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# check if car is in manual mode
if not dbw_enabled:
self.throttle_controller.reset()
# disconnect the PID controllet to avoid error accumulating for the Integral term
# to avoid erratic behaviours at re-insertion of the controller
return 0., 0., 0.
# get current velocity
current_vel = self.velocity_lpf.filt(current_vel)
## get the comands
steer = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
velocity_error = linear_vel - current_vel
# self.last_velocity = current_vel
## update internal time
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
# get throttle
aceeleration = self.throttle_controller.step(velocity_error,
sample_time)
#the square difference between the proposed velocity (linear_vel) and the current velocity (current_vel)
#divided by 2 times the distance of 30 meters.
#The positive value that returns this algorithm are the throttle values of the car.
smooth_acc = ((linear_vel * linear_vel) -
(current_vel * current_vel)) / (2 * 30)
if smooth_acc >= 0:
throttle = smooth_acc
else:
throttle = 0
if throttle > 0.5:
throttle = 0.5
#smoothing throttle acceleration and deceleration
if (throttle > 0.025) and (throttle - self.last_throttle) > 0.005:
throttle = max((self.last_throttle + 0.0025), 0.005)
if throttle > 0.025 and (throttle - self.last_throttle) < -0.05:
throttle = self.last_throttle - 0.05
self.last_throttle = throttle
# get brake
brake = 0
if linear_vel == 0 and current_vel < 0.1: # 0.1 m/s is the minimum velocity
throttle = 0
brake = 700 # N*m - amount necessary to hold the car in place while stopped
# like if we're waiting at a traffic light
# tha corresponding acceleration is around 1m/s^2
elif throttle < 0.025 and velocity_error < 0.: #if we're stopping/decelerating
throttle = 0
decel = max((smooth_acc * 5), self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius # Torque N*m
#smoothing brake
if brake > 100 and (brake - self.last_brake) > 20:
brake = max((self.last_brake + 20), 100)
if brake > 20 and (brake - self.last_brake) > 20:
brake = max((self.last_brake + 20), 20)
#rospy.loginfo('brake: %f', brake)
#rospy.loginfo('trottle: %f', throttle)
self.last_brake = brake
return throttle, brake, steer
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Instance values
self.dbw_enabled = True
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # check
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) # check
self.brake_deadband = rospy.get_param('~brake_deadband', .1) # check
self.decel_limit = -650.0
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) # check
self.wheel_base = rospy.get_param('~wheel_base', 2.8498) # check
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) # check
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) # check
self.current_velocity = 0.0
self.current_angular = 0.0
self.proposed_linear = 0.0
self.proposed_angular = 0.0
self.cte = 0.0
self.lp_filter = LowPassFilter(0.5, CONTROL_PERIOD)
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)
# NOTE: I pass the dbw_node object to the controller so I don't
# have huge ugly param list for initialization. Perhaps
# not the best practice...
self.controller = Controller(self)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# rospy.Subscriber('/cross_track_error', Float64, self.cross_track_cb)
self.loop()
def loop(self):
rate = rospy.Rate(CONTROL_RATE) # 50Hz
while not rospy.is_shutdown():
throttle, brake, steering = self.controller.control(
self.proposed_linear, self.proposed_angular,
self.current_velocity, self.current_angular)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
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):
# There is a bug in the pure_pursuit code that sometimes returns a negative
# linear.x, so we just take the abs() here as suggested on Slack
self.proposed_linear = abs(msg.twist.linear.x)
self.proposed_angular = msg.twist.angular.z
return
def current_velocity_cb(self, msg):
# lowpass filter the finite difference approximation to the acceleration
self.lp_filter.filt(
(self.current_velocity - msg.twist.linear.x) / CONTROL_RATE)
self.current_velocity = msg.twist.linear.x
self.current_angular = msg.twist.angular.z
return
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
return
def cross_track_cb(self, msg):
self.cte = msg.data
return
class Controller(object):
def __init__(self, *args, **kwargs):
wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
min_velocity = kwargs['min_velocity']
max_lat_accel = kwargs['max_lat_accel']
max_steer_angle = kwargs['max_steer_angle']
self.decel_limit = kwargs['decel_limit']
accel_limit = kwargs['accel_limit']
self.deadband = kwargs['deadband']
self.yawController = YawController(wheel_base, self.steer_ratio,
min_velocity, max_lat_accel,
max_steer_angle)
#self.correcting_pid = PID(0.065, 0.000775, 0.775, -1.57, 1.57)
#self.velocity_pid = PID(0.8, 0.001, 0.2, decel_limit, accel_limit)
#self.velocity_pid = PID(1.6, 0.00001, 0.04, decel_limit, accel_limit) Good: best so far
#self.velocity_pid = PID(2.0, 0.4, 0.1, decel_limit, accel_limit) Too much bias
#self.velocity_pid = PID(0.1, 0.001, 1.0, decel_limit, accel_limit) Not responsive enough
#self.velocity_pid = PID(0.9, 0.0005, 0.07, decel_limit, accel_limit) fair
#self.velocity_pid = PID(5.0, 0.00001, 0.2, decel_limit, accel_limit) ok
#self.velocity_pid = PID(2.0, 0.0, 0.05, decel_limit, accel_limit) not as good as 1.6
#self.velocity_pid = PID(2.0, 0.00001, 0.04, decel_limit, accel_limit) good
#self.velocity_pid = PID(0.35, 0., 0., decel_limit, accel_limit)
self.velocity_pidHI = PID(0.35, 0., 0., self.decel_limit, accel_limit)
self.velocity_pidLO = PID(0.7, 0., 0., self.decel_limit / 2.,
accel_limit)
#self.lowpassFilt = LowPassFilter(accel_limit/2., 0.02) # good
self.lowpassFilt = LowPassFilter(0.07, 0.02)
self.topVelocity = 0.
def control(self, linear_velocity_target, angular_velocity_target,
linear_velocity_current):
# Throttle
if linear_velocity_target > 2.:
throttle_correction = self.velocity_pidHI.step(
linear_velocity_target - linear_velocity_current, 0.02)
else:
throttle_correction = self.velocity_pidLO.step(
linear_velocity_target - linear_velocity_current, 0.02)
throttle_correction = self.lowpassFilt.filt(throttle_correction)
if throttle_correction > 0.:
brake = 0.0
if linear_velocity_target <= 0.01:
throttle = 0.
self.velocity_pidLO.reset() # <-- Reset the velocity PID
self.velocity_pidHI.reset()
else:
throttle = throttle_correction
# Braking
else:
throttle = 0.
brake = -throttle_correction
if brake < self.deadband * self.decel_limit / 2.:
brake = 0
# Steering
if linear_velocity_target > self.topVelocity: # mitigate rapid turning
self.topVelocity = linear_velocity_target
if linear_velocity_current > 0.05:
steering = self.yawController.get_steering(
self.topVelocity, angular_velocity_target,
linear_velocity_current
) # self.correcting_pid.step(angular_velocity_target, 0.1)
else:
steering = 0.
# Alternate approach: steering = angular_velocity_target * self.steer_ratio
return throttle, brake, steering
def reset(self):
self.velocity_pidLO.reset()
self.velocity_pidHI.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):
# Create an object with class YawController
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# Define PID tuning parameters for the throttle controller
kp = 0.3 #5, 0.3
ki = 0.1 #0.5, 0.1
kd = 0. # 0.5, 0.
mn = 0. # Minimum throttle value
mx = 0.2 # Maximum throttle value
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.steer_lpf = LowPassFilter(tau, ts)
self.vel_lpf = LowPassFilter(tau, ts)
self.t_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.throttle_controller = PID(kp, ki, kd, -5, 1)
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# The function uses the YawController class and PID class to calculate the throttle, steering inputs and applies the brake based on throttle, velocity.
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
vel_error = linear_vel - current_vel
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
steering = self.steer_lpf.filt(steering)
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
acc = self.throttle_controller.step(vel_error, sample_time)
acc = self.t_lpf.filt(acc)
brake = 0.0
if linear_vel == 0.0: # and current_vel < 0.1:
throttle = 0.0
brake = 700 #N-m - to hold the car in place if we stopped at a light.
self.throttle_controller.reset()
else: # and vel_error < 0:
throttle = acc
if acc <= 0.0:
throttle = 0
decel = -acc
if decel < self.brake_deadband:
decel = 0
brake = abs(decel) * (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * 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
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/(2*pi*tau) = cutoff frequency
ts = 0.02 # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
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.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
0.1, self.max_lat_accel,
self.max_steer_angle)
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
'''
CASE: dirving by wire not enabled.
In this case nothing should be done: throttle = 0, brake=0, steer=0
'''
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
'''
CASE: dirving by wire enabled
'''
current_vel = self.vel_lpf.filt(
current_vel
) # filtering the current velocity by using the lower pass to let car run smoothly
steering = self.yaw_controller.get_steering(
linear_vel, angular_vel, current_vel) # calculate stering
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) # calculate throttle
'''
decision for brake
'''
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 ~1 m/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
return throttle, brake, steering
class Controller(object):
def __init__(self, pid_controller_properties, yaw_controller_properties,
lowpass_filter_properties):
# @done Find suitable values for PID properties
self.pid = PID(kp=2.0,
ki=0.05,
kd=0.005,
mn=pid_controller_properties.decel_limit,
mx=pid_controller_properties.accel_limit)
self.max_brake_torque = pid_controller_properties.max_brake_torque
self.max_throttle = pid_controller_properties.max_throttle
self.yaw_controller = YawController(
yaw_controller_properties.wheel_base,
yaw_controller_properties.steer_ratio,
yaw_controller_properties.min_speed,
yaw_controller_properties.max_lat_accel,
yaw_controller_properties.max_steer_angle)
# @done Check what are actual correct properties for tau and ts for the lowpass filter
self.low_pass_filter_pid = LowPassFilter(
lowpass_filter_properties.brake_deadband,
lowpass_filter_properties.ts)
self.low_pass_filter_yaw_controller = LowPassFilter(
lowpass_filter_properties.wheel_radius,
lowpass_filter_properties.ts)
def control(self, proposed_linear_velocity, proposed_angular_velocity,
current_linear_velocity, is_dbw_enabled):
throttle = 0.0
brake = 0.0
steer = 0.0
if not is_dbw_enabled:
rospy.loginfo(
"Gauss - DBW not enabled, resetting PID, throttle, brake and steer"
)
self.pid.reset()
return throttle, brake, steer
# Compute the error for the PID
error = proposed_linear_velocity - current_linear_velocity
rospy.loginfo("Gauss - Got error for PID: " + str(error))
pid_result = self.pid.step(error=error, sample_time=1.0 / 50.0)
rospy.loginfo("Gauss - PID Result: " + str(pid_result))
stopdeadzone = 0.5
if pid_result >= 0.0 and proposed_linear_velocity >= stopdeadzone:
# We want to accelerate
throttle = self.scale(pid_result, self.max_throttle)
brake = 0.0
else:
# We want to decelerate
throttle = 0.0
brake = self.scale(abs(pid_result), self.max_brake_torque)
# Apply low pass filter on braking
brake = self.low_pass_filter_pid.filt(brake)
steer = self.yaw_controller.get_steering(proposed_linear_velocity,
proposed_angular_velocity,
current_linear_velocity)
# Apply low pass filter on steering
kSteeringFactor = 1.2
steer = self.low_pass_filter_yaw_controller.filt(
steer) * kSteeringFactor
rospy.loginfo("Gauss - Throttle: " + str(throttle))
rospy.loginfo("Gauss - Brake: " + str(brake))
rospy.loginfo("Gauss - Steering: " + str(steer))
# Return throttle, brake, steer
return throttle, brake, steer
def scale(self, value, scale):
kStretch = 0.5
if value < 0.0:
return 0.0
else:
return scale * math.tanh(value * kStretch)
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, *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 YawController(object):
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.min_speed = 5
self.max_lat_accel = max_lat_accel
self.previous_dbw_enabled = False
self.min_angle = -max_steer_angle
self.max_angle = max_steer_angle
self.linear_pid = PID(0.9, 0.001, 0.0004, self.min_angle, self.max_angle)
#self.cte_pid = PID(0.4, 0.1, 0.1, self.min_angle, self.max_angle)
self.cte_pid = PID(0.4, 0.1, 0.2, self.min_angle, self.max_angle)
self.tau = 0.2
self.ts = 0.1
self.low_pass_filter = LowPassFilter(self.tau, self.ts)
def get_angle(self, radius, current_velocity):
angle = math.atan(self.wheel_base / radius) * self.steer_ratio
return max(self.min_angle, min(self.max_angle, angle))
def get_steering_calculated(self, linear_velocity, angular_velocity, current_velocity):
"""
Formulas:
angular_velocity_new / current_velocity = angular_velocity_old / linear_velocity
radius = current_velocity / angular_velocity_new
angle = atan(wheel_base / radius) * self.steer_ratio
"""
angular_velocity = current_velocity * angular_velocity / linear_velocity if abs(linear_velocity) > 0. else 0.
if abs(current_velocity) > 0.1:
max_yaw_rate = abs(self.max_lat_accel / current_velocity)
angular_velocity = max(-max_yaw_rate, min(max_yaw_rate, angular_velocity))
return self.get_angle(max(current_velocity, self.min_speed) / angular_velocity, current_velocity) if abs(angular_velocity) > 0. else 0.0;
def get_steering_pid(self, angular_velocity, angular_current, dbw_enabled):
angular_error = angular_velocity - angular_current
sample_step = 0.02
if not(self.previous_dbw_enabled) and dbw_enabled:
self.previous_dbw_enabled = True
self.linear_pid.reset()
self.low_pass_filter = LowPassFilter(self.tau, self.ts)
else:
self.previous_dbw_enabled = False
steering = self.linear_pid.step(angular_error, sample_step)
steering = self.low_pass_filter.filt(steering)
return steering
def get_steering_pid_cte(self, final_waypoint1, final_waypoint2, current_location, dbw_enabled):
steering = 0
if final_waypoint1 and final_waypoint2:
# vector from car to first way point
a = np.array([current_location.x - final_waypoint1.pose.pose.position.x, current_location.y - final_waypoint1.pose.pose.position.y, current_location.z - final_waypoint1.pose.pose.position.z])
# vector from first to second way point
b = np.array([final_waypoint2.pose.pose.position.x-final_waypoint1.pose.pose.position.x, final_waypoint2.pose.pose.position.y-final_waypoint1.pose.pose.position.y, final_waypoint2.pose.pose.position.z-final_waypoint1.pose.pose.position.z])
# progress on vector b
# term = (a.b / euclidian_norm(b)**2) * b where a.b is dot product
# term = progress * b => progress = term / b => progress = (a.b / euclidian_norm(b)**2)
progress = (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2])
# position where the car should be: waypoint1 + progress * b
error_pos = np.array([final_waypoint1.pose.pose.position.x, final_waypoint1.pose.pose.position.y, final_waypoint1.pose.pose.position.z]) + progress * b
# difference vector between where the car should be and where the car currently is
error = (error_pos - np.array([current_location.x, current_location.y, current_location.z]))
# is ideal track (b) left or right of the car's current heading?
dot_product = a[0]*-b[1] + a[1]*b[0]
direction = 1.0
if dot_product >= 0:
direction = -1.0
else:
direction = 1.0
# Cross track error is the squared euclidian norm of the error vector: CTE = direction*euclidian_norm(error)**2
cte = direction * math.sqrt(error[0]*error[0]+error[1]*error[1]+error[2]*error[2])
sample_step = 0.02
if not(self.previous_dbw_enabled) and dbw_enabled:
self.previous_dbw_enabled = True
self.cte_pid.reset()
self.low_pass_filter = LowPassFilter(self.tau, self.ts)
else:
self.previous_dbw_enabled = False
steering = self.cte_pid.step(cte, sample_step)
#steering = self.low_pass_filter.filt(steering)
return steering
class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
#pass
kp = 2.01
ki = 0.001
kd = 0.01
kp = 1.6
ki = 0.0
kd = 0.001
#kp = 3.03
#kd = 0.012
mn = -1.0
mx = 1.0
self.pid_controller = PID(kp, ki, kd, mn, mx)
ts = 1.0 / 50.0
tau = 1.0 / 1000 #default value
tau = (7.0 / 8.0) * ts * 0.632
self.throttle_filter = LowPassFilter(tau, ts)
self.steer_filter = LowPassFilter(tau, ts)
def control(self, *args, **kwargs):
current_linear_velocity = kwargs['current_vel']
target_linear_velocity = kwargs['target_vel']
target_angular_velocity = kwargs['target_angular_vel']
yaw_controller = kwargs['yaw_control']
max_brake = (GAS_DENSITY * kwargs['fuel_capacity'] + kwargs['vehicle_mass']) * \
kwargs['decel_limit'] * kwargs['wheel_radius']
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
actual_target_vel = target_linear_velocity
if 0:
if target_linear_velocity < 0.015: #to protect against small movement
target_linear_velocity = 0
#rospy.loginfo("target velocity: %f %f %f", actual_target_vel, target_linear_velocity, current_linear_velocity)
error = (target_linear_velocity - current_linear_velocity)
if 0:
#ease on throttle if current is trying to meet the target_linear_velocity
#but brake quickly if the current exceeds target
if abs(error) < 0.01: #error < 0.05 and error > -0.0001 :
error = 0
#rospy.loginfo("error:%f", error)
#rospy.loginfo("kd:%f %f %f", self.pid_controller.kp, self.pid_controller.kd, self.pid_controller.ki)
noise_throttle = self.pid_controller.step(error,
sample_time=1.0 / 50.0)
#filter the throttle
throttle = self.throttle_filter.filt(noise_throttle)
#throttle = noise_throttle # for time being
noise_steering = yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_linear_velocity)
steering = self.steer_filter.filt(noise_steering)
brake = 0.0
actual_th = throttle
if throttle < 0:
brake = throttle * max_brake
throttle = 0
#rospy.loginfo("th:%f steer: %f brake:%f", actual_th, steering, brake)
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
## Initialize Throttle Controller as PID:
kp = 0.123757 ## used values from PID project to begin with
ki = 0.0
kd = 0.770457
mn = 0
mx = 0.2
self.throttle_controller = PID(kp, ki, kd, mn, mx)
## Initialize yaw controller::
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
## Initialize low pass filter::
tau = 0.5
ts = 0.2
self.lpf = LowPassFilter(tau, ts)
## Initialize 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, curr_vel, linear_vel, angular_vel, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# Reset to stop if dbw is not enabled:
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
# Filter out the curr_vel to remove high frequency noise:
curr_vel = self.lpf.filt(curr_vel)
self.last_vel = curr_vel
# Compute Steering using Yaw Controller :
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
curr_vel)
#rospy.logwarn('linera_Vel $s ,current vel %s, angular_vel: %s',
#linear_vel, curr_vel, angular_vel)
# Compute inputs for the Throttle controller step function (error, sample_time):
vel_err = linear_vel - curr_vel
sample_time = rospy.get_time() - self.last_time
throttle = self.throttle_controller.step(vel_err, sample_time)
#Since Throttle is set now, break should be set to zero
brake = 0
# Store curr values of time and velocity into variable
self.last_time = rospy.get_time()
self.last_vel = curr_vel
# Check on braking conditions:
# Assumption here is if, linear_vel is zero and curr velocity is <0.1, we are trying to stop:
if linear_vel == 0 and curr_vel < 0.1:
throttle = 0
brake = 400 # set to max torque to stay at stop
# Assumption -> if vel_error is negative, vehicle is moving faster than expected, we need to decelerate by applying brake
if 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
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']
min_speed = 0
self.s_lpf = LowPassFilter(tau=0.5, ts=1)
self.linear_pid = PID(kp=0.8,
ki=0,
kd=0.05,
mn=self.decel_limit,
mx=0.5 * self.accel_limit)
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
min_speed, self.max_lat_accel,
self.max_steer_angle)
self.steering_pid = PID(kp=0.15,
ki=0.001,
kd=1,
mn=-self.max_steer_angle,
mx=self.max_steer_angle)
def control(self, proposed_linear_velocity, proposed_angular_velocity,
current_linear_velocity, cross_track_error,
duration_in_seconds):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
linear_velocity_error = proposed_linear_velocity - current_linear_velocity
velocity_correction = self.linear_pid.step(linear_velocity_error,
duration_in_seconds)
brake = 0
throttle = velocity_correction
if (throttle < 0):
deceleration = abs(throttle)
brake = (
self.vehicle_mass + self.fuel_capacity * GAS_DENSITY
) * self.wheel_radius * deceleration if deceleration > self.brake_deadband else 1.
throttle = 0.0
predictive_steering = self.yaw_controller.get_steering(
proposed_linear_velocity, proposed_angular_velocity,
current_linear_velocity)
corrective_steering = self.steering_pid.step(cross_track_error,
duration_in_seconds)
steering = predictive_steering + corrective_steering * 0.33
steering = self.s_lpf.filt(steering)
return throttle, brake, steering
def reset(self):
self.linear_pid.reset()
self.steering_pid.reset()
class Controller(object):
def __init__(self, vehicle_mass, brake_deadband, decel_limit, accel_limit,
wheel_radius, wheel_base, max_steer_angle, max_lat_accel,
steer_ratio):
# TODO: Implement
# Steering
self.yc = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
# Throttle
kp = 0.3
ki = 0.1
kd = 0
mn = 0 #min throttle
mx = 0.4 #max throttle
self.tc = PID(kp, ki, kd, mn, mx)
# LPF
tau = 0.5
ts = 0.02
self.lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
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, proposed_linear_velocity, proposed_angular_velocity,
current_linear_velocity, current_angular_velocity, dbw_status):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_status:
self.tc.reset()
return 0., 0., 0.
current_linear_velocity = self.lpf.filt(current_linear_velocity)
steering = self.yc.get_steering(proposed_linear_velocity,
proposed_angular_velocity,
current_linear_velocity)
vel_error = proposed_linear_velocity - current_linear_velocity
self.last_vel = current_linear_velocity
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.tc.step(vel_error, sample_time)
brake = 0
if proposed_linear_velocity == 0 and current_linear_velocity < 0.1:
throttle = 0
brake = 700
elif throttle < 0.01 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius #Torque in Nm
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
# pass
tau = 0.5
ts = .02
self.vel_lpf = LowPassFilter(tau, ts)
kp = 0.3
ki = 0.1
kd = 0.
mn = 0.
mx = 0.5
self.throttle_controller = PID(kp, ki, kd, mn, mx)
wheel_base = kwargs['wheel_base']
steer_ratio = kwargs['steer_ratio']
max_lat_accel = kwargs['max_lat_accel']
max_steer_angle = kwargs['max_steer_angle']
min_speed = 0.1
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
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.last_time = rospy.get_time()
def control(self, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# return 1., 0., 0.
proposed_linear_velocity = args[0]
proposed_angular_velocity = args[1]
current_linear_velocity = args[2]
dbw_status = args[3]
if not dbw_status:
self.throttle_controller.reset()
return 0., 0., 0.
current_linear_velocity = self.vel_lpf.filt(current_linear_velocity)
# http://wiki.ros.org/rospy/Overview/Logging
# rospy.logwarn("Target linear velocity: {0}".format(proposed_linear_velocity))
# rospy.logwarn("Target angular velocity: {0}".format(proposed_angular_velocity))
# rospy.logwarn("Current linear velocity: {0}".format(current_linear_velocity))
# rospy.logwarn("filtered linear velocity: {0}".format(self.vel_lpf.get()))
steer = self.yaw_controller.get_steering(proposed_linear_velocity,
proposed_angular_velocity,
current_linear_velocity)
vel_error = proposed_linear_velocity - current_linear_velocity
now_time = rospy.get_time()
sample_time = now_time - self.last_time
self.last_time = now_time
throttle = self.throttle_controller.step(vel_error, sample_time)
#rospy.loginfo("Target_lv: {0} Target_av: {1} Current_lv: {2} filtered_lv: {3} sample_time: {4}".format(proposed_linear_velocity,
# proposed_angular_velocity,
# current_linear_velocity,
# self.vel_lpf.get(),
# sample_time))
brake = 0
if proposed_linear_velocity == 0. and current_linear_velocity < 0.1:
throttle = 0
brake = 400
elif throttle < .1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius #Torque N*m
# rospy.loginfo("throttle: {0} brake: {1} steer: {2}".format(throttle, brake, steer))
return throttle, brake, steer
class Controller(object): #def __init__(self, *args, **kwargs):
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)
# Parameters for the throttle control PID controller #
kp = 0.3
#kp = 1.0
#kp = 1.5
#kp = 0.5
#kp = 0.2
#kp = 0.05
#kp = 0.02
#kp = 0.03
#kp = 0.045
#kp = 0.035
#kp = 0.03
#kp = 0.027
#kp = 0.02
#kp = 0.5
#kp = 0.035
ki = 0.1
#ki = 0.3
#ki = 0.0
#ki = 0.02
#ki = 0.005
#ki = 0.01
#ki = 0.005
#ki = 0.008
#ki = 0.007
#ki = 0.006
#ki = 0.0055
#ki = 0.005
#ki = 0.003
#ki = 0.004
#ki = 0.0045
#ki = 0.003
#ki = 0.0
kd = 0.0
#kd = 0.04
#kd = 0.0
#kd = 0.005
#kd = 0.001
#kd = 0.0005
#kd = 0.0
mn = 0.0 # Minimum throttle value #
mx = 0.2 # Maximum throttle value #
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Low pass filter to reduce the effect of the noise of the incoming velocity #
tau = 0.5 # 1/(2pi*tau)_ = cutoff frequency #
ts = 0.02 # [s] 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
# Check if the dbw is enabled in order not to erroneously add to the I part of the PID controller #
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)
# Calculate the difference between where the vehicle is requested to be vs where to vehicle currently is #
vel_error = linear_vel - current_vel
self.last_vel = current_vel
# Get the sample time for each step of the 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.0
# If the requested velocity is 0 and the vehicle has come to a standstill, apply the brakes #
if linear_vel == 0.0 and current_vel < 0.1:
throttle = 0.0
brake = 400.0 # [N*m] - To hold the car in place when waiting for the traffic light to turn from red to green. Acceleration ~ 1 [m/s^2] #
# If the requested velocity is smaller than the current velocity, apply the brakes with a factor depending on the required deceleration and maximum deceleration #
elif throttle < 0.1 and vel_error < 0.0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = abs(
decel
) * self.vehicle_mass * self.wheel_radius # [N*m] Torque #
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):
# Done: 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
mx = 1.0
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()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# Done: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# Video 8: 7.01
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: # When we need to break for traffic light
throttle = 0
brake = 400
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
# In case debugging needed then enter log values here
return throttle, brake, steering
class Controller(object):
def __init__(self, max_speed, 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.4
ki = 0.1
kd = 0.
mn = 0. # Minimum throttle value
mx = max_speed * 0.005 # 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 + fuel_capacity*GAS_DENSITY
self.fuel_capaciy = 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, target_velocity, current_velocity):
current_vel = self.vel_lpf.filt(current_velocity.linear.x)
steering = self.yaw_controller.get_steering(
target_velocity.linear.x,
target_velocity.angular.z,
current_velocity.linear.x)
vel_error = target_velocity.linear.x - 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 target_velocity.linear.x == 0. and current_vel < 0.1:
throttle = 0
brake = 400
elif vel_error < 0.:
decel = max(vel_error, self.decel_limit)
brake = 0.2*abs(decel)*self.vehicle_mass*self.wheel_radius # Torque N*m
if vel_error == 0.0 and throttle == 0.0:
brake = 0.0
return throttle, brake, steering
def reset(self):
self.throttle_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):
# TODO: Implement
self.yaw_controller = YawController(
wheel_base, steer_ratio, 0.1, max_lat_accel,
max_steer_angle) # 0.1 m/s is the lowest speed of the car
# PID gain parameters
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)
#the velocity data that's coming from the messages is noisy, so low-pass filter is used to filter out all the high frequency noise in the velocity data
tau = 0.5 # 1/(2*pi*tau) = cut-off 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
# If the vehicle is just sitting and in our pid we have an integral term and the DBW is enabled, then the intergral
# term will be just accumulating error
# So, if drive-by-wire is not enabled, then reset the controller
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
# Filter the current velocity from /current_velocity topic using the low-pass filter
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 car is supposed to remain stopped
# Carla has an automatic transmission, which means the car will roll forward if no brake and no throttle is applied
# So, a minimum brake force to prevent Carla from rolling has to be applied
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
# If we need to decelerate
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 # Braking torque in N-m
return throttle, brake, steering
class Controller(object):
def __init__(self, car_params):
# TODO: Implement
self.car_params = car_params
self.dbw_enabled = False
self.prev_time = None
self.brake_db = car_params.brake_db
self.steer_filter = LowPassFilter(0.9, 0.8)
self.yaw_controller = YawController(car_params.wheel_base,
car_params.steer_ratio,
car_params.min_speed,
car_params.max_lat_accel,
car_params.max_steer_angle)
self.pid = PID(kp=0.7,
ki=0.003,
kd=0.1,
mn=car_params.decel_limit,
mx=car_params.accel_limit)
def control(self, time, ref_lin_vel, ref_ang_vel, lin_vel, ang_vel,
dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# TODO_NEHAL - Check if it works without self.dbw_enabled
if not self.dbw_enabled and dbw_enabled:
self.pid.reset()
self.dbw_enabled = dbw_enabled
if not dbw_enabled or self.prev_time is None:
self.prev_time = time
return 0.0, 0.0, 0.0
delta_time = time - self.prev_time
self.prev_time = time
result = self.pid.step(ref_lin_vel - lin_vel, delta_time)
if result > 0:
throttle = result
brake = 0.0
elif math.fabs(result) > self.brake_db:
throttle = 0.0
brake = (self.car_params.vehicle_mass +
(self.car_params.fuel_capacity *
GAS_DENSITY)) * -result * self.car_params.wheel_radius
else:
throttle = 0.0
brake = 0.0
if ref_lin_vel == 0 and lin_vel < 0.5:
throttle = 0.0
temp_brake_val = (self.car_params.vehicle_mass +
(self.car_params.fuel_capacity * GAS_DENSITY)
) * -1.0 * self.car_params.wheel_radius
brake = max(brake, temp_brake_val)
steer = self.yaw_controller.get_steering(ref_lin_vel, ref_ang_vel,
lin_vel)
steer = self.steer_filter.filt(steer)
# Return throttle, brake, steer
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.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()
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 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_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 = True # 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("/dbw_enabled", Bool, self.dbw_enabled_cb)
rospy.loginfo("### Controller initialized Yeah")
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>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <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
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)
self.publish(thro_cmd, brake_cmd, steer_cmd)
rospy.loginfo("Publish:" + str(thro_cmd) + " " +
str(steer_cmd))
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
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 # min throttle
mx = 0.2 # max throttle
self.throttle_controller = PID(kp, ki, kd, mn, mx)
self.brake_controller = PID(0.3, 0.0, 0.0, 0.0, 700)
tau = 0.5 # 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, linear_vel, dbw_enabled, current_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(current_vel, angular_vel,
linear_vel)
vel_error = -(linear_vel - current_vel)
self.last_vel = current_vel
print(vel_error)
current_time = rospy.get_time()
sampled_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sampled_time)
brake = 0 #self.brake_controller.step(-vel_error, sampled_time)
if linear_vel == 0.0 and current_vel < 0.1:
throttle = 0
brake = 700
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, **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()
class Controller(object):
def __init__(self, vehicle_mass, decel_limit, accel_limit, wheel_radius,
wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
# Controller paramters are from "DBW Walkthrough"
tau = 0.5
ts = 0.02
self.current_velocity_lpf = LowPassFilter(tau, ts)
kp = 0.3
ki = 0.1
kd = 0.0
throttle_min = 0.0
throttle_max = 0.4
self.throttle_pid = PID(kp, ki, kd, throttle_min, throttle_max)
self.steering_yaw_controller = YawController(wheel_base, steer_ratio,
0.1, max_lat_accel,
max_steer_angle)
self.vehicle_mass = vehicle_mass
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
# For PID
self.last_time = rospy.get_time()
def control(self, dbw_enabled, cmd_linear_velocity, cmd_angular_velocity,
current_velocity):
if not dbw_enabled:
self.throttle_pid.reset()
return 0.0, 0.0, 0.0
velocity_error = None
current_velocity_filtered = None
current_velocity_filtered = self.current_velocity_lpf.filt(
current_velocity)
velocity_error = cmd_linear_velocity - current_velocity_filtered
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_pid.step(velocity_error, sample_time)
brake = 0
# Close enough to being stationary
if cmd_linear_velocity == 0.0 and current_velocity_filtered < 0.1:
throttle = 0
brake = 700 # To prevent Carla from moving requires about 700 Nm of torque.
# Want to slow down the car, but just letting off the throttle is not enough
if velocity_error < 0 and throttle < 0.1:
throttle = 0
decel = abs(
velocity_error /
1.0) # a = dv/dt is in m/s^2, so divide dv (m/s) by 1 second
decel_limit = abs(self.decel_limit)
# torque = radius * force = radius * mass * acceleration
if decel < decel_limit:
brake = self.wheel_radius * self.vehicle_mass * decel
else:
brake = self.wheel_radius * self.vehicle_mass * decel_limit
steering = self.steering_yaw_controller.get_steering(
cmd_linear_velocity, cmd_angular_velocity, current_velocity)
return throttle, brake, steering
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, vehicle_mass, fuel_cap, brake_deadband, decel_lmt,
accel_lmt, wheel_radius, wheel_base, steer_ratio,
max_lat_accel, max_steer_angle):
self.yaw_ctrl = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.0
mn = 0.0 # Minimum throttle value
mx = 0.4 # Maximum throttle value
self.throttle_ctrl = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2*pi*tau) = cutoff frequency
ts = 0.02 # sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_cap = fuel_cap
self.brake_deadband = brake_deadband
self.decel_lmt = decel_lmt
self.accel_lmt = accel_lmt
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
self.last_vel = 0
def control(self, current_vel, angular_vel, linear_vel, dbw_enabled):
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_ctrl.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_ctrl.get_steering(linear_vel, angular_vel,
current_vel)
vel_err = linear_vel - current_vel
self.last_vel = current_vel
curr_time = rospy.get_time()
sample_time = curr_time - self.last_time
self.last_time = curr_time
throttle = self.throttle_ctrl.step(vel_err, sample_time)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 700 # to hold the car in place
elif throttle < .1 and vel_err < 0:
throttle = 0
decel = max(vel_err, self.decel_lmt)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius #Torqur 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):
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.min_speed = 0.1
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
self.min_speed, self.max_lat_accel,
self.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.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
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 abs(linear_vel) < 0.0001 and abs(current_vel) < 0.1:
throttle = 0.
brake = 700 # Nm - 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 = -1. * decel * self.vehicle_mass * self.wheel_radius # Nm - Torque
return throttle, brake, steering