class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, brake_deadband, accel_limit, decel_limit,
vehicle_mass, wheel_radius, fuel_capacity, sample_freq):
self.dt = 1.0 / sample_freq
self.brake_deadband = brake_deadband
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.fuel_capacity = fuel_capacity
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.accel_controller = PID(kp=0.45,
ki=0.02,
kd=0.01,
mn=decel_limit,
mx=accel_limit)
self.throttle_controller = PID(kp=1.0,
ki=0.001,
kd=0.10,
mn=0.0,
mx=0.2)
self.lowpass_filter = LowPassFilter(0.15, self.dt)
def control(self, dbw_enabled, target_linear_velocity,
target_angular_velocity, current_velocity):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
vel_error = target_linear_velocity - current_velocity
raw_accel = self.accel_controller.step(vel_error, self.dt)
self.lowpass_filter.filt(raw_accel)
accel = self.lowpass_filter.get()
brake = 0.0
throttle = 0.0
steer = self.yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_velocity)
if accel > 0.0:
throttle = self.throttle_controller.step(accel, self.dt)
if target_linear_velocity == 0 and current_velocity < 0.1:
throttle = 0
brake = 400
elif throttle < .1 and accel < 0:
throttle = 0
accel = max(accel, self.decel_limit)
brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * self.wheel_radius * 2
return throttle, brake, steer
class Controller(object):
def __init__(self,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle,
decel_limit,
accel_limit,
wheel_radius,
vehicle_mass
):
self.wheel_radius = wheel_radius
self.vehicle_mass = vehicle_mass
self.YawController = YawController(
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
# self.PID = PID(0.9, 0.0005, 0.075, decel_limit, accel_limit)
# self.PID = PID(4.0, 0.001, 0.05, decel_limit, accel_limit)
self.PID = PID(1.0, 0.0, 0.001, decel_limit, accel_limit)
self.low_pass_filer_vel = LowPassFilter(10.0, 1.0)
self.lastT = None
self.last_dbw_enabled = False
def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enabled):
if dbw_enabled is True and self.last_dbw_enabled is False:
#restart
self.lastT = None
self.PID.reset()
self.last_dbw_enabled = dbw_enabled
self.low_pass_filer_vel.filt(linear_velocity)
linear_velocity = self.low_pass_filer_vel.get()
velocity_error = linear_velocity - current_velocity
T = rospy.get_time()
dt = T - self.lastT if self.lastT else 0.05
self.lastT = T
a = self.PID.step(velocity_error,dt)
if a > 0.0:
throttle, brake = min(a, 1.0), 0.0
else:
throttle, brake = 0.0, self.wheel_radius * self.vehicle_mass * math.fabs(a)
steer = self.YawController.get_steering(linear_velocity, angular_velocity, current_velocity)
print('--- pid: {} {} {}'.format(velocity_error, throttle, brake))
return throttle, brake, steer
class Controller(object):
def __init__(self, car_params):
self.car_params = car_params
self.lpf_accel = LowPassFilter(tau = 0.5, ts = 0.02)
self.accel_pid = PID(kp = 0.4, ki = 0.1, kd = 0.0, mn = 0.0, mx = 1.0)
self.speed_pid = PID(kp = 2.0, ki = 0.0, kd = 0.0, mn = car_params.decel_limit,
mx = car_params.accel_limit)
self.yaw_control = YawController(car_params.wheel_base, car_params.steer_ratio, 4. * ONE_MPH, car_params.max_lat_accel, car_params.max_steer_angle)
self.prev_velocity = 0
def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, dbw_enabled, control_period):
# TODO: Change the arg, kwarg list to suit your needs
accel = (current_linear_velocity - self.prev_velocity) / control_period
self.lpf_accel.filt(accel)
self.prev_velocity = current_linear_velocity
vel_error = proposed_linear_velocity - current_linear_velocity
if abs(proposed_linear_velocity) < ONE_MPH:
self.speed_pid.reset()
accel_cmd = self.speed_pid.step(vel_error, control_period)
min_speed = ONE_MPH * 5
if proposed_linear_velocity < 0.01:
accel_cmd = min(accel_cmd,
-530. / self.car_params.vehicle_mass / self.car_params.wheel_radius)
elif proposed_linear_velocity < min_speed:
proposed_angular_velocity *= min_speed/proposed_linear_velocity
proposed_linear_velocity = min_speed
throttle = brake = steer = 0
if dbw_enabled:
if accel_cmd >= 0:
throttle = self.accel_pid.step(accel_cmd - self.lpf_accel.get(),control_period)
else:
self.accel_pid.reset()
if (accel_cmd < -self.car_params.brake_deadband) or (proposed_linear_velocity < min_speed):
brake = -accel_cmd * self.car_params.vehicle_mass * self.car_params.wheel_radius
steer = self.yaw_control.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity)
rospy.loginfo('%s,%s,%s',throttle, brake, steer);
else :
self.accel_pid.reset()
self.speed_pid.reset()
# Return throttle, brake, steer
return throttle, brake, steer
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, brake_deadband,
accel_limit, decel_limit, vehicle_mass, wheel_radius, fuel_capacity, sample_freq):
# TODO: Implement
self.dt = 1.0/sample_freq
self.brake_deadband = brake_deadband
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.fuel_capacity = fuel_capacity
self.decel_limit = decel_limit
self.accel_limit = accel_limit
#initalize the YAW controller, accel controller, throttle controller and low pass filter
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
self.accel_controller = PID(kp=0.45, ki=0.02, kd=0.01, mn=decel_limit, mx=accel_limit)
self.throttle_controller = PID(kp=1.0, ki=0.001, kd=0.10, mn=0.0, mx=0.2)
self.lowpass_filter = LowPassFilter(0.15, self.dt)
def control(self, dbw_enabled, target_linear_velocity, target_angular_velocity, current_velocity):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
#check if dbw enabled then no need to compute the brake, steering and throttle values, return 0
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
vel_error = target_linear_velocity - current_velocity
raw_accel = self.accel_controller.step(vel_error, self.dt)
self.lowpass_filter.filt(raw_accel)
accel = self.lowpass_filter.get()
brake = 0.0
throttle = 0.0
#Get sterring values after getting angular and current velocity
steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_velocity)
if accel > 0.0:
throttle = self.throttle_controller.step(accel, self.dt)
if target_linear_velocity == 0 and current_velocity < 0.1:
throttle = 0
# To prevent Carla from moving requires about 700 Nm of torque
brake = 400
elif throttle < .1 and accel < 0:
throttle = 0
accel = max(accel, self.decel_limit)
brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius * 2
return throttle, brake, steer
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, brake_deadband, accel_limit, decel_limit,
vehicle_mass, wheel_radius, fuel_capacity, sample_freq):
self.dt = 1.0 / sample_freq
self.brake_deadband = brake_deadband
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.fuel_capacity = fuel_capacity
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.accel_controller = PID(kp=0.45,
ki=0.02,
kd=0.01,
mn=decel_limit,
mx=accel_limit)
self.throttle_controller = PID(kp=1.0,
ki=0.001,
kd=0.10,
mn=0.0,
mx=1.0)
self.lowpass_filter = LowPassFilter(0.15, self.dt)
def control(self, target_linear_velocity, target_angular_velocity,
current_velocity):
vel_error = target_linear_velocity - current_velocity
raw_accel = self.accel_controller.step(vel_error, self.dt)
self.lowpass_filter.filt(raw_accel)
accel = self.lowpass_filter.get()
brake = 0.0
throttle = 0.0
steer = self.yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_velocity)
if accel > 0.0:
throttle = self.throttle_controller.step(accel, self.dt)
if accel <= 0.0:
self.throttle_controller.reset()
if abs(accel) > self.brake_deadband:
# Assumes wheel is point mass
brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * self.wheel_radius
return throttle, brake, steer
class Controller(object):
# constructor that takes all the car's caracteristics
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, decel_limit, accel_limit):
# TODO: Implement
# use the provided YawController and initialize it
self.YawController = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
# Setup the PID controller and the lowpass
# kp, ki and kd were found using trial and error method from the semester project
self.PID = PID(4.0, 0.001, 0.05, decel_limit, accel_limit)
self.low_pass_filer_vel = LowPassFilter(10.0, 1.0)
self.lastTime = None
self.last_dbw_enabled = False
#Control function
def control(self, linear_velocity, angular_velocity, current_velocity,
dbw_enabled):
# take care of the dbw_enabled. n case it is turned down, reset the PID.
if dbw_enabled is True and self.last_dbw_enabled is False:
#restart
self.lastT = None
self.PID.reset()
self.last_dbw_enabled = dbw_enabled
self.low_pass_filer_vel.filt(linear_velocity)
linear_velocity = self.low_pass_filer_vel.get()
velocity_error = linear_velocity - current_velocity
T = rospy.get_time()
dt = T - self.lastTime if self.lastTime else 0.05
self.lastTime = T
a = self.PID.step(velocity_error, dt)
if a > 0.0:
throttle, brake = a, 0.0
else:
throttle, brake = 0.0, math.fabs(a)
steer = self.YawController.get_steering(linear_velocity,
angular_velocity,
current_velocity)
# print('--- pid: {} {}'.format(velocity_error, a))
return throttle, brake, steer
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, accel_limit, decel_limit, loop_frequency,
vehicle_mass, wheel_radius):
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_steer_angle = max_steer_angle
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.steering_controller = YawController(wheel_base, steer_ratio,
min_speed, max_lat_accel,
max_steer_angle)
self.throttle_controller = PID(0.15,
0.0,
0.09,
mn=decel_limit,
mx=accel_limit)
self.low_pass_filter = LowPassFilter(12.0, 1)
self.last_timestamp = None
def control(self, target_angular_velocity, target_linear_velocity,
current_angular_velocity, current_linear_velocity,
dbw_enabled):
# Return throttle, brake, steer
if target_linear_velocity < 0.5 or not dbw_enabled:
self.reset()
self.last_timestamp = rospy.Time.now()
return 0., 35., 0.
steer = self.steering_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_linear_velocity)
throttle = 0.
brake = 0.
current_timestamp = rospy.Time.now()
if self.last_timestamp != None:
dt = (current_timestamp - self.last_timestamp).nsecs / 1e9
cte = target_linear_velocity - current_linear_velocity
acceleration = self.throttle_controller.step(cte, dt)
filtvalue = self.low_pass_filter.filt(acceleration)
if self.low_pass_filter.ready:
acceleration = self.low_pass_filter.get()
if acceleration >= 0:
throttle = acceleration
brake = 0.
else:
brake = self.vehicle_mass * abs(
acceleration) * self.wheel_radius
throttle = 0.
self.last_timestamp = current_timestamp
rospy.loginfo(
'SENDING - [throttle,brake,steer]:[{:.4f},{:.4f},{:.4f}], [cA,cL]:[{:.4f},{:.4f}]m [tA, tL]:[{:.4f},{:.4f}]'
.format(throttle, brake, steer, current_angular_velocity,
current_linear_velocity, target_angular_velocity,
target_linear_velocity))
return throttle, brake, steer
def reset(self):
"""
Reset the controller's state.
"""
self.throttle_controller.reset()
self.low_pass_filter = LowPassFilter(12.0, 1)
class Controller(object):
# TODO: Implement
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)
# PID hyperparameters
kp = 0.3
ki = 0.1
kd = 0
# Throttle values min, max
mn = 0.
mx = 0.2
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Low pass filter
tau = 0.5 # cutoff-frequence
ts = 0.02 # sample time = 50 Hz
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)
if IS_DEBUG:
rospy.loginfo("Angular vel: {0}".format(angular_vel))
rospy.loginfo("Target vel: {0}".format(linear_vel))
rospy.loginfo("Target ang vel: {0}".format(angular_vel))
rospy.loginfo("Current vel: {0}".format(current_vel))
rospy.loginfo("Filtered vel: {0}".format(self.vel_lpf.get()))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 700 # Nm --> needed to hold car in place
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit) # limit brake value
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
if IS_DEBUG:
rospy.logwarn("vel error: {0}, throttle: {1}, brake: {2}".format(
vel_error, throttle, brake))
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
self.last_time = rospy.get_time()
self.yaw_control = yaw_controller.YawController(
kwargs['wheel_base'], kwargs['steer_ratio'], ONE_MPH,
kwargs['max_lat_accel'], kwargs['max_steer_angle'])
self.brake_deadband = kwargs['brake_deadband']
self.fuel_capacity = kwargs['fuel_capacity']
self.vehicle_mass = kwargs['vehicle_mass']
self.decel_limit = kwargs['decel_limit']
self.accel_limit = kwargs['accel_limit']
self.wheel_radius = kwargs['wheel_radius']
# Throttle PID and filter
self.throttle_PID = pid.PID(KP_accel, KI_accel, KD_accel, 0, 1)
self.throttle_lpf = LowPassFilter(0.02, 0.03)
# Steering filter
self.steer_lpf = LowPassFilter(0.015, 0.01)
def control(self, desired_velocity_x, desired_vel_angular_z,
current_velocity_x, dbw_enabled):
actual_time = rospy.get_time()
throttle = 0
brake = 0
steer = self.yaw_control.get_steering(desired_velocity_x,
desired_vel_angular_z,
current_velocity_x)
# Feed steering filter
steer = self.steer_lpf.filt(steer)
if not dbw_enabled:
# Reset Throttle PID if dbw is not enable
self.throttle_PID.reset()
#Update timestamp
self.last_time = actual_time
return 0., 0., 0.
# Calculate error to correct
diff_velocity = desired_velocity_x - current_velocity_x
diff_time = actual_time - self.last_time
self.last_time = actual_time
#Limit the new acceleration value accordingly
new_accel = min(max(self.decel_limit, diff_velocity), self.accel_limit)
# Feed throttle filter
self.throttle_lpf.filt(new_accel)
throttle = self.throttle_PID.step(self.throttle_lpf.get(), diff_time)
# If the difference is positive, them we accelerate
if throttle > 0:
brake = 0
# If the difference is negative, them we deacelerate
else:
throttle = 0
# Torque = F * radius -> F = mass * acceleration. Include mass of fuel
brake = abs(new_accel *
(self.vehicle_mass + self.fuel_capacity * GAS_DENSITY +
DRIVER_WEIGHT) * self.wheel_radius)
rospy.loginfo("DV: " + str(desired_velocity_x) + " DA: " +
str(desired_vel_angular_z) + " CV: " +
str(current_velocity_x) + " new_accel: " +
str(new_accel))
rospy.loginfo("Steer: " + str(steer) + " Throttle: " + str(throttle) +
" Brake: " + str(brake))
return throttle, brake, steer
class Controller(object):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
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, 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,
min_speed=0.1,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
# Throttle PID controller parameters
# Tuned using Ziegler-Nichols method, temporarily using mx=1.0
# see https://en.wikipedia.org/wiki/PID_controller#Ziegler%E2%80%93Nichols_method
# Which gives the parameters Ku=2.1, Tu=1.4s
# Which in turn results in:
kp = 1.26
ki = 1.8
kd = 0.294
# Throttle range, 0 is minimum. 0.2 max for safety and comfort, real max is 1.0
mn = 0.0
mx = 0.2
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Lowpass filter for measured velocity, the sensor is noisy
tau = 0.5 # 1/(2*pi*tau) = cutoff frequency
ts = 0.02 # Sample time at 50 Hz
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.0, 0.0
if DEBUG_LOG:
rospy.logwarn("Angular vel: {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()))
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 we want to stop and are not going too fast, brake with known-good torque for staying still
if linear_vel == 0.0 and current_vel < 0.1:
throttle = 0
brake = 700 # Nm - according to Slack comments, minimum 550 Nm required, but use 700 to be safer
# If no/low throttle was indicated and we want to slow down (decelerate), give zero throttle and brake
elif throttle < 0.1 and vel_error < 0.0:
throttle = 0.0
decel = max(vel_error,
self.decel_limit) # decel_limit is negative as well
brake = abs(
decel
) * self.vehicle_mass * self.wheel_radius # Braking torque in Nm
# return 1., 0., 0. # For debugging only, full gas ahead!
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):
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.last_velocity = 0.
self.lpf_fuel = LowPassFilter(60.0, 0.1)
self.lpf_accel = LowPassFilter(0.5, 0.02)
#self.lpf_angv = LowPassFilter(1.0, 1)
self.accel_pid = PID(0.4, 0.1, 0.0, 0.0, 1.0)
self.speed_pid = PID(2.0, 0.0, 0.0, self.decel_limit, self.accel_limit)
self.yaw_control = YawController(wheel_base, steer_ratio, 4. * ONE_MPH,
max_lat_accel, max_steer_angle)
self.last_ts = None
def set_fuel(self, level):
self.lpf_fuel.filt(level)
def get_vehicle_mass(self):
return self.vehicle_mass + \
self.lpf_fuel.get() / 100.0 * self.fuel_capacity * GAS_DENSITY
def time_elasped(self, msg=None):
now = rospy.get_time()
if self.last_ts is None:
self.last_ts = now
elasped, self.last_ts = now - self.last_ts, now
return elasped
def control(self, current_velocity, dbw_enabled, linear_velocity,
angular_velocity):
#rospy.logwarn("Current Velocity = %f, Linear Velocity = %f, Angular Velocity = %f", current_velocity, linear_velocity, angular_velocity)
time_elasped = self.time_elasped()
given_av = angular_velocity
if time_elasped > 1. / 5 or time_elasped < 1e-4:
self.speed_pid.reset()
self.accel_pid.reset()
self.last_velocity = current_velocity
return 0., 0., 0.
vehicle_mass = self.get_vehicle_mass()
vel_error = linear_velocity - current_velocity
#rospy.logwarn("Velocity Error : %s",vel_error)
if abs(linear_velocity) < ONE_MPH:
self.speed_pid.reset()
accel_cmd = self.speed_pid.step(vel_error, time_elasped)
#rospy.logwarn("Velocity step : %s",accel_cmd)
min_speed = ONE_MPH * 5
if linear_velocity < 0.01:
#rospy.logwarn("Current Velocity = %f, Linear Velocity = %f, Angular Velocity = %f", current_velocity, linear_velocity, angular_velocity)
#rospy.logwarn("Velocity Error : %s",vel_error)
#rospy.logwarn("Velocity step Before: %s",accel_cmd)
accel_cmd = min(accel_cmd,
-530. / vehicle_mass / self.wheel_radius)
#rospy.logwarn("computed accel_cmd : %s",accel_cmd)
#rospy.logwarn("Velocity step After: %s",accel_cmd)
elif linear_velocity < min_speed:
angular_velocity *= min_speed / linear_velocity
linear_velocity = min_speed
#rospy.logwarn("updated angular_velocity : %s", angular_velocity)
#rospy.logwarn("updated linear velocity: %s",linear_velocity)
accel = (current_velocity - self.last_velocity) / time_elasped
#rospy.logwarn("Current accel = %f, Last = %f, New = %f", accel, self.last_velocity, current_velocity)
self.lpf_accel.filt(accel)
self.last_velocity = current_velocity
throttle, brake, steering = 0., 0., 0.
if dbw_enabled:
if accel_cmd >= 0:
throttle = self.accel_pid.step(
accel_cmd - self.lpf_accel.get(), time_elasped)
else:
self.accel_pid.reset()
if (accel_cmd < -self.brake_deadband) or \
(linear_velocity < min_speed):
brake = -accel_cmd * vehicle_mass * self.wheel_radius
#angular_velocity_filt = self.lpf_angv.filt(angular_velocity)
steering = self.yaw_control.get_steering(linear_velocity,
angular_velocity,
current_velocity)
else:
self.speed_pid.reset()
self.accel_pid.reset()
#rospy.logwarn("Throttle = %f, Brake = %f, Steering = %f" , throttle, brake, steering)
#rospy.logwarn("given_av = %f, comp_av = %f, filt_av = %f, Steering = %f" , given_av, angular_velocity, angular_velocity_filt, steering)
return throttle, brake, steering
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo("initing dbw_node")
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
self.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)
# self.prev_steer_val = None
# self.prev_steer_val_set = False
# self.tl_distance = -1
# self.prev_tl_distance = -1
# self.red_tl = True
# self.tl_count = 0
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/tl_distance', Float32, self.tl_distance_cb)
rospy.Subscriber('/go_to_stop_state', Bool, self.go_to_stop_cb)
self.dbw = False
self.angular_velocity_filter = LowPassFilter(.90, 1)
self.velocity_filter = LowPassFilter(.9, 1)
self.twist_yaw_filter = LowPassFilter(.2, .96)
self.twist_velocity_filter = LowPassFilter(.96, .9)
self.steer_filter = LowPassFilter(.2, .90)
#self.p_v = [1.187355162, 0.044831144, 0.00295747] # v1.3
#self.p_v = [1.325735147117472, 0.06556341512981727, 0.013549012506233077] # v1.4
#self.p_v = [1.9285529383307387, 0.0007904838169666957, 0.019058015342866958]
#self.p_v = [1.181681729, 0.084559526, 0.021058816] v.1.5
#self.p_v = [1.0671285679286078, 0.0011578159618311297, 0.011254946679196659] ## noise 0, 1.
self.p_v = [0.9999999999, 0.0, 0.0] ## noise 0
#self.p_v = [0.923685948, 0.004035275, -0.000129007] ## noise 0.05
#self.p_v = [2.8144929000000034, 0.3190704767458041, 0.04469070609966564] # new values based on new brake system
self.pidv = pid.PID(self.p_v[0], self.p_v[1], self.p_v[2])
self.throttle = 0.
min_speed = .01
# At high speeds, a multiple of 1.2 seems to work a bit
# better than 1.0
self.yaw_controller = YawController(wheel_base, 1.2 * steer_ratio,
min_speed, 8 * max_lat_accel,
max_steer_angle)
# contol if the car is going to stop
self.go_to_stop = False
self.loop()
'''
/twist_cmd
Type: geometry_msgs/TwistStamped
geometry_msgs/TwistStampedstd_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
'''
def go_to_stop_cb(self, msg):
self.go_to_stop = msg.data
def tl_distance_cb(self, msg):
# self.tl_distance = msg.data
# if self.prev_tl_distance == msg.data == -1:
# self.tl_count += 1 % 1000
# if self.tl_count > 10: self.red_tl = False
# else:
# self.tl_count = 0
# self.prev_tl_distance = msg.data
# self.red_tl = True
pass
def twist_cmd_cb(self, msg):
seq = msg.header.seq
(x, y, yaw) = twist_to_xyy(msg)
# rospy.loginfo("twist_cmd_cb %d", seq)
#print("twist linear: [%f, %f, %f]" % (x, y, yaw))
self.twist_yaw_filter.filt(yaw)
# if self.red_tl == True:
# vtwist = self.twist_velocity_filter.filt(0.1)
# else:
vtwist = self.twist_velocity_filter.filt(x)
# calculate error between desired velocity and current velocity
e = vtwist - self.velocity_filter.get()
# feed pid controller with a dt of 0.033
self.throttle = self.pidv.step(e, 0.03)
if self.throttle < -3.0:
rospy.logerr("Resetting PID !!!")
self.pidv.reset()
self.throttle = self.pidv.step(e, 0.03)
if msg.header.seq % 5 == 0:
ts = msg.header.stamp.secs + 1.e-9 * msg.header.stamp.nsecs
# rospy.loginfo("tcc %d %f %f %f %f", seq, ts, x, yaw, self.twist_yaw_filter.get())
pass
'''
/vehicle/dbw_enabled
Type: std_msgs/Bool
'''
def dbw_enabled_cb(self, msg):
rospy.loginfo("dbw_enabled_cb %s", msg.data)
self.dbw = msg.data
'''
/current_velocity
Type: geometry_msgs/TwistStamped
'''
def current_velocity_cb(self, msg):
seq = msg.header.seq
(x, y, yaw) = twist_to_xyy(msg)
# rospy.loginfo("current_velocity_cb %i", seq)
# factor = .90
# if yaw != 0.:
# self.filtered_angular_velocity = factor*self.filtered_angular_velocity + (1-factor)*yaw
# self.filtered_velocity = factor*self.filtered_velocity + (1-factor)*x
self.angular_velocity_filter.filt(yaw)
self.velocity_filter.filt(x)
if msg.header.seq % 5 == 0:
ts = msg.header.stamp.secs + 1.e-9 * msg.header.stamp.nsecs
# ts seems to always be 0.
yaw_per_meter = self.angular_velocity_filter.get(
) / self.velocity_filter.get()
# rospy.loginfo("cv %d %f %f %f %f %f", seq, self.velocity_filter.get(), x, self.angular_velocity_filter.get(), yaw, yaw_per_meter)
pass
def loop(self):
# rate = rospy.Rate(50) # 50Hz
rate = rospy.Rate(10) # 10Hz
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)
if self.dbw == True:
twist = self.twist_yaw_filter.get()
#steer = self.yaw_controller.get_steering(self.twist_velocity_filter.get(), twist, self.velocity_filter.get())
steer = self.yaw_controller.get_steering(
self.velocity_filter.get(), twist,
self.velocity_filter.get())
steer = self.steer_filter.filt(steer)
# define throttle command based on pid controller
throttle = brake = 0.
if self.go_to_stop == False:
if abs(self.throttle) > 1.:
if self.throttle > 0:
throttle = 1.0
else:
throttle = 0.
#brake = 1.0 * 10
v = self.velocity_filter.get()
t = 0.03 # time to brake
u = 0.70 # friction coeficcient
g = 9.8 # gravity force
D = (v * t) + (v**2 / (2. * u * 9.8))
# calculate work needed to stop
w = 0.5 * self.vehicle_mass * (v**2 - (v - t)**2)
# caculate the force
F = w / D
brake = (2 * u * F *
self.wheel_radius) + self.brake_deadband
elif self.throttle < 0:
throttle = 0
#brake = (abs(self.throttle) + self.brake_deadband) #* 10
#if brake > 1.: brake = 1. * 1000
v = self.velocity_filter.get()
t = 0.03 # time to brake
u = 0.70 # friction coeficcient
g = 9.8 # gravity force
D = (v * t) + (v**2 / (2. * u * 9.8))
# calculate work needed to stop
w = 0.5 * self.vehicle_mass * (v**2 - (v - t)**2)
# caculate the force
F = w / D
brake = (2 * u * F *
self.wheel_radius) + self.brake_deadband
else:
throttle = self.throttle + self.brake_deadband
if throttle > 1.: throttle = 1.
else:
v = self.velocity_filter.get()
t = 4.0 # time to brake
u = 0.70 # friction coeficcient
g = 9.8 # gravity force
D = (v * t) + (v**2 / (2. * u * 9.8))
# calculate work needed to stop
w = 0.5 * self.vehicle_mass * v**2
# caculate the force
F = w / D
brake = (2 * u * F *
self.wheel_radius) + self.brake_deadband
#brake *= 10.
rospy.loginfo(
"[%f] throttle: %f brake: %f steering angle: %f " %
(self.throttle, throttle, brake, steer))
# throttle is 0.35, which runs the car at about 40 mph.
# throttle of 0.98 will run the car at about 115 mph.
rospy.loginfo(
"[%f] throttle: %f brake: %f steering angle: %f " %
(self.throttle, throttle, brake, steer))
self.publish(throttle, brake, steer)
else:
self.pidv.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)
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. # min throttle value
mx = 0.2 # max throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1 / (2pi * tau) = cutoff freq
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)
rospy.loginfo('Angular Velocillty: {0}'.format(angular_vel))
rospy.loginfo('Target Velocity: {0}'.format(linear_vel))
rospy.loginfo('Target Angular Velocity: {0}'.format(angular_vel))
rospy.loginfo('Current Velocity: {0}'.format(current_vel))
rospy.loginfo('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. 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 # Torque N * m
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):
# 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.brake_torque_const = (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * self.wheel_radius
self.twist = None
self.current_velocity = 0.0
self.past_velocity = 0.0
self.current_acc = 0.0
self.lowpass_filter_acc = LowPassFilter(LPF_ACC_TAU, 1.0 / 50)
self.pid_vel = PID(PID_P_VEL, PID_I_VEL, PID_D_VEL, self.decel_limit,
self.accel_limit)
self.pid_acc = PID(PID_P_ACC, PID_I_ACC, PID_D_ACC, 0.0, 0.75)
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
MIN_SPEED, self.max_lat_accel,
self.max_steer_angle)
#rospy.logerr('twist_controller.self() done')
pass
def control(self):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
return 0., 0., 0.
def control(self, required_vel_linear, required_vel_angular,
current_velocity):
throttle, brake, steering = 0.0, 0.0, 0.0
velocity_error = required_vel_linear - current_velocity
#get current acc using lowpass filter
#rospy.logerr('twist_controller: velocity_error=%d',velocity_error)
acc_temp = (self.past_velocity - current_velocity) * 50.0
self.past_velocity = current_velocity
self.lowpass_filter_acc.filt(acc_temp)
self.current_acc = self.lowpass_filter_acc.get(
) #at first, current_acc=0
#rospy.logerr('twist_controller: current_acc=%d',self.current_acc)
required_acc = self.pid_vel.step(velocity_error, 1.0 / 50)
#rospy.logerr('twist_controller: required_acc=%d',required_acc)
if required_acc < 0:
throttle = 0
self.pid_acc.reset()
if -required_acc > self.brake_deadband:
#brake, but not over the brak deadband
brake = -self.decel_limit * self.brake_torque_const
else:
brake = -required_acc * self.brake_torque_const
else:
if required_acc > self.accel_limit:
throttle = self.pid_acc.step(
self.accel_limit - self.current_acc, 1.0 / 50)
else:
throttle = self.pid_acc.step(required_acc - self.current_acc,
1.0 / 50)
steering = self.yaw_controller.get_steering(required_vel_linear,
required_vel_angular,
current_velocity)
return throttle, brake, steering
def reset(self):
self.pid_vel.reset()
pass
class Controller(object):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, speed_kp, accel_kp, accel_ki, max_lat_accel,
accel_tau, 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.last_velocity = 0.
self.lpf_fuel = LowPassFilter(60.0, 0.1)
self.lpf_accel = LowPassFilter(accel_tau, 0.02)
self.accel_pid = PID(accel_kp, accel_ki, 0.0, 0.0, 1.0)
self.speed_pid = PID(speed_kp, 0.0, 0.0, self.decel_limit,
self.accel_limit)
self.yaw_control = YawController(wheel_base, steer_ratio, 4. * ONE_MPH,
max_lat_accel, max_steer_angle)
self.last_ts = None
def set_fuel(self, level):
self.lpf_fuel.filt(level)
def get_vehicle_mass(self):
return self.vehicle_mass + \
self.lpf_fuel.get() / 100.0 * self.fuel_capacity * GAS_DENSITY
def time_elasped(self, msg=None):
now = rospy.get_time()
if self.last_ts is None:
self.last_ts = now
elasped, self.last_ts = now - self.last_ts, now
return elasped
def control(self, linear_velocity, angular_velocity, current_velocity,
dbw_enbaled):
time_elasped = self.time_elasped()
if time_elasped > 1. / 5 or time_elasped < 1e-4:
self.speed_pid.reset()
self.accel_pid.reset()
self.last_velocity = current_velocity
return 0., 0., 0.
vehicle_mass = self.get_vehicle_mass()
vel_error = linear_velocity - current_velocity
if abs(linear_velocity) < ONE_MPH:
self.speed_pid.reset()
accel_cmd = self.speed_pid.step(vel_error, time_elasped)
min_speed = ONE_MPH * 5
if linear_velocity < 0.01:
accel_cmd = min(accel_cmd,
-530. / vehicle_mass / self.wheel_radius)
elif linear_velocity < min_speed:
angular_velocity *= min_speed / linear_velocity
linear_velocity = min_speed
accel = (current_velocity - self.last_velocity) / time_elasped
#rospy.logwarn("Current accel = %f, Last = %f, New = %f", accel, self.last_velocity, current_velocity)
self.lpf_accel.filt(accel)
self.last_velocity = current_velocity
throttle, brake, steering = 0., 0., 0.
if dbw_enbaled:
if accel_cmd >= 0:
throttle = self.accel_pid.step(
accel_cmd - self.lpf_accel.get(), time_elasped)
else:
self.accel_pid.reset()
if (accel_cmd < -self.brake_deadband) or \
(linear_velocity < min_speed):
brake = -accel_cmd * vehicle_mass * self.wheel_radius
steering = self.yaw_control.get_steering(linear_velocity,
angular_velocity,
current_velocity)
else:
self.speed_pid.reset()
self.accel_pid.reset()
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.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
# check is dbw node enabled, if not, reset throttle controller
if not dbw_enabled:
self.throttle_controller.reset()
return 0.0, 0.0, 0.0
# otherwise, get current velocity through low pass filter
current_vel = self.vel_lpf.filt(current_vel)
rospy.loginfo("Angular vel: {0}".format(angular_vel))
rospy.loginfo("Target velocity: {0}".format(linear_vel))
rospy.loginfo("Target angular velocity: {0}\n".format(angular_vel))
rospy.loginfo("Current velocity: {0}".format(current_vel))
rospy.loginfo("Filtered velocity: {0}".format(self.vel_lpf.get()))
# get steering from yaw_controller
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
# get velocity error
vel_error = linear_vel - current_vel
self.last_vel = current_vel
# get sample time from difference between rospy current time and last time
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
# Get sample time for each step of our throttle PID controller
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
# check if our linear velocity is 0 and we are going very slow, we should stop
if linear_vel == 0.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. accel ~ 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, steer
return throttle, brake, steering
class Controller(object):
def __init__(self, vehicle):
assert isinstance(vehicle, Vehicle)
self.vehicle = vehicle
self.accel_pid = PID(kp=.4, ki=.1, kd=0., mn=0., mx=1.)
self.speed_pid = PID(kp=2.,
ki=0.,
kd=0.,
mn=self.vehicle.deceleration_limit,
mx=self.vehicle.acceleration_limit)
self.last_velocity = 0.
self.curr_fuel = LowPassFilter(tau=60.0, ts=0.1)
self.lpf_accel = LowPassFilter(tau=.5, ts=0.02)
self.yaw_control = YawController(
wheel_base=self.vehicle.wheel_base,
steer_ratio=self.vehicle.steer_ratio,
min_speed=4. * ONE_MPH,
max_lat_accel=self.vehicle.max_lat_acceleration,
max_steer_angle=self.vehicle.max_steer_angle)
self.last_ts = None
def control(self, linear_velocity, angular_velocity, current_velocity):
time_elapsed = self.time_elapsed()
if time_elapsed > 1. / 5 or time_elapsed < 1e-4:
self.reset_pids()
self.last_velocity = current_velocity
return 0., 0., 0.
vehicle_mass = self.vehicle.gross_mass(curr_fuel=self.curr_fuel.get(),
fuel_density=GAS_DENSITY)
vel_error = linear_velocity - current_velocity
if abs(linear_velocity) < ONE_MPH:
self.speed_pid.reset()
accel_cmd = self.speed_pid.step(vel_error, time_elapsed)
min_speed = ONE_MPH * 5.
if linear_velocity < .01:
accel_cmd = min(accel_cmd,
-530. / vehicle_mass / self.vehicle.wheel_radius)
elif linear_velocity < min_speed:
angular_velocity *= min_speed / linear_velocity
linear_velocity = min_speed
accel = (current_velocity - self.last_velocity) / time_elapsed
_ = self.lpf_accel.filt(accel)
self.last_velocity = current_velocity
throttle, brake = 0., 0.
if accel_cmd >= 0:
throttle = self.accel_pid.step(accel_cmd - self.lpf_accel.get(),
time_elapsed)
else:
self.accel_pid.reset()
if (accel_cmd < -self.vehicle.brake_deadband) or (linear_velocity <
min_speed):
brake = -accel_cmd * vehicle_mass * self.vehicle.wheel_radius
steering = self.yaw_control.get_steering(linear_velocity,
angular_velocity,
current_velocity)
return throttle, brake, steering
def reset_pids(self):
self.accel_pid.reset()
self.speed_pid.reset()
def time_elapsed(self):
now = rospy.get_time()
if not self.last_ts:
self.last_ts = now
elapsed, self.last_ts = now - self.last_ts, now
return elapsed
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 # maximum throttle angle
mn = 0.0 # minimum throttle value
mx = 0.3 # 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):
# 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)
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.0 and current_vel < 0.1:
throttle = 0
brake = 700 # N.m - to hold the car in place
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,
min_speed, *args, **kwargs):
# 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.min_speed = min_speed
self.previous_linear_velocity = None
self.speed_controller = PID(2., 0., 0.) # values provided by DataSpeedInc for MKZ
self.accel_controller = PID(0.4, 0.1, 0., mn=0., mx=self.accel_limit) # values provided by DataSpeedInc for MKZ
self.accel_lowpassfilter = LowPassFilter(0.5, 0.2) # values provided by DataSpeedInc for MKZ
self.steering_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
pass
def control(self, target_linear_velocity, target_angular_velocity,
current_linear_velocity, current_angular_velocity, control_rate,
dbw_is_enabled, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
if self.previous_linear_velocity is None:
self.previous_linear_velocity = current_linear_velocity
else:
raw_accel = control_rate * (current_linear_velocity.x - self.previous_linear_velocity.x) # TODO: parameterize: 50 comes from the dbw_node Rate value
self.accel_lowpassfilter.filt(raw_accel)
velocity_error = target_linear_velocity.x - current_linear_velocity.x
accel = self.speed_controller.step(velocity_error, 1./ control_rate)
steer = 0.
throttle =0.
brake = 0.
if accel >= 0.:
throttle = self.accel_controller.step(accel-self.accel_lowpassfilter.get(), 1./ control_rate)
else: # TODO: Check if accel requested for braking is smaller than brake_deadband
accel = max(self.decel_limit, accel)
brake = -accel * self.vehicle_mass * self.wheel_radius #TODO: Add passanger weights and consider GAS weight
steer = self.steering_controller.get_steering(target_linear_velocity.x,target_angular_velocity.z,current_linear_velocity.x)
steer = max(-self.max_steer_angle, min(self.max_steer_angle, steer))
if not dbw_is_enabled:
self.accel_controller.reset()
self.speed_controller.reset()
self.previous_linear_velocity = current_linear_velocity
# 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):
# 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_vel = 0
self.last_time = rospy.get_time()
pass
def control(self, linear_vel, angular_vel, current_vel, curr_ang_vel, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
"""
Params:
linear_vel: proposed linear velocity
angular_vel: proposed angular velocity
current_vel: current linear velocity
curr_ang_vel: current angular velocity
dbw_enabled: DBW status
"""
if not dbw_enabled:
# Turn off/Reset PID controller
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
rospy.logwarn("Current Linear Velocity: {0}".format(current_vel))
rospy.logwarn("Filtered Velocity: {0}".format(self.vel_lpf.get()))
rospy.logwarn("Current Angular Velocity: {0}".format(curr_ang_vel))
rospy.logwarn("Target Linear Velocity: {0}".format(linear_vel))
rospy.logwarn("Target Angular Velocity {0}".format(angular_vel))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel, 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 = 400 # 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):
self.mass = rospy.get_param('~vehicle_mass', 1736.35)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
self.decel_limit = rospy.get_param('~decel_limit', -5)
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
self.wheel_base = rospy.get_param('~wheel_base', 2.8498)
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.fuel_density = rospy.get_param('~fuel_density', 2.858)
self.vehicle_mass = self.mass + self.fuel_capacity * self.fuel_density
self.vehicle_mass_wheel_radius = self.vehicle_mass * self.wheel_radius
self.throttle_pid = PID(.4, .1, 0., 0., 1.)
self.accel_pid = PID(2., 0., 0., self.decel_limit, self.accel_limit)
self.accel_f = LowPassFilter(.5, 0.02)
self.yaw_control = YawController(self.wheel_base, self.steer_ratio, 1,
self.max_lat_accel,
self.max_steer_angle)
self.min_speed = 2.
self.last_velocity = 0.0
self.last_time = 0
def control(self, linear_velocity, angular_velocity, current_velocity):
delta_t = self.delta_time()
lv_delta = linear_velocity - current_velocity
v_delta = current_velocity - self.last_velocity
self.last_velocity = current_velocity
if delta_t == 0:
self.reset()
return 0., 0., 0.
if abs(linear_velocity) < ONE_MPH: self.accel_pid.reset()
accel_cmd = self.accel_pid.step(lv_delta, delta_t)
if linear_velocity < .01:
accel_cmd = min(accel_cmd,
-530. / self.vehicle_mass / self.wheel_radius)
elif linear_velocity < self.min_speed:
angular_velocity *= self.min_speed / linear_velocity
linear_velocity = self.min_speed
throttle = self.get_trottle(accel_cmd, v_delta, delta_t)
brake = self.get_brake(accel_cmd, linear_velocity)
steering = self.yaw_control.get_steering(linear_velocity,
angular_velocity,
current_velocity)
return throttle, brake, steering
def delta_time(self):
""" Calculate step time delta current time - previous time. """
current_time = rospy.get_time()
if self.last_time == 0: self.last_time = current_time
delta_t = current_time - self.last_time
self.last_time = current_time
return delta_t
def get_brake(self, accel_cmd, linear_velocity):
if (accel_cmd < -self.brake_deadband) or (linear_velocity <
self.min_speed):
return -accel_cmd * self.vehicle_mass_wheel_radius
return 0.0
def get_trottle(self, accel_cmd, v_delta, delta_t):
accel = v_delta / delta_t
self.accel_f.filt(accel)
if accel_cmd >= 0:
return self.throttle_pid.step(accel_cmd - self.accel_f.get(),
delta_t)
self.throttle_pid.reset()
return 0.0
def reset(self):
self.accel_pid.reset()
self.throttle_pid.reset()
class Controller(object):
def __init__(self, *args, **kwargs):
self.vehicle_mass = args[0]
self.fuel_capacity = args[1]
self.brake_deadband = args[2]
self.decel_limit = args[3]
self.accel_limit = args[4]
self.wheel_radius = args[5]
self.wheel_base = args[6]
self.steer_ratio = args[7]
self.max_lat_accel = args[8]
self.max_steer_angle = args[9]
self.velocity_pid_ = PID(VELOCITY_Kp, VELOCITY_Ki, VELOCITY_Kd, -abs(self.decel_limit), abs(self.accel_limit))
self.accel_pid_ = PID(ACCEL_Kp, ACCEL_Ki, ACCEL_Kd)
self.steer_pid_ = PID(STEER_Kp, STEER_Ki, STEER_Ki, -abs(self.max_steer_angle), abs(self.max_steer_angle))
self.steering_cntrl = YawController(self.wheel_base, self.steer_ratio, MIN_SPEED, self.max_lat_accel, self.max_steer_angle)
self.accel_lpf_ = LowPassFilter(ACCEL_Tau, ACCEL_Ts)
def control(self, *args, **kwargs):
current_time = args[0]
last_cmd_time = args[1]
control_period = args[2]
twist_cmd = args[3]
current_velocity = args[4]
dbw_enabled = args[5]
brake_deadband = args[6]
cte = args[7]
if (current_time - last_cmd_time) > 10 * control_period:
self.velocity_pid_.reset()
self.accel_pid_.reset()
# assuming 2 pax in car plus full tank
vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + 2 * WEIGHT_PERSON
velocity_error = twist_cmd.twist.linear.x - current_velocity.twist.linear.x
if abs(twist_cmd.twist.linear.x) < 1.0 * ONE_MPH:
self.velocity_pid_.reset()
accel_cmd = self.velocity_pid_.step(velocity_error, control_period)
if twist_cmd.twist.linear.x <= 1e-2:
accel_cmd = min(accel_cmd, -530 / vehicle_mass / self.wheel_radius)
elif twist_cmd.twist.linear.x < MIN_SPEED:
twist_cmd.twist.angular.z = twist_cmd.twist.angular.z * MIN_SPEED / twist_cmd.twist.linear.x
twist_cmd.twist.linear.x = MIN_SPEED
if dbw_enabled:
if accel_cmd >= 0:
throttle_val = self.accel_pid_.step(accel_cmd - self.accel_lpf_.get(), control_period)
else:
throttle_val = 0
self.accel_pid_.reset()
if accel_cmd < -brake_deadband or twist_cmd.twist.linear.x < MIN_SPEED:
brake_val = -accel_cmd * vehicle_mass * self.wheel_radius
else:
brake_val = 0
steering_val = self.steering_cntrl.get_steering(twist_cmd.twist.linear.x, twist_cmd.twist.angular.z, current_velocity.twist.linear.x) \
+ self.steer_pid_.step(cte, control_period)
steering_val = max(-abs(self.max_steer_angle), min(abs(self.max_steer_angle), steering_val))
return throttle_val, brake_val, steering_val
else:
self.velocity_pid_.reset()
self.accel_pid_.reset()
return 0., 0., 0.
def filter_accel_value(self, value):
self.accel_lpf_.filt(value)
def get_filtered_accel(self):
return self.accel_lpf_.get()
class Controller(object):
def __init__(self, sampling_rate, 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.sampling_rate = sampling_rate
tau = LPF_ACCEL_TAU # 1/(2pi*tau) = cutoff frequency
self.sampling_time = 1. / self.sampling_rate
# rospy.logwarn('TwistController: Sampling rate = ' + str(self.sampling_rate))
# rospy.logwarn('TwistController: Sampling time = ' + str(self.sampling_time))
self.prev_vel = 0.0
self.current_accel = 0.0
self.acc_lpf = LowPassFilter(tau, self.sampling_time)
self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity *
GAS_DENSITY) * self.wheel_radius
# Initialise PID controller for speed control
self.pid_spd_ctl = PID(PID_SPDCTL_P, PID_SPDCTL_I, PID_SPDCTL_D,
self.decel_limit, self.accel_limit)
# second controller to get throttle signal between 0 and 1
self.accel_pid = PID(PID_ACC_P, PID_ACC_I, PID_ACC_D, 0.0, 0.8)
# Initialise Yaw controller for steering control
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# self.last_time = rospy.get_time()
def reset(self):
#self.accel_pid.reset()
self.pid_spd_ctl.reset()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
"""Returns (throttle, brake, steer), or None"""
throttle, brake, steering = 0.0, 0.0, 0.0
if not dbw_enabled:
self.reset()
return None
vel_error = linear_vel - current_vel
# calculate current acceleration and smooth using lpf
accel_temp = self.sampling_rate * (self.prev_vel - current_vel)
# update
self.prev_vel = current_vel
self.acc_lpf.filt(accel_temp)
self.current_accel = self.acc_lpf.get()
# use velocity controller compute desired accelaration
desired_accel = self.pid_spd_ctl.step(vel_error, self.sampling_time)
if desired_accel > 0.0:
if desired_accel < self.accel_limit:
throttle = self.accel_pid.step(
desired_accel - self.current_accel, self.sampling_time)
else:
throttle = self.accel_pid.step(
self.accel_limit - self.current_accel, self.sampling_time)
brake = 0.0
else:
throttle = 0.0
# reset just to be sure
self.accel_pid.reset()
if abs(desired_accel) > self.brake_deadband:
# don't bother braking unless over the deadband level
# make sure we do not brake to hard
if abs(desired_accel) > abs(self.decel_limit):
brake = abs(self.decel_limit) * self.brake_torque_const
else:
brake = abs(desired_accel) * self.brake_torque_const
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
# 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 < .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
#pass
# init in dbw:
# self.controller = Controller(vehicle_mass = vehicle_mass,
# fuel_capacity = fuel_capacity,
# brake_deadband = brake_deadband,
# decel_limit = decel_limit,
# accel_limit = accel_limit,
# wheel_radius = wheel_radius,
# wheel_base = wheel_base,
# steer_ratio = steer_ratio,
# max_lat_accel = max_lat_accel,
# max_steer_angle = max_steer_angle)
# usage in dbw:
#self.throttle, self.brake, self.steering = self.controller.control(self.current_vel,
# self.dbw_enabled,
# self.linear_vel,
# self.angular_vel)
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. # mimnimum throttle value
mx = 0.2 # maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# velocity is noisy, so use low-pass filter
tau = 0.5 # 1/ (2*pi*tau) = cutoff-frequence
ts = 0.02 # sample time = 50 Hz
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, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
#return 1., 0., 0.
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.
# get lowpass-filtered velocity
current_vel = self.vel_lpf.filt(current_vel)
if IS_DEBUG:
rospy.loginfo("Angular vel: {0}".format(angular_vel))
rospy.loginfo("Target vel: {0}".format(linear_vel))
rospy.loginfo("Target ang vel: {0}".format(angular_vel))
rospy.loginfo("Current vel: {0}".format(current_vel))
rospy.loginfo("Filtered vel: {0}".format(self.vel_lpf.get()))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
# brake = 400 # Nm --> needed to hold car in place if we are at a light, resulting acceleration = -1 m/s^2
brake = 700 # Nm --> needed to hold car in place if we are at a light, resulting acceleration = -1 m/s^2
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit) # limit brake value
brake = abs(
decel
) * self.vehicle_mass * self.wheel_radius # torque = N * m = acceleration * mass * radius
if IS_DEBUG:
rospy.logwarn("vel error: {0}, throttle: {1}, brake: {2}".format(
vel_error, throttle, brake))
# problem with this controller:
# by the time, car is away from waypoint, waypoint_follower send new commands
# 1. waypoint_folloer: make sure, it will update every time; if not following waypoints, do update
# 2. change yaw-controller and add some damping terms (current_ang_vel vs target_ang_vel --> if too large...)
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
self.vehicle_mass = args[0]
self.fuel_capacity = args[1]
self.brake_deadband = args[2]
self.decel_limit = args[3]
self.accel_limit = args[4]
self.wheel_radius = args[5]
self.wheel_base = args[6]
self.steer_ratio = args[7]
self.max_lat_accel = args[8]
self.max_steer_angle = args[9]
self.velocity_pid = PID(VELOCITY_Kp, VELOCITY_Ki, VELOCITY_Kd,
-abs(self.decel_limit), abs(self.accel_limit))
self.accel_pid = PID(ACCEL_Kp, ACCEL_Ki, ACCEL_Kd)
self.steering_cntrl = YawController(self.wheel_base, self.steer_ratio,
MIN_SPEED, self.max_lat_accel,
self.max_steer_angle)
self.accel_lpf = LowPassFilter(ACCEL_Tau, ACCEL_Ts)
#pass
def control(self, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
#return 1., 0., 0.
current_time = args[0]
last_cmd_time = args[1]
control_period = args[2]
twist_cmd = args[3]
current_velocity = args[4]
dbw_enabled = args[5]
if (current_time - last_cmd_time) > 10 * control_period:
self.velocity_pid.reset()
self.accel_pid.reset()
vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + 2 * WEIGHT_PERSON
velocity_error = twist_cmd.twist.linear.x - current_velocity.twist.linear.x
if abs(twist_cmd.twist.linear.x) < (1.0 * ONE_MPH):
self.velocity_pid.reset()
accel_cmd = self.velocity_pid.step(velocity_error, control_period)
if twist_cmd.twist.linear.x <= 1e-2:
accel_cmd = min(accel_cmd, -530 / vehicle_mass / self.wheel_radius)
elif twist_cmd.twist.linear.x < MIN_SPEED:
twist_cmd.twist.angular.z = twist_cmd.twist.angular.z * MIN_SPEED / twist_cmd.twist.linear.x
twist_cmd.twist.linear.x = MIN_SPEED
if dbw_enabled:
if accel_cmd >= 0:
throttle_val = self.accel_pid.step(
accel_cmd - self.accel_lpf.get(), control_period)
else:
throttle_val = 0.0
self.accel_pid.reset()
if accel_cmd < -self.brake_deadband or twist_cmd.twist.linear.x < MIN_SPEED:
brake_val = -accel_cmd * vehicle_mass * self.wheel_radius
else:
brake_val = 0.0
steering_val = self.steering_cntrl.get_steering(
twist_cmd.twist.linear.x, twist_cmd.twist.angular.z,
current_velocity.twist.linear.x
) #+ STEER_Kp * (twist_cmd.twist.angular.z - current_velocity.twist.angular.z)
return throttle_val, brake_val, steering_val
else:
self.velocity_pid.reset()
self.accel_pid.reset()
return 0.0, 0.0, 0.0
def filter_accel_value(self, value):
self.accel_lpf.filt(value)
def get_filtered_accel(self):
return self.accel_lpf.get()
class Controller(object):
def __init__(self, *args, **kwargs):
rospy.loginfo('TwistController: Start init')
self.sampling_rate = kwargs["sampling_rate"]
self.decel_limit = kwargs["decel_limit"]
self.accel_limit = kwargs["accel_limit"]
# brake_deadband is the interval in which the brake would be ignored
# the car would just be allowed to slow by itself/coast to a slower speed
self.brake_deadband = kwargs["brake_deadband"]
self.vehicle_mass = kwargs["vehicle_mass"]
self.fuel_capacity = kwargs["fuel_capacity"]
self.wheel_radius = kwargs["wheel_radius"]
# bunch of parameters to use for the Yaw (steering) controller
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.delta_t = 1 / self.sampling_rate
self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity \
* GAS_DENSITY) * self.wheel_radius
self.past_vel_linear = 0.0
self.current_accel = 0.0
self.low_pass_filter_accel = LowPassFilter(LPF_ACCEL_TAU, self.delta_t)
# Initialise speed PID, with tuning parameters
# Will use this PID for the speed control
self.pid_vel_linear = PID(PID_VEL_P, PID_VEL_I, PID_VEL_D,
self.decel_limit, self.accel_limit)
# second controller to get throttle signal between 0 and 1
self.accel_pid = PID(PID_ACC_P, PID_ACC_I, PID_ACC_D, 0.0, 0.75)
# Initialise Yaw controller - this gives steering values using
# vehicle attributes/bicycle model
# Need to have some minimum speed before steering is applied
self.yaw_controller = YawController(
wheel_base=self.wheel_base,
steer_ratio=self.steer_ratio,
min_speed=5.0,
max_lat_accel=self.max_lat_accel,
max_steer_angle=self.max_steer_angle)
rospy.loginfo('TwistController: Complete init')
rospy.loginfo('TwistController: Steer ratio = ' +
str(self.steer_ratio))
def control(self, required_vel_linear, required_vel_angular,
current_vel_linear):
throttle, brake, steering = 0.0, 0.0, 0.0
# uncomment for debugging pruposes otherwise we write logs at 50 Hz
#rospy.loginfo('TwistController: Control call at ' + str(rospy.get_time()))
#rospy.loginfo('TwistController: ' + 'Desired linear/angular = ' + str(required_vel_linear) + ","
# + str(required_vel_angular) + ' Current = ' + str(current_vel_linear) + ',' + str(current_vel_angular))
# calculate the difference (error) between desired and current linear velocity
velocity_error = required_vel_linear - current_vel_linear
# calculate current acceleration and smooth using lpf
accel_temp = self.sampling_rate * (self.past_vel_linear -
current_vel_linear)
# update
self.past_vel_linear = current_vel_linear
self.low_pass_filter_accel.filt(accel_temp)
self.current_accel = self.low_pass_filter_accel.get()
# use velocity controller compute desired accelaration
desired_accel = self.pid_vel_linear.step(velocity_error, self.delta_t)
# TODO think about emergency brake command
if desired_accel > 0.0:
if desired_accel < self.accel_limit:
throttle = self.accel_pid.step(
desired_accel - self.current_accel, self.delta_t)
else:
throttle = self.accel_pid.step(
self.accel_limit - self.current_accel, self.delta_t)
brake = 0.0
else:
throttle = 0.0
# reset just to be sure
self.accel_pid.reset()
if abs(desired_accel) > self.brake_deadband:
# don't bother braking unless over the deadband level
# make sure we do not brake to hard
if abs(desired_accel) > abs(self.decel_limit):
brake = abs(self.decel_limit) * self.brake_torque_const
else:
brake = abs(desired_accel) * self.brake_torque_const
# steering - yaw controller takes desired linear, desired angular, current linear as params
#steering = required_vel_angular * self.steer_ratio
steering = self.yaw_controller.get_steering(required_vel_linear,
required_vel_angular,
current_vel_linear)
# uncomment for debugging
#if throttle <> 0.0:
# rospy.loginfo('TwistController: Accelerating = ' + str(throttle))
#if brake <> 0.0:
# rospy.loginfo('TwistController: Braking = ' + str(brake))
if abs(steering) <> 0.0:
#rospy.loginfo('TwistController: Steering = ' + str(steering))
rospy.loginfo('Veer: Steering = ' + str(steering) +
', required = ' + str(required_vel_angular))
return throttle, brake, steering
def reset(self):
self.pid_vel_linear.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
# Yaw controller
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# PID controller
# mn: minimum throttle
# mx: maximun throttle
self.pid = PID(kp=0.3, ki=0.1, kd=0.0, mn=0.0, mx=0.2)
# Low pass filter
# tau: cut-off frequency
# ts: sample time
self.lowpassfilter = LowPassFilter(tau=0.5, ts=0.02)
# Some 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.last_vel = self.lowpassfilter.get()
self.last_time = rospy.get_time()
def control(self, linear_velocity, angular_velocity, curr_linear_velocity,
dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# Reset if dbw not enabled
if not dbw_enabled:
self.pid.reset()
return 0.0, 0.0, 0.0
curr_linear_velocity = self.lowpassfilter.filt(curr_linear_velocity)
# Throttle control
vel_err = linear_velocity - curr_linear_velocity
self.last_vel = curr_linear_velocity
curr_time = rospy.get_time()
sample_time = curr_time - self.last_time
self.last_time = curr_time
throttle = self.pid.step(vel_err, sample_time)
# Brake control
brake = 0.0
# To hold the car from moving
if abs(linear_velocity
) < sys.float_info.epsilon and curr_linear_velocity < 0.1:
throttle = 0.0
brake = 700.0 # Nm of torque to hold the car
# Deceleration
elif throttle < 0.1 and vel_err < 0.0:
throttle = 0.0
decel = max(vel_err, self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius # Torque (Nm)
# Steering control
steering = self.yaw_controller.get_steering(linear_velocity,
angular_velocity,
curr_linear_velocity)
return throttle, brake, steering
