def __init__(self, **ros_param): """ :param ros_param: Note: sample time (sec) is based on the dbw node frequency 50Hz low_pass filter: val = w * cur_val + (1 - w) * prev_val w is 0 ~ 1 """ # used ros_param self.vehicle_mass = ros_param['vehicle_mass'] # self.fuel_capacity = ros_param['fuel_capacity'] self.brake_deadband = ros_param['brake_deadband'] self.decel_limit = ros_param['decel_limit'] self.accel_limit = ros_param['accel_limit'] self.wheel_radius = ros_param['wheel_radius'] self.last_time = rospy.get_time() # low pass filter for velocity self.vel_lpf = LowPassFilter(0.5, .02) # Init yaw controller min_speed = 0.1 # I think min_speed self.steer_controller = YawController(min_speed, **ros_param) self.throttle_lpf = LowPassFilter(0.05, 0.02) # w = 0.28 # Init throttle PID controller # TODO: tweaking kp = 0.5 ki = 0.005 kd = 0.1 acc_min = 0. acc_max = 0.5 #self.accel_limit self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max)
def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lat_accel = kwargs['max_lat_accel'] self.max_steer_angle = kwargs['max_steer_angle'] self.min_speed = 0.0 self.yaw_contoller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) kp = 0.1 ki = 0.007 kd = 0.0 minth = 0.0 maxth = 0.2 self.throttle_contoller = PID(kp, ki, kd, minth, maxth) tau = 150 ts = 75 self.lpf_velocity = LowPassFilter(tau, ts) # self.lpf_steering = LowPassFilter(tau, ts) self.t0 = rospy.get_time()
def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min_speed max_lat_accel, max_steer_angle, ) # PID coefficients kp = 0.3 ki = 0.1 kd = 0.0 # For convenience (no real limits on throttle): mn_th = 0.0 # Minimal throttle mx_th = 0.2 # Maximal throttle self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th) tau = 0.5 # 1 / (2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.wheel_radius = wheel_radius self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.last_time = rospy.get_time()
def __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 = 1.0 * ONE_MPH self.pid = PID(2.0, 0.4, 0.1) self.lpf = LowPassFilter(0.5, 0.02) self.yaw = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.v_mass = vehicle_mass self.w_radius = wheel_radius self.d_limit = decel_limit self.last_time = None
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 = 1.0 * ONE_MPH self.pid = PID(2.0, 0.4, 0.1) self.lpf = LowPassFilter(0.5, 0.02) self.yaw = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.v_mass = vehicle_mass self.w_radius = wheel_radius self.d_limit = decel_limit self.last_time = None def control(self, proposed_v, proposed_omega, current_v): # Return throttle, brake, steer if self.last_time is None: throttle, brake, steer = 1.0, 0.0, 0.0 else: dt = time.time() - self.last_time # get throttle, if negative, change it to break error = proposed_v.x - current_v.x throttle = self.pid.step(error, time.time() - self.last_time) throttle = max(0.0, min(1.0, throttle)) if error < 0: # if we need to decelerate deceleration = abs(error) / dt if abs(deceleration ) > abs(self.d_limit) * 500: deceleration = self.d_limit * 500 longitudinal_force = self.v_mass * deceleration brake = longitudinal_force * self.w_radius throttle = 0 else: brake = 0 steer = self.yaw.get_steering(proposed_v.x, proposed_omega.z, current_v.x) self.last_time = time.time() # update time # rospy.logwarn('proposed v: {}'.format(proposed_v.x)) # rospy.logwarn('current v: {}'.format(current_v.x)) # rospy.logwarn('Error: {}'.format(proposed_v.x - current_v.x)) # rospy.logwarn('Throttle: {}'.format(throttle)) # rospy.logwarn('Brake: {}'.format(brake)) # rospy.logwarn('Steer: {}'.format(steer)) # rospy.logwarn('Speed: {}'.format(current_v)) # rospy.logwarn('') return throttle, brake, steer
def __init__(self, *args, **kwargs): vehicle_mass = kwargs['vehicle_mass'] fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] accel_limit = kwargs['accel_limit'] wheel_radius = kwargs['wheel_radius'] wheel_base = kwargs['wheel_base'] steer_ratio = kwargs['steer_ratio'] max_lat_accel = kwargs['max_lat_accel'] max_steer_angle = kwargs['max_steer_angle'] self.brake_tourque_const = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius self.current_dbw_enabled = False yaw_params = [wheel_base, steer_ratio, max_lat_accel, max_steer_angle] self.yaw_controller = YawController(*yaw_params) self.linear_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, accel_limit) self.tau_correction = 0.2 self.ts_correction = 0.1 self.low_pass_filter_correction = LowPassFilter(self.tau_correction, self.ts_correction) self.previous_time = None pass
def __init__( self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle, max_throttle): # Yaw controller to controller the turning self.vehicle_min_velocity = .1 self.yaw_controller = YawController( wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=self.vehicle_min_velocity, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) self.steering_controller = PID( kp=.0, ki=.001, kd=.1, min=-.2, max=.2) # Throttle controller self.throttle_controller = PID( kp=.8, ki=.001, kd=.1, min=0, max=max_throttle) # Remove high frequency noise on the velocity self.low_pass = LowPassFilter(tau=.5, ts=.02) # Assume the tank is full fuel_mass = fuel_capacity * GAS_DENSITY self.total_mass = vehicle_mass + fuel_mass self.decel_limit = decel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time()
class Controller(object): def __init__(self, *args, **kwargs): vehicle_mass = kwargs['vehicle_mass'] fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] accel_limit = kwargs['accel_limit'] wheel_radius = kwargs['wheel_radius'] wheel_base = kwargs['wheel_base'] steer_ratio = kwargs['steer_ratio'] max_lat_accel = kwargs['max_lat_accel'] max_steer_angle = kwargs['max_steer_angle'] self.brake_tourque_const = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius self.current_dbw_enabled = False yaw_params = [wheel_base, steer_ratio, max_lat_accel, max_steer_angle] self.yaw_controller = YawController(*yaw_params) self.linear_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, accel_limit) self.tau_correction = 0.2 self.ts_correction = 0.1 self.low_pass_filter_correction = LowPassFilter(self.tau_correction, self.ts_correction) self.previous_time = None pass def update_sample_step(self): current_time = rospy.get_time() sample_step = current_time - self.previous_time if self.previous_time else 0.05 self.previous_time = current_time return sample_step def control(self, linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity, angular_current, dbw_enabled, final_waypoint1, final_waypoint2, current_location): if (not self.current_dbw_enabled) and dbw_enabled: self.current_dbw_enabled = True self.linear_pid.reset() self.previous_time = None else: self.current_dbw_enabled = False linear_velocity_error = linear_velocity_setpoint - linear_current_velocity sample_step = self.update_sample_step() velocity_correction = self.linear_pid.step(linear_velocity_error, sample_step) velocity_correction = self.low_pass_filter_correction.filt(velocity_correction) if abs(linear_velocity_setpoint)<0.01 and abs(linear_current_velocity) < 0.3: velocity_correction = self.decel_limit throttle = velocity_correction brake = 0. if throttle < 0.: decel = abs(throttle) #[alexm]NOTE: let engine decelerate the car if required deceleration below brake_deadband brake = self.brake_tourque_const * decel if decel > self.brake_deadband else 0. throttle = 0. #[alexm]::NOTE this lowpass leads to sending both throttle and brake nonzero. Maybe it is better to filter velocity_correction #brake = self.low_pass_filter_brake.filt(brake) #steering = self.yaw_controller.get_steering_pid(angular_velocity_setpoint, angular_current, dbw_enabled) #steering = self.yaw_controller.get_steering_pid_cte(final_waypoint1, final_waypoint2, current_location, dbw_enabled) #[alexm]::NOTE changed static 10.0 to linear_current_velocity and surprisingly car behave better on low speeds. Need to look close to formulas... #PID also improves the same with the factor #moved factor into function because limits are checked in that function steering = self.yaw_controller.get_steering_calculated(linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity) return throttle, brake, steering
class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_speed, max_lat_accel, max_steer_angle, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius): self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.min_speed = min_speed 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.low_pass_steer_filter = LowPassFilter(0.3, 1.0) self.acceleration_controller = PID(0.19, 0.00005, 0.017, self.decel_limit, self.accel_limit) self.throttle_controller = PID(1.5, 0.00007, 0.01, 0.0, 0.5) self.steering_controller = PID(2.0, 0.0005, 0.1, -max_steer_angle, max_steer_angle) self.throttle_controller.reset() self.acceleration_controller.reset() self.lastControlTime = None self.dbw_status = True def control(self, linear_velocity, angular_velocity, current_velocity, dbw_status): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # control is taken by driver if not dbw_status: if self.dbw_status: self.throttle_controller.reset() self.acceleration_controller.reset() self.steering_controller.reset() self.dbw_status = False return None, None, None current_time = time.time() if self.lastControlTime is None: sample_time = 0.01 else: sample_time = current_time - self.lastControlTime self.lastControlTime = current_time throttle = 0.0 brake = 0.0 steering = self.steering_controller.step( self.yaw_controller.get_steering( abs(linear_velocity), self.low_pass_steer_filter.filt(angular_velocity), current_velocity), sample_time) acceleration = self.acceleration_controller.step( linear_velocity - current_velocity, sample_time) if (acceleration < -self.brake_deadband): brake = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * abs(acceleration) * self.wheel_radius throttle = self.throttle_controller.step( linear_velocity - current_velocity, sample_time) # Holding if (linear_velocity < STOP_THRESHOLD_VELOCITY and current_velocity < self.min_speed): brake = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * 0.3 * self.wheel_radius throttle = 0 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: change to using *args and **kwargs # Yaw Controller self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # PID Throttle controller kp = 0.3 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = 0.25 # Maximum throttle value tau = 0.5 # 1/(2pi * tau) = cutoff frequency ts = 0.02 # Sample time self.throttle_controller = PID(kp, ki, kd, mn, mx) 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 # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("Angular velocity: {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())) 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 ## Special Cases if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2 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 1., 0., 0. return throttle, brake, steering
class Controller(object): def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, accel_limit, vehicle_total_mass, brake_deadband, wheel_radius): # TODO: Implement # pass # Variables for Yaw, LowPass and PID controller min_speed = 0.1 Kp = 1.1 Ki = 0.010 Kd = 0.005 pid_cmd_range = 4 filter_tau = 0.0 filter_ts = 0.8 # YawController # def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # LowPass Filter # def __init__(self, tau, ts) self.low_pass_filter = LowPassFilter(filter_tau, filter_ts) # PID Controller for the target velocity #def __init__(self, kp, ki, kd, mn=MIN_NUM, mx=MAX_NUM) self.pid_controller = PID(Kp, Ki, Kd, decel_limit, accel_limit) self.vehicle_total_mass = vehicle_total_mass self.brake_deadband = brake_deadband self.wheel_radius = wheel_radius self.t_start = time.time() self.dt = 0.0 self.feed_forward_brake_control = True def control(self, current_velocity, linear_velocity, angular_velocity): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # return 1., 0., 0. # Check if all required parameters are set if not (self.vehicle_total_mass and self.brake_deadband and self.wheel_radius): rospy.logerror('vehicle parameters not set') # Apply the filter to the angular velocity angular_velocity = self.low_pass_filter.filt(angular_velocity) # Compute the steering angle steer = self.yaw_controller.get_steering(linear_velocity, angular_velocity, current_velocity) # Compute the throttle command throttle_cmd = 0 velocity_error = linear_velocity - current_velocity if self.dt: throttle_cmd = self.pid_controller.step(velocity_error, self.dt) self.dt = time.time() - self.t_start self.t_start += self.dt throttle = throttle_cmd brake = 0.0 rospy.logdebug('throttle = %.2f, T = %.2f, B = %.2f, S = %.2f (BAND: %.2f)', throttle_cmd, throttle, brake, steer, self.brake_deadband) return throttle, brake, steer def reset(self): self.pid_controller.reset()
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): 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 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() # TODO: Implement def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 1000 elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, accel_limit, brake_deadband, decel_limit, fuel_capacity, max_lat_accel, max_steer_angle, steer_ratio, vehicle_mass, wheel_base, wheel_radius): ######################################################################### #### PID Controller parameters #### Kp = 0 Ki = 0 Kd = 0 mn = 0 mx = 0 #### Initalizing the PID controller using Controller gains, minimum and maximum throttle value #### self.PID_controller = PID(Kp, Ki, Kd, mn, mx) #### Low Pass Filter parameters ### tau = 0 ts = 0 #### Intialize the low pass filter using time constant (tau) and sample time #### self.Low_pass_filter = LowPassFilter(tau, ts) #### Initialize the yaw controller using the wheel_base, steer_ratio, min_speed, max_lat_accel and max_steer_angle #### self.Yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) #### Vehicle parameters and contraints #### self.accel_limit = accel_limit self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius #### Time Initialization #### previous_time = rospy.get_time() Last_throttle = 0 ######################################################################### pass def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enabled): ######################################################################### #### Resetting the PID integrator error when the driver controls the vehicle #### if not dbw_enabled: self.PID_controller.reset() #### Filtering the noise in velcoity readings #### current_velocity = self.Low_pass_filter.filt(current_velocity) #### Throttle Control #### ## Error calculation ## Velocity_error = linear_velocity - current_velocity ## Sample time calculation ## current_time = rospy.get_time() sample_time = current_time - previous_time previous_time = current_time ## Throttle value from PID controller ## Throttle = self.PID_controller.step(Velocity_error, sample_time) if (Throttle - Last_throttle) > 0.05: Throttle = Last_throttle + 0.05 Last_throttle = Throttle #### Steering Control #### Steering = self.Yaw_controller.get_steering(linear_velocity, angular_velocity, current_velocity) #### Brake Control #### ## For normal case, brake torque applied is equal to 0 ## Brake = 0 ## For stopping the vehicle at low speed ## if linear_velocity == 0 and current_velocity < 0.1: Throttle = 0 Brake = MAX_BRAKE ## For releasing throttle when current velocity becomes greater than required velocity ## elif Throttle > 0.1 and Velocity_error < 0: Throttle -= 0.01 ## For heavy braking when current velocity becomes greater than required velocity and throttle value is small ## elif Throttle < 0.1 and Velocity_error < 0: Throttle = 0 Deceleration = abs(max(Velocity_error, self.decel_limit)) Brake = min(MAX_BRAKE, (Deceleration * vehicle_mass * wheel_radius)) ######################################################################### return Throttle, Brake, Steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 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_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 # When dbw node is disabled (at traffic light) this will prevent us from accumulating error if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = MAX_BRAKE #N*m - to hold the car in place if we are stopped at a light. Acceleration - lm/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, 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.001 kd = 10. mn_acceleration = 0. mx_acceleration = 0.2 self.throttle_controller = PID(kp, ki, kd, mn_acceleration, mx_acceleration) tau = 0.5 ts = .02 self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): #return 1., 0., 0. # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("angular vel: {0}".format(angular_vel)) 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 #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, 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 #Filtering out the high frequencey "Velocity" noise 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. #reset controller in manual mode current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel # calculate sample_time for each step of our PID controller current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 #We are going faster than target velocity, so slow down 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 light acceleration 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): ## Initialize Yaw Controller Object .. self.yaw_controller = YawController(kwargs['wheel_base'], kwargs['steer_ratio'], kwargs['min_speed'], kwargs['max_lat_accel'], kwargs['max_steer_angle']) ## Define PID, to be used later for Throttle computation self.throttle_pid = PID(kp=0.3, ki=0.0, kd=0.0, mn=kwargs['decel_limit'], mx=kwargs['accel_limit']) ## Minimum Speed placeholder self.min_speed = kwargs['min_speed'] self.steer_ratio = kwargs['steer_ratio'] ## Placeholders for effective breaking .. self.wheel_base = kwargs['wheel_base'] self.wheel_radius = kwargs['wheel_radius'] self.total_mass = kwargs['vehicle_mass'] + kwargs[ 'fuel_capacity'] * GAS_DENSITY # And even air pressure would effect mass distrubution self.brake_deadband = kwargs['brake_deadband'] ## Time Logging Placeholder self.prev_time = None ### Define low pass filter & apply if needed self.s_lpf = LowPassFilter(tau=.2, ts=.1) # control method: for any preprocessing of throttle, brake, yaw def control(self, *args, **kwargs): # Retrieve present throttle, brake, steer target_velocity_linear_x = args[0] target_velocity_angular_z = args[1] current_velocity_linear_x = args[2] current_velocity_angular_z = args[3] dbw_enabled = args[4] throttle = 0.0 brake = 0.0 steering = 0.0 ## If DBW is interrupted, reset throttle to avoid errors & return .. if not dbw_enabled: self.throttle.reset() self.steer_pid.reset() return 0, 0, 0 # Compute difference between target and current velocity as CTE for throttle. diff_velocity = target_velocity_linear_x - current_velocity_linear_x current_time = rospy.get_time() dt = 0 # Compute Delta Time from last compute.. if self.prev_time is not None: dt = current_time - self.prev_time self.prev_time = current_time velocity_controller = 0 # Determine if throttle or braking is needed .. if dt > 0: velocity_controller = self.throttle_pid.step(diff_velocity, dt) if velocity_controller > 0: # Throttle throttle = velocity_controller elif velocity_controller < 0: # Braking brake = -velocity_controller # Check for dead-band defined in config if brake < self.brake_deadband: brake = 0.0 # Apply other factors which impact braking intensity like vehcile mass, wheel radius, air pressure .. brake = brake * self.total_mass * self.wheel_radius * 4 # Define yaw from yaw conrtoller, given target and present linear, angular velocities .. steering = self.yaw_controller.get_steering(target_velocity_linear_x, target_velocity_angular_z, current_velocity_linear_x) # Apply filter if needed #steering = self.s_lpf.filt(steering) return throttle, brake, steering
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, *args, **kwargs): # TODO: Implement self.pid = PID(100, 1, 1) self.yawcontroller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
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.25 #ki=0.001 #FOPDT(first order plus dead-time model is used. #Tuning is done using IMC(Internal Model Control) kp = 0.5 ki = kp / 8.0 kd = 0.00001 mn = 0. #mx=0.2 mx = 0.8 #well... 1 is possible but... it might not be for the car.. self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = .02 self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 450 # N*m 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 # current_vel is m/s. therefore 5m/s is around 20 km/h vel_idx = int(current_vel) if vel_idx < 5: limit = throttle_for_slowstart[vel_idx] throttle = limit if current_vel > limit else current_vel # throttle=0.2 # brake = 0 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. mn = 0.0 # minimum throttle value mx = 0.2 # maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) # filter out high frequency noise in velocity tau = 0.5 # 1/(2*pi*tau) is the 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): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("Current velocity is: {}".format(current_vel)) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel print("Velocity error is " + str(vel_error)) current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time print("Sample time is " + str(sample_time)) throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0. if linear_vel == 0. and current_vel < 0.1: # the car should stop throttle = 0. brake = 700. # Nm to hold the car in place if stopped at traffic light elif throttle < 0.1 and vel_error < 0.: # the car should brake throttle = 0. decel = max( vel_error, self.decel_limit ) # both are negative, so we take the smaller absolute value brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement # implemented based on "DBW Walkthrough" 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 in Hz ts = 0.02 # Sample time in seconds 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.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 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 # implemented based on "DBW Walkthrough" if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 filtered_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("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(filtered_vel)) steer = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel) # rospy.logwarn("steer: {0}".format(steer)) vel_error = linear_vel - filtered_vel self.last_vel = filtered_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0.0 and filtered_vel < 0.1: throttle = 0.0 brake = 800.0 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2 elif throttle < 0.1 and vel_error < 0: throttle = 0.0 decel = max(vel_error, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque N*m 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) ## PID parameters for throttle Kp = 0.1 Ki = 0.02 Kd = 0.0 mn = 0.0 #min throttle mx = 0.23 #max throttle ## PID parameters for braking Kp_b = 60.0 Ki_b = 0.0 Kd_b = 10.0 mn_b = 0.0 #min brake # setup two PIDs self.throttle_controller = PID(Kp, Ki, Kd, mn, mx) self.brake_controller = PID(Kp_b, Ki_b, Kd_b, mn_b, MAX_BRAKE) tau = 0.5 #1/(2pi*tau) = cutoff freq ts = 0.02 # sampling time, this should reflect refresh rate from dbw_node 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() self.brake_controller.reset() return 0.0, 0.0, 0.0 current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) #rospy.loginfo('test:%s\n', linear_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 # when car is supposed to stop and current velocity is small, throw on max brake if linear_vel < 0.4 and current_vel < 0.4: throttle = 0 brake = MAX_BRAKE # when car is going faster than target speed, let go off throttle and apply brake elif throttle < 0.1 and vel_error < 0: throttle = 0 brake = self.brake_controller.step(-vel_error, sample_time) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lat_accel = kwargs['max_lat_accel'] self.max_steer_angle = kwargs['max_steer_angle'] min_speed = 0 self.velocity_pid = PID(kp=0.8, ki=0, kd=0.05, mn=self.decel_limit, mx=0.5 * self.accel_limit) self.steering_pid = PID(kp=0.16, ki=0.001, kd=0.1, mn=-self.max_steer_angle, mx=self.max_steer_angle) self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, min_speed, self.max_lat_accel, self.max_steer_angle) def reset(self): self.velocity_pid.reset() self.steering_pid.reset() def control(self, target_linear_vel, target_angular_vel, current_linear_vel, cross_track_error, sample_time): # Difference between target velocity and current velocity. vel_error = target_linear_vel - current_linear_vel throttle = self.velocity_pid.step(vel_error, sample_time) # N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2 brake = 0.0 # Stop if target_linear_vel == 0.0 and current_linear_vel < 0.1: throttle = 0.0 brake = MAX_BRAKE # Decelerate elif throttle < 0: deceleration = abs(throttle) if deceleration > self.brake_deadband: total_weight = self.vehicle_mass + (self.fuel_capacity * GAS_DENSITY) brake = total_weight * self.wheel_radius * deceleration throttle = 0 # without Steering PID car will wiggle around the waypoint path predictive_steering = self.yaw_controller.get_steering( target_linear_vel, target_angular_vel, current_linear_vel) corrective_steering = self.steering_pid.step(cross_track_error, sample_time) steering = predictive_steering + corrective_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): # Yaw controller (pre-provided) self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # Throttle PID controller kp = 0.3 ki = 0.1 kd = 0.0 # rospy.loginfo("PID Kp :{0}".format(kp)) # rospy.loginfo("PID Ki :{0}".format(ki)) # rospy.loginfo("PID Kd :{0}".format(kd)) mn = 0.0 # Min throttle mx = 0.2 # Max throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) # Velocity low-pass filter - to filter noise tau = 0.5 # Time constant ts = 0.02 # Sampling time # rospy.loginfo("Low pass filter Time constant :{0}".format(tau)) # rospy.loginfo("Low pass filter Sampling time :{0}".format(ts)) self.vel_lpf = LowPassFilter(tau, ts) # Other parameters self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, linear_vel, angular_vel, current_vel, dbw_enabled): # Return throttle, brake, steer # In case of drive-bt-wire NOT enabled, reset PID and return 0 if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 # Otherwise, first filter velocity current_vel = self.vel_lpf.filt(current_vel) # Calculate steering steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # Calculate throttle 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) # Calculate brake brake = 0.0 if linear_vel == 0.0 and current_vel < 0.1: # Vehcile stopped throttle = 0.0 brake = STOP_TORQUE # N*m - needed to keep the car in place elif throttle < 0.1 and vel_error < 0.0: # Stop throttle and calculate brake throttle = 0.0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_control = 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_control = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cut-off frequency ts = 0.02 # sample time # Low pass filter for incoming, noisy velocity message self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, linear_vel, angular_vel, current_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_control.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # Steering steer = self.yaw_control.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 throttle = self.throttle_control.step(vel_error, sample_time) # Brake brake = 0 if linear_vel == 0 and current_vel < 0.1: throttle = 0 brake = 400 # N*m - To hold the car in a place if we are stopped at a light. Acceleration ~ 1 m/s^2 elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = self.vehicle_mass * self.wheel_radius * abs( decel) # Torque - N*m 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 = 1.0 Ki = 0.001 Kd = 0 mn = 0. # Minmum 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() #pass 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: #rospy.loginfo("dbw_enabled is false !") self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0 and current_vel < 0.1: throttle = 0 brake = 400 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2 elif throttle < 0.1 and vel_error < 0: throttle = 0 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_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, min_speed=0.0): # PID controller for throttle and braking self.tb_pid = PID(kp=0.4, ki=0.01, kd=0.01, mn=-1.0, mx=accel_limit) # Low pass filter to smooth out throttle/braking actuations. self.tb_lpf = LowPassFilter(0.1) # Constants that are relevant to computing the final throttle/brake value 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 # Compute the braking conversion constant according to the formula `braking_torque = total_mass * max_deceleration * wheel_radius`. # `self.tb_pid` outputs barking values in `[-1,0]`, which will then be scaled by the constant below. self.braking_constant = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY ) * self.decel_limit * self.wheel_radius # Record the time of the last control computation. self.last_time = None # Controller for steering angle self.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) def control(self, target_linear_velocity, target_angular_velocity, current_linear_velocity, dbw_enabled): if dbw_enabled: if self.last_time is None: sample_time = 0.02 self.last_time = time.time() else: this_time = time.time() sample_time = this_time - self.last_time self.last_time = this_time # Compute the raw throttle/braking value. tb = self.tb_pid.step(error=(current_linear_velocity - target_linear_velocity), sample_time=sample_time) # Smoothen it. tb = self.tb_lpf.filt(tb) # Scale it. if tb < -self.brake_deadband: # We're braking. # Convert the raw braking value to torque in Nm (Newton-meters). brake = self.braking_constant * tb * TEST_BRAKING_CONST throttle = 0 elif tb < 0: # We want to slow down, but don't need to brake actively in order to do so. brake = 0 throttle = 0 else: # We're accelerating. throttle = tb brake = 0 # Compute the steering value. steer = self.yaw_controller.get_steering( linear_velocity=target_linear_velocity, angular_velocity=target_angular_velocity, current_velocity=current_linear_velocity) rospy.loginfo("tb PID: %s, brake: %s, steer: %s, tv: %s, cv: %s", tb, brake, steer, target_linear_velocity, current_linear_velocity) return throttle, brake, steer else: # If `dbw_enabled` is False. self.tb_pid.reset() return 0, 0, 0
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.yaw_controller = YawController(kwargs['wheel_base'], kwargs['steer_ratio'], MIN_SPEED, kwargs['max_lat_accel'], kwargs['max_steer_angle']) # Create PID to control throttle mn=0.0 # minimum throttle value mx=0.8 # maximum throttle value self.throttle_controller = PID(kp=0.8, ki=0.001, kd=0.2, mn=mn, mx=mx) #ki=0.15, kd=0.0001 # Create PID to control steering angle mn=-kwargs['max_steer_angle'] # minimum steering angle mx=kwargs['max_steer_angle'] # maximum steering angle self.steer_controller = PID(kp=1.4, ki=0.000001, kd=0.2, mn=mn, mx=mx) # Set low pass filter parameters: sample_time=1/50hz, cutoff_frecuency=tau tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 # Sample time = 1 / 50hz self.vel_lpf = LowPassFilter(tau, ts) 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'] # Add the fuel mass to the vehicle mass self.vehicle_mass = self.vehicle_mass + self.fuel_capacity*GAS_DENSITY self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, target_linear_vel, target_angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 # Filter current speed of the car #not_filtered_vel = current_vel current_vel = self.vel_lpf.filt(current_vel) #rospy.loginfo("dbw_node: LowPassFilter (not_filtered_speed={0}, filtered_speed={1})".format(not_filtered_vel, current_vel)) current_time = rospy.get_time() dt = current_time - self.last_time self.last_time = current_time # Calculate steering angle using yaw controller and pid controller steering = self.yaw_controller.get_steering(target_linear_vel, target_angular_vel, current_vel) steering = self.steer_controller.step(steering, dt) # Calculate throttle using pid controller vel_error = target_linear_vel - current_vel self.last_vel = current_vel throttle = self.throttle_controller.step(vel_error, dt) brake = 0 if target_linear_vel == 0.0 and current_vel < 0.1: # Stop the car an activate brake throttle = 0 brake = 700 #N*m - to hold the car in place if we are stopped at light. Acceleration ~ 1m/s^2 elif throttle < 0.1 and vel_error < 0: # Decrease speed of the car throttle = 0 decel = max(vel_error/dt, self.decel_limit) #tranform speed in acceleration brake = abs(decel)*self.vehicle_mass*self.wheel_radius #Torque N*m return throttle, brake, steering
def __init__(self): rospy.init_node('dbw_node') vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) brake_deadband = rospy.get_param('~brake_deadband', .1) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) wheel_radius = rospy.get_param('~wheel_radius', 0.2413) wheel_base = rospy.get_param('~wheel_base', 2.8498) steer_ratio = rospy.get_param('~steer_ratio', 14.8) max_lat_accel = rospy.get_param('~max_lat_accel', 3.) max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.rate = 50 # Hz self.sample_time = 1.0 / self.rate self.dbw_enabled = True self.initialized = False self.current_linear_velocity = None self.target_linear_velocity = None self.target_angular_velocity = None self.final_waypoints = None self.current_pose = None self.logger = DBWLogger(self, rate=1) 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) yaw_controller = YawController(wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=0., max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) self.controller = TwistController(yaw_controller, max_steer_angle, self.sample_time) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb, queue_size=1) self.loop()
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.001 mn = 0.0 # Minimum throttle value mx = 0.4 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() self.log_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0.0 if linear_vel == 0.0 and current_vel < 0.1: throttle = 0.0 brake = MAX_BRAKE # N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2 elif throttle < 0.1 and vel_error < 0: throttle = 0.0 decel = max(vel_error, self.decel_limit) brake = min(MAX_BRAKE, (abs(decel) * self.vehicle_mass * self.wheel_radius)) # Torque N*m if (current_time - self.log_time) > LOGGING_THROTTLE_FACTOR: self.log_time = current_time rospy.logwarn( "POSE: current_vel={:.2f}, linear_vel={:.2f}, vel_error={:.2f}" .format(current_vel, linear_vel, vel_error)) rospy.logwarn( "POSE: throttle={:.2f}, brake={:.2f}, steering={:.2f}".format( throttle, brake, steering)) return throttle, brake, steering
class 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) # Determining parameters experimentally kp = 0.3 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = 0.2 # maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 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 # comfort parameter self.accel_limit = accel_limit # comfort parameter self.wheel_radius = wheel_radius self.last_time = self.print_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. # Filter the high frequency velocities off current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # Calculate the error for the PID controller vel_err = 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_err, sample_time) 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 elif throttle < 0.1 and vel_err < 0: throttle = 0 decel = max(vel_err, self.decel_limit) brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # Torque per meter 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, max_throttle): # Yaw controller to controller the turning self.vehicle_min_velocity = .1 self.yaw_controller = YawController( wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=self.vehicle_min_velocity, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) self.steering_controller = PID( kp=.0, ki=.001, kd=.1, min=-.2, max=.2) # Throttle controller self.throttle_controller = PID( kp=.8, ki=.001, kd=.1, min=0, max=max_throttle) # Remove high frequency noise on the velocity self.low_pass = LowPassFilter(tau=.5, ts=.02) # Assume the tank is full fuel_mass = fuel_capacity * GAS_DENSITY self.total_mass = vehicle_mass + fuel_mass self.decel_limit = decel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control( self, current_velocity, linear_velocity, angular_velocity, cte): current_velocity = self.low_pass.filter(current_velocity) # Calculate the velocity error dv = linear_velocity - current_velocity # Update time current_time = rospy.get_time() dt = current_time - self.last_time self.last_time = current_time # Calculate the predictive path and correct for the cross track error steering = self.yaw_controller.get_steering( linear_velocity, angular_velocity, current_velocity) steering -= self.steering_controller.step(cte, dt) # Calculate the throttle and brake throttle = self.throttle_controller.step(dv, dt) brake = 0. if linear_velocity < 0. and current_velocity < self.vehicle_min_velocity: # Keep car in place when it is stopped throttle = 0. brake = 700. elif throttle < .1 and dv < 0.: throttle = 0. decel = max(dv, self.decel_limit) brake = abs(decel) * self.total_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lat_accel = kwargs['max_lat_accel'] self.max_steer_angle = kwargs['max_steer_angle'] self.min_speed = 0.0 self.yaw_contoller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) kp = 0.1 ki = 0.007 kd = 0.0 minth = 0.0 maxth = 0.2 self.throttle_contoller = PID(kp, ki, kd, minth, maxth) tau = 150 ts = 75 self.lpf_velocity = LowPassFilter(tau, ts) # self.lpf_steering = LowPassFilter(tau, ts) self.t0 = rospy.get_time() def control(self, proposed_twist, current_twist, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_contoller.reset() return 0.0, 0.0, 0.0 proposed_linear_velocity = proposed_twist.twist.linear.x proposed_angular_velocity = proposed_twist.twist.angular.z current_linear_velocity = current_twist.twist.linear.x current_angular_velocity = current_twist.twist.angular.z current_linear_velocity = self.lpf_velocity.filt(current_linear_velocity) # Filter current linear velocity err_linear_velocity = proposed_linear_velocity - current_linear_velocity t1 = rospy.get_time() sample_time = t1 - self.t0 self.t0 = t1 throttle = self.throttle_contoller.step(err_linear_velocity, sample_time) brake = 0.0 if proposed_linear_velocity == 0.0 and current_linear_velocity < 0.1: throttle = 0.0 brake = 400 # from walkthrough elif throttle < 0.1 and err_linear_velocity < 0: throttle = 0.0 decel = max(err_linear_velocity, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius steering = self.yaw_contoller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) return throttle, brake, steering
class Controller(object): def __init__(self, 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. # 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): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. rospy.loginfo("current v is : %f", current_vel) current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("Angular vel: {0}".format(angular_vel)) # rospy.logwarn("Target vel: {0}".format(linear_vel)) # rospy.logwarn("Target angular vel: {0}".format(angular_vel)) # rospy.logwarn("Current vel: {0}".format(current_vel)) # rospy.logwarn("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 # N*m to hold the car in place when at a light 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 # N*m return throttle, brake, steering
class Controller(object): def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min_speed max_lat_accel, max_steer_angle, ) # PID coefficients kp = 0.3 ki = 0.1 kd = 0.0 # For convenience (no real limits on throttle): mn_th = 0.0 # Minimal throttle mx_th = 0.2 # Maximal throttle self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th) tau = 0.5 # 1 / (2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.wheel_radius = wheel_radius self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 filtered_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn( # '\nAngular vel: {}\n' # 'Target velocity: {}\n' # 'Current velocity: {}\n' # 'Filtered velocity: {}\n' # .format(angular_vel, linear_vel, current_vel, filtered_vel) # ) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel) vel_error = linear_vel - filtered_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0 and filtered_vel < 0.1: throttle = 0 brake = 400 # [N*m] -- to hold the car in place if we are stopped # at a light. Acceleration ~ 1 m/s/s elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque [N*m] return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # 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.last_time = None # setup the yaw controller for steering self.yaw_controller = YawController(wheel_base = wheel_base, steer_ratio = steer_ratio, min_speed = ONE_MPH, max_lat_accel = max_lat_accel, max_steer_angle = max_steer_angle) # setup the PID controller parameters for the accelerator/brake self.accel_pid = PID(kp=1., ki=0., kd=0., mn=decel_limit, mx=accel_limit) # setup the low pass filter for steering self.steer_lpf = LowPassFilter(tau = 3., ts = 1.) # setup the low pass filter for acceleration self.accel_lpf = LowPassFilter(tau = 3., ts = 1.) #pass def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # only proceed if dbw is enabled; else error may accumulate in control loops if not dbw_enabled: return 0., 0., 0. # don't send commands if this is first time through loop if self.last_time is None: self.last_time = rospy.get_time() return 0.0, 0.0, 0.0 # calculate delta time for the PID controller delta_time = rospy.get_time() - self.last_time rospy.loginfo("proposed_linear_velocity %f, current_linear_velocity %f", proposed_linear_velocity, current_linear_velocity) rospy.loginfo("proposed_angular_velocity %f", proposed_angular_velocity) velocity_error = proposed_linear_velocity - current_linear_velocity steering = self.yaw_controller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) steer_cmd = self.steer_lpf.filt(steering) acceleration = self.accel_pid.step(velocity_error, delta_time) acceleration = self.accel_lpf.filt(acceleration) if acceleration > 0.0: throttle_cmd = acceleration brake_cmd = 0.0 else: throttle_cmd = 0.0 brake_cmd = abs(acceleration) # only brake if commanded braking higher than deadband if brake_cmd < self.brake_deadband: brake_cmd = 0.0 # translate percentage of braking to braking force desired brake_cmd = brake_cmd * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius #return 1., 0., 0. return throttle_cmd, brake_cmd, steer_cmd
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 mx = 0.9 self.throttle_controler = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 0.02 self.vel_lpf = LPF(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.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() def control(self, curr_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controler.reset() return 0.0, 0.0, 0.0 curr_vel = self.vel_lpf.filt(curr_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, curr_vel) vel_error = linear_vel - curr_vel self.last_vel = curr_vel curr_time = rospy.get_time() sample_time = curr_time - self.last_time self.last_time = curr_time throttle = self.throttle_controler.step(vel_error, sample_time) brake = 0.0 if linear_vel < 0.05 and curr_vel < 0.05: throttle = 0.0 brake = 700.0 elif throttle < 0.1 and vel_error < 0: throttle = 0.0 decel = max(vel_error, self.decel_limit) #decel = self.decel_limit brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.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.throttle_controller = PID(1.5, 0.001, 0.0, 0, 1) self.steer_controller = YawController(self.wheel_base, self.steer_ratio, 0.001, self.max_lat_accel, self.max_steer_angle) self.previous_time = 0 self.lpfilter = LowPassFilter(0.2, 0.1) self.sim_scale_factor = 15 # set to 1 for Carla??? def control(self, cmd_linear_x, cmd_angular_z, current_linear_x, current_angular_z, reset_dt): if reset_dt: self.previous_time = rospy.get_time() throttle = 0.0 steer = 0.0 brake = 0.0 else: delta_t = rospy.get_time() - self.previous_time speed_error = cmd_linear_x - current_linear_x steer = self.steer_controller.get_steering(cmd_linear_x, cmd_angular_z, current_linear_x) steer = self.lpfilter.filt(steer) if speed_error >= 0.0: brake = 0.0 speed_error = self.sim_scale_factor * min( speed_error, self.accel_limit * delta_t) throttle = self.throttle_controller.step(speed_error, delta_t) else: throttle = 0.0 decel = max(self.decel_limit, speed_error / delta_t) if abs(decel) < self.brake_deadband: brake = 0.0 else: mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY brake = abs(decel) * mass * self.wheel_radius rospy.loginfo('speed_error is %f', speed_error) rospy.loginfo('pid result is %f', throttle) self.previous_time = rospy.get_time() return throttle, brake, steer
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) brake_deadband = rospy.get_param('~brake_deadband', .1) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) wheel_radius = rospy.get_param('~wheel_radius', 0.2413) wheel_base = rospy.get_param('~wheel_base', 2.8498) steer_ratio = rospy.get_param('~steer_ratio', 14.8) max_lat_accel = rospy.get_param('~max_lat_accel', 3.) max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) # TODO: Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) self.pid = PID(-0.9, -0.001, -0.07, decel_limit, accel_limit) self.lowpass_filter = LowPassFilter( 1.0, 2.0) # the less the value, the more smooth #self.yaw_lowpass = LowPassFilter(1.0, 1.0) self.yaw_controller = YawController(wheel_base, steer_ratio, 0, max_lat_accel, max_steer_angle) self.brake_factor = vehicle_mass * wheel_radius self.brake_deadband = brake_deadband # TODO: Subscribe to all the topics you need to # init the vars self.current_velocity = None self.twist_cmd = None self.dbw_enabled = False # For debugging, use true self.last_timestamp = rospy.get_time() # subscribe rospy.Subscriber("/current_velocity", TwistStamped, self.current_velocity_cb) rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cmd_cb) rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_enabled_cb) rospy.loginfo("### Final submission with dbw") rospy.loginfo("### Controller initialized Yeah 3x Speed") self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # TODO: Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) # init the vars ref_v = 0. cur_v = 0. err_v = 0. ref_yaw = 0. dt = 0.01 thro_cmd = 0. brake_cmd = 0. steer_cmd = 0. if self.twist_cmd and self.current_velocity: ref_v = self.twist_cmd.twist.linear.x # play sth! rospy.loginfo("RefV B4: %f" % ref_v) #ref_v = ref_v * 3 rospy.loginfo("RefV Afta: %f" % ref_v) ref_yaw = self.twist_cmd.twist.angular.z cur_v = self.current_velocity.twist.linear.x err_v = cur_v - ref_v dt = rospy.get_time() - self.last_timestamp self.last_timestamp = rospy.get_time() rospy.loginfo("CurV: %f, RefV: %f" % (cur_v, ref_v)) rospy.loginfo("Change the errors: " + str(err_v)) if self.dbw_enabled: acc = self.pid.step(err_v, dt) # get the acc if acc >= 0: thro_cmd = self.lowpass_filter.filt(acc) brake_cmd = 0. else: thro_cmd = 0. brake_cmd = self.brake_factor * ( -acc) + self.brake_deadband steer_cmd = self.yaw_controller.get_steering( ref_v, ref_yaw, cur_v) #steer_cmd = self.yaw_lowpass.filt(steer_cmd) self.publish(thro_cmd, brake_cmd, steer_cmd) rospy.loginfo("Publish:" + str(thro_cmd) + " " + str(steer_cmd)) rospy.loginfo("Time used: %.3f" % dt) else: self.pid.reset() rate.sleep() def publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def twist_cmd_cb(self, msg): self.twist_cmd = msg rospy.loginfo("Get twist cmd:" + str(self.twist_cmd.twist)) def current_velocity_cb(self, msg): self.current_velocity = msg def dbw_enabled_cb(self, msg): self.dbw_enabled = msg
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement ''' yaw controller initialize ''' self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) self.last_time = rospy.get_time() ''' throttle control initialize ''' kp = 0.3 ki = 0.1 kd = 0.0 mn = 0.0 # Min throttle value mx = 0.4 # Max throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) ''' Low pass filter initialize To address velocity msg noise ''' tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) ''' store vehicle parameters ''' self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius 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 ''' reset controller while dbw is disable, in case integral component accumulating error ''' if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 ''' drive by wire enable ''' # filtering current velocity current_vel = self.vel_lpf.filt(current_vel) # steering control steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # throttle control 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 control brake = 0 if linear_vel == 0.0 and current_vel < 0.1: throttle = 0.0 brake = 700 #Nm elif throttle <0.1 and vel_error < 0: throttle = 0.0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): #def __init__(self, *args, **kwargs): def __init__(self, vehicle_mass, fuel_capacity, brake_demand, 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) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_demand = brake_demand self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() kp = 0.25 ki = 0.0 kd = 0.15 mn = 0. # minimum throttle value mx = 0.5 # maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 0.02 # sample time self.vel_lpf = LowPassFilter(tau, ts) # ----------------------------------------------------------------------------------------------------- def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel= current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 400 # Nm - to hold car in place if stopped at 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, **ros_param): """ :param ros_param: Note: sample time (sec) is based on the dbw node frequency 50Hz low_pass filter: val = w * cur_val + (1 - w) * prev_val w is 0 ~ 1 """ # used ros_param self.vehicle_mass = ros_param['vehicle_mass'] # self.fuel_capacity = ros_param['fuel_capacity'] self.brake_deadband = ros_param['brake_deadband'] self.decel_limit = ros_param['decel_limit'] self.accel_limit = ros_param['accel_limit'] self.wheel_radius = ros_param['wheel_radius'] self.last_time = rospy.get_time() # low pass filter for velocity self.vel_lpf = LowPassFilter(0.5, .02) # Init yaw controller min_speed = 0.1 # I think min_speed self.steer_controller = YawController(min_speed, **ros_param) self.throttle_lpf = LowPassFilter(0.05, 0.02) # w = 0.28 # Init throttle PID controller # TODO: tweaking kp = 0.5 ki = 0.005 kd = 0.1 acc_min = 0. acc_max = 0.5 #self.accel_limit self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max) def control(self, target_linear_velocity, target_angular_velocity, cur_linear_velocity, dbw_status): # Check input info is ready if not dbw_status: self.throttle_controller.reset() return 0., 0., 0. # dbw enabled: control! cur_linear_velocity = self.vel_lpf.filt(cur_linear_velocity) # get steer value steering = self.steer_controller.get_steering(target_linear_velocity, target_angular_velocity, cur_linear_velocity) # get throttle (could be < 0 so it will be updated by `get brake` as well) vel_err = target_linear_velocity - cur_linear_velocity current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_err, sample_time) # get brake brake = 0 if target_linear_velocity == 0. and cur_linear_velocity < 0.1: throttle = 0 brake = 400 # N * m - hold the car in place for the red traffic light elif throttle < .1 and vel_err < 0.: throttle = 0. decel_velocity = max(vel_err, self.decel_limit) # attention this value is < 0 # if less than brake_deaband, we don't need to add brake # The car will deceleration by friction just release peddle if abs(decel_velocity) > self.brake_deadband: brake = abs(decel_velocity) * self.vehicle_mass * self.wheel_radius else: brake = 0 return throttle, brake, steering def reset(self): self.throttle_controller.reset()