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