class TwistController(object): def __init__(self, **veh_caps): # Capture key vehicle capabilites rospy.logwarn(str(veh_caps)) self.vehicle_mass = veh_caps['vehicle_mass'] self.fuel_capacity = veh_caps['fuel_capacity'] self.brake_deadband = veh_caps['brake_deadband'] self.decel_limit = veh_caps['decel_limit'] self.accel_limit = veh_caps['accel_limit'] self.wheel_radius = veh_caps['wheel_radius'] self.wheel_base = veh_caps['wheel_base'] self.steer_ratio = veh_caps['steer_ratio'] self.max_lat_accel = veh_caps['max_lat_accel'] self.max_steer_angle = veh_caps['max_steer_angle'] self.min_speed = 0.0 self.current_vehicle_mass = self.vehicle_mass + ( (CURRENT_FUEL_LEVEL / 100) * (self.fuel_capacity * GAS_DENSITY)) # @Kostas Oreopoulos - carnd slack commennt self.max_brake_force = abs(MAX_brake_FORCE_RATIO * self.vehicle_mass * self.decel_limit * self.wheel_radius) # 1st implementation of steering is to use provided Yaw Controller. self.yaw = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) self.yaw_lp = LowPassFilter(YAW_LP_TAO, YAW_LP_TS) self.throttle_pid = PID(VEL_PID_KP, VEL_PID_KI, VEL_PID_KD, THROTTLE_PID_LIMIT_MIN, THROTTLE_PID_LIMIT_MAX) self.throttle_lp = LowPassFilter(THROTTLE_LP_TAO, THROTTLE_LP_TS) self.brake_lp = LowPassFilter(BRAKE_LP_TAO, BRAKE_LP_TS) self.lastTime = 0.0 self.first_call = True self.pref_vel = 0.0 def control(self, proposed_linear_vel, proposed_angular_vel, current_linear_vel, dbw_enabled): throttle = 0.0 brake = 0.0 steer = 0.0 # Testing - hardcode proposed velicity #proposed_linear_vel = 20.0 * ONE_MPH # 0.621371 # Why is ONE_MPH 0.44704 above? if self.first_call: # It would be best to get timestampe from currnet vel message but not populated. # We assume that current velicity is now, and no latency in transport to this node. while True: # Time may not valid at startup, should wait for time to be setup. self.lastTime = rospy.get_time() if self.lastTime != 0.0: break self.prev_vel = current_linear_vel self.prev_throttle = 0.0 self.yaw_lp.filt(.0) self.throttle_pid.reset() self.throttle_lp.filt(0.) self.brake_lp.filt(0.) self.first_call = False else: # Calculate time since last message for PID controller. now = rospy.get_time() dt = now - self.lastTime self.lastTime = now # At breaking, we are generating negative proposed velocity, possible error in waypoint gen while breaking. if (proposed_linear_vel < 0.): #rospy.logwarn("Proposed liner vel:%f set to zero:", proposed_linear_vel) proposed_linear_vel = 0 delta_vel = proposed_linear_vel - current_linear_vel if (dbw_enabled): # For Steerig - use provided yaw controller steer_err = self.yaw.get_steering(proposed_linear_vel, proposed_angular_vel, current_linear_vel) steer = self.yaw_lp.filt(steer_err) # rospy.logwarn("Yaw:%f : Yaw_lp:%f : P-lin-vel:%f Pre-ang-Vel:%f current-lin-vel:%f",steer_err,steer,proposed_linear_vel,proposed_angular_vel,current_linear_vel) #rospy.logwarn("Proposed_linear_vel:%f current_velocity:%f", proposed_linear_vel, current_linear_vel) error = self.throttle_pid.step(delta_vel, dt) if (error > 0.0): # Acceleration condition. , at proposed vel = 0, we get some small accel in current vel, focce accel to zero for that case. if proposed_linear_vel == 0.: throttle = 0. else: throttle = self.throttle_lp.filt(error) self.brake_lp.set( 0. ) # While we are accelerating set the brake lp to zero. else: throttle = 0.0 # use linear map from pid to brake force range. error 0 to -1/ brake_force = abs(error) * self.max_brake_force brake = self.brake_lp.filt(brake_force) #rospy.logwarn("BRAKING: BF:%f Brake:%f Error:%f abs_error:%f max_bf:%f proposed_linear_vel:%f", #brake_force, brake, error,abs(error), self.max_brake_force ,proposed_linear_vel) #Need to apply final presure as error gets too small, we creep along. if (brake <= self.brake_deadband * 5): brake = self.brake_deadband * 5 self.throttle_lp.set( 0. ) # While we are braking set the accelerator lp to zero. #rospy.logwarn("PID Error:%f : Throttle:%f : brake:%f DeltaVel:%f Target vel:%f : Actual Vel:%f",error,throttle,brake,delta_vel,proposed_linear_vel,current_linear_vel) else: self.yaw_lp.filt(.0) self.throttle_pid.reset() self.throttle_lp.set(0.) self.brake_lp.set(0.) throttle = brake = steer = 0.0 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.05, max_lat_accel, max_steer_angle) kp = 0.53 ki = 0.001 kd = 0.00001 mn = -5 #Minimum throttle value mx = 1 #maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.22 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) self.yaw_lp = LowPassFilter(0.2, 0.1) self.brake_lp = LowPassFilter(0.045, 0.02) 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.total_mass = vehicle_mass + fuel_capacity * GAS_DENSITY self.last_time = None 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.yaw_lp.filt(0.0) self.brake_lp.set(0.0) return 0., 0., 0. if self.last_time is None: self.last_time = rospy.get_time() return 0., 0., 0. linear_vel2 = linear_vel #if linear_vel < 0.: # linear_vel = 0 #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: {0}".format(angular_vel)) rospy.logwarn("Current vel: {0}".format(current_vel)) rospy.logwarn("Filtered vel: {0}".format(self.vel_lpf.get()))''' current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time steer_err = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) steering = self.yaw_lp.filt(steer_err) vel_error = min(linear_vel, MAX_SPEED * ONE_MPH) - current_vel vel_error = max(vel_error, self.decel_limit * sample_time) self.last_vel = current_vel #rospy.loginfo("vel error:%s,time:%s, target vel:linear_vel",vel_error,sample_time) throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 throttle2 = throttle sss = "a" '''if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 701 #rospy.loginfo("break 701") elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = 0.8*abs(decel)* self.total_mass * self.wheel_radius # Torque rospy.loginfo("linear_vel:{0} current_vel:{1} vel_err:{2} throttle:{3} decel:{4} brake:{5}".format(linear_vel,current_vel,vel_error,throttle2, decel,brake)) ''' #rospy.loginfo("linear_vel:%s current_vel:%s throttle:%s",linear_vel,current_vel,throttle2) if (vel_error > 0.0): self.brake_lp.set(0.) brake = 0. else: decel = max(vel_error, self.decel_limit * sample_time) brake = abs( decel) * 0.80 * self.total_mass * self.wheel_radius # Torque #brake = self.brake_lp.filt(brake) #rospy.loginfo("brake:%s",brake) throttle = 0.0 if (brake <= 0.5): brake = 0.5 if linear_vel <= 0.1: brake = 800 rospy.loginfo( 'linearV:%s currentV:%s brake:%s throttle:%s decel:%s steer:%s', linear_vel2, current_vel, brake, throttle2, self.decel_limit, steer_err) #sss = "linearV:{0} currentV:{1} brake:{2} throttle:{3} decel:{4}".format(linear_vel,current_vel,brake,throttle2, decel) #print(sss) return throttle, brake, steering