def __init__(
self,
throttle_kp,
throttle_ki,
throttle_kd,
max_speed,
accel_limit,
decel_limit,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle,
vehicle_mass,
wheel_radius,
**kwargs):
self.throttle_brake_controller = throttle_brake_controller.ThrottleBrakeController(
kp=throttle_kp,
ki=throttle_ki,
kd=throttle_kd,
max_speed=max_speed,
accel_limit=accel_limit,
decel_limit=decel_limit,
brake_coef=vehicle_mass * wheel_radius)
self.yaw_controller = yaw_controller.YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=min_speed,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.lowpass_filter = lowpass.LowPassFilter(kwargs.get('tau', 0.2),kwargs.get('ts', 0.1))
self.last_time = None
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 = yaw_controller.YawController(
wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
# min and max throttle
mn = 0.
mx = 0.2
self.throttle_controller = pid.PID(kp, ki, kd, mn, mx)
# 1/(2pi*tau) = cutoff freq
tau = 0.5
# sample time
ts = .02
self.vel_lpf = lowpass.LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
pass
def __init__(self, vehicle_mass, fuel_capacity, accel_limit, decel_limit,
wheel_base, wheel_radius, steer_ratio, max_lat_accel,
max_steer_angle, brake_deadband):
# TODO: Implement
self.velocity_controller = pid.PID(kp=K_P,
ki=K_I,
kd=K_D,
mn=decel_limit,
mx=accel_limit)
self.yaw_controller = yaw_controller.YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=1.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.yaw_filter = lowpass.LowPassFilter(YAW_TAU, YAW_TS)
# need to calculate torque
self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = wheel_radius
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
vehicle_mass,
fuel_capacity,
brake_deadband,
wheel_radius):
self.throttle_pid = pid.PID(kp=3, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.wheel_radius = wheel_radius
self.max_throttle = max_throttle
self.max_brake = max_brake
self.max_brake_torque = (vehicle_mass + fuel_capacity*GAS_DENSITY) * wheel_radius * max_brake
self.clk = rospy.get_time()
self.count = 0
def __init__(self, parmams):
self.vehicle_mass = parmams['vehicle_mass']
self.fuel_capacity = parmams['fuel_capacity']
self.brake_deadband = parmams['brake_deadband']
self.decel_limit = parmams['decel_limit']
self.accel_limit = parmams['accel_limit']
self.wheel_radius = parmams['wheel_radius']
self.wheel_base = parmams['wheel_base']
self.steer_ratio = parmams['steer_ratio']
self.max_lat_accel = parmams['max_lat_accel']
self.max_steer_angle = parmams['max_steer_angle']
self.pid_accel = PID.PID(0.3,
0.05,
0.3,
mn=self.decel_limit,
mx=self.accel_limit)
self.yaw_cont = yaw_controller.YawController(self.wheel_base,
self.steer_ratio, 0.1,
self.max_lat_accel,
self.max_steer_angle)
self.vel_LP = lowpass.LowPassFilter(0.5, 0.02)
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
wheel_radius,
vehicle_mass,
fuel_capacity):
self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.clk = rospy.get_time()
self.max_acc = max_throttle
self.max_dec = max_brake
self.wheel_radius = wheel_radius
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.max_brk_trq = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius * self.max_dec # -1809 N*m
def __init__(self, *args, **kwargs):
# Timesteps
self.dt = 0.1 # 10Hz
self.prevtime = rospy.get_time()
# TODO: Pull PID Gains out to Launch Files
# TODO: Tune Coefficients
# Initialize Gains on PID Controllers
self.PIDCont_Thr = pid.PID(kp=1.0,ki=0.04,kd=0.1,mn= 0.0,mx=1.0)
self.PIDCont_Brk = pid.PID(kp=200.0,ki=0.00,kd=0.0,mn= 0.0,mx=10000.0)
if STEER_CTE:
# cross track error based steering
self.PIDCont_Str = pid.PID(kp=0.25,ki=0.075,kd=1.5,mn=-0.5,mx=0.5)
else:
# angular velocity error based steering
self.PIDCont_Str = pid.PID(kp=4.0,ki=0.5,kd=0.05,mn=-0.5,mx=0.5)
self.YawCont_Str = yc.YawController(wheel_base=2.8498, steer_ratio=14.8, min_speed=10.0, max_lat_accel=3.0, max_steer_angle=8.0)
# Initialize Low Pass Filters
self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
if STEER_CTE:
# cross track error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.7,ts=self.dt)
else:
# angular velocity error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt)
self.first = True # first pass flag
pass
def __init__(self, ego, **kwargs):
self.ego = ego
self.throttle_pid = pid.PID(THROTTLE_P_VAL,
THROTTLE_I_VAL,
THROTTLE_D_VAL,
mn=self.ego.decel_limit,
mx=self.ego.accel_limit)
self.yaw_controller = yaw_controller.YawController(
self.ego.wheel_base, self.ego.steer_ratio, self.ego.min_speed,
self.ego.max_lat_accel, self.ego.max_steer_angle)
def __init__(self, params):
self.yaw_controller = yaw_controller.YawController(
params['wheel_base'],
params['steer_ratio'],
params['min_speed'],
params['max_lat_accel'],
params['max_steer_angle'],
)
self.pid = pid.PID(params['kp'], params['ki'], params['kd'])
self.params = params
self.vel_linear_goal = None
self.vel_angular_goal = None
self.vel_linear_curr = None
def __init__(self, vehicle_mass, fuel_capacity, acceleration_limit, deceleration_limit,
wheel_base, wheel_radius, steer_ratio,
max_lat_acceleration, max_steer_angle, min_speed):
self.velocity_controller = pid.PID(kp=K_P, ki=K_I, kd=K_D, mn=deceleration_limit, mx=acceleration_limit)
self.yaw_controller = yaw_controller.YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=min_speed,
max_lat_accel=max_lat_acceleration,
max_steer_angle=max_steer_angle)
self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = wheel_radius
self.deceleration_limit = deceleration_limit
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 __init__(self, *args, **kwargs):
max_abs_angle = kwargs.get('max_steer_angle')
fuel_capacity = kwargs.get('fuel_capacity')
self.vehicle_mass = kwargs.get('vehicle_mass')
self.vehicle_mass = self.vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = kwargs.get('wheel_radius')
self.accel_limit = kwargs.get('accel_limit')
self.decel_limit = kwargs.get('decel_limit')
self.brake_deadband = kwargs.get('brake_deadband')
self.max_acceleration = kwargs.get('max_acceleration')
self.wheel_base = kwargs.get('wheel_base')
self.steer_ratio = kwargs.get('steer_ratio')
self.max_steer_angle = kwargs.get('max_steer_angle')
self.max_lat_accel = kwargs.get('max_lat_accel')
# PID controllers
if use_steering_pid:
# create lowpass filters
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.pid_steer = pid.PID(kp=S_KP,
ki=S_KI,
kd=S_KD,
mn=-max_abs_angle,
mx=max_abs_angle)
self.pid_velocity = pid.PID(kp=V_KP, ki=V_KI, kd=V_KD)
self.yaw_controller = yaw_controller.YawController(
self.wheel_base,
self.steer_ratio,
1.0, #0.0,
self.max_lat_accel,
self.max_steer_angle) # aLEX YAW CONTROLLER SET TO 0
# max torque (1.0 throttle) and max brake torque (deceleration lmt)
self.max_acc_torque = self.vehicle_mass * self.max_acceleration * self.wheel_radius
self.max_brake_torque = self.vehicle_mass * abs(
self.decel_limit) * self.wheel_radius
# 1st timestamp
self.last_time = rospy.get_time()
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel):
self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.clk = rospy.get_time()
def __init__(self, vehicle_mass, fuel_capacity, acceleration_limit,
deceleration_limit, wheel_base, wheel_radius, steer_ratio,
max_lat_acceleration, max_steer_angle, min_speed):
# rospy.logwarn("*** accel_limit {} ***".format(acceleration_limit))
kp, ki, kd = 2.0, 0.005, 0 # pid params
mn, mx = deceleration_limit, acceleration_limit # acceleration limit
tau = 0.5 # 1/(2*pi*tau) = cutoff frequency
ts = .02 # sample time
self.vel_lpf = LowPassFilter(tau, ts)
self.velocity_controller = PID(kp, ki, kd, mn, mx)
self.yaw_controller = yaw_controller.YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=min_speed,
max_lat_accel=max_lat_acceleration,
max_steer_angle=max_steer_angle)
self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = wheel_radius
self.deceleration_limit = deceleration_limit
