class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement min_speed = 0.1 self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = .02 # Sample time (50 hertz) # ts = .033 # Sample time (30 hertz) self.vel_lpf = LowPassFilter(tau, ts) self.ang_vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, linear_vel, angular_vel, current_vel, current_ang_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs if not dbw_enabled: # Reset throttle and steering self.throttle_controller.reset() self.yaw_controller.reset() return 0., 0., 0. # rospy.logwarn("Curr ang vel: {0}".format(current_ang_vel)) current_vel = self.vel_lpf.filt(current_vel) current_ang_vel = self.ang_vel_lpf.filt(current_ang_vel) # rospy.logwarn("Angular vel: {0}".format(angular_vel)) # rospy.logwarn("Target vel: {0}".format(linear_vel)) # rospy.logwarn("Current vel: {0}".format(current_vel)) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel, current_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) # Throttle to steering ratio function Reduce throttle for larger steerng # if steering is not None: # throttle = -0.0015625 * steering * steering + throttle brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 #N*m - to hold the car in place if we are stopped at a light. Accl - 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 # rospy.logwarn("Throttle: {0}".format(throttle)) # rospy.logwarn("Brake: {0}".format(brake)) # rospy.logwarn("Steering: {0}".format(steering)) # Return throttle, brake, steer # return 1., 0., 0. return throttle, brake, steering
class Controller(object): def __init__( self, dbw_parameters, # Set of base parameters ): self.is_initialized = False self.last_time = rospy.get_time() self.dbw_parameters = dbw_parameters self.steering_controller = YawController( wheel_base = self.dbw_parameters['wheel_base'], steer_ratio = self.dbw_parameters['steer_ratio'], min_speed = self.dbw_parameters['min_vehicle_speed'], max_lat_accel = self.dbw_parameters['max_lat_accel'], max_steer_angle = self.dbw_parameters['max_steer_angle']) self.low_pass_filter_vel = LowPassFilter(samples_num = LOW_PASS_FREQ_VEL * self.dbw_parameters['dbw_rate']) self.low_pass_filter_yaw = LowPassFilter(samples_num = LOW_PASS_FREQ_YAW * self.dbw_parameters['dbw_rate']) self.throttle_controller = PIDController( kp = 0.3, ki = 0.01, kd = 0.01, mn = self.dbw_parameters['vehicle_min_throttle_val'], mx = self.dbw_parameters['vehicle_max_throttle_val'], integral_period_sec = INTEGRAL_PERIOD_SEC) self.steering_controller2 = PIDController( kp = 3.0, ki = 0.0, kd = 1.0, mn = -self.dbw_parameters['max_steer_angle'], mx = self.dbw_parameters['max_steer_angle'], integral_period_sec = INTEGRAL_PERIOD_SEC) def control( self, target_dx, target_dyaw, current_dx, current_dyaw, dbw_status = True): throttle = 0 brake = 0 steering = 0 correct_data = False cur_time = rospy.get_time() current_dx_smooth = self.low_pass_filter_vel.filt(current_dx) target_dyaw_smooth = self.low_pass_filter_yaw.filt(target_dyaw) if not dbw_status: self.is_initialized = False else: if not self.is_initialized: self.reset() self.is_initialized = True dv = target_dx - current_dx_smooth dt = cur_time - self.last_time if dt >= MIN_TIME_DELTA: steering = self.steering_controller.get_steering(target_dx, target_dyaw_smooth, current_dx_smooth) + self.steering_controller2.step(target_dyaw_smooth, dt) steering = max(min(steering, self.dbw_parameters['max_steer_angle']), -self.dbw_parameters['max_steer_angle']) throttle = self.throttle_controller.step(dv, dt) if target_dx <= current_dx_smooth and current_dx_smooth <= self.dbw_parameters['min_vehicle_speed']: throttle = 0.0 brake = self.dbw_parameters['min_vehicle_break_torque'] elif throttle <= self.dbw_parameters['vehicle_throttle_dead_zone']: deceleration = abs(self.dbw_parameters['decel_limit'] * (throttle - self.dbw_parameters['vehicle_throttle_dead_zone']) / (self.dbw_parameters['vehicle_throttle_dead_zone'] - self.dbw_parameters['vehicle_min_throttle_val'])) throttle = 0.0 brake = self.dbw_parameters['vehicle_mass'] * self.dbw_parameters['wheel_radius'] * deceleration correct_data = True self.last_time = cur_time return throttle, brake, steering, correct_data def reset(self): self.steering_controller.reset() self.steering_controller2.reset() self.throttle_controller.reset()