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, **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 get_steering_pid_cte(self, final_waypoint1, final_waypoint2, current_location, dbw_enabled): steering = 0 if final_waypoint1 and final_waypoint2: # vector from car to first way point a = np.array([current_location.x - final_waypoint1.pose.pose.position.x, current_location.y - final_waypoint1.pose.pose.position.y, current_location.z - final_waypoint1.pose.pose.position.z]) # vector from first to second way point b = np.array([final_waypoint2.pose.pose.position.x-final_waypoint1.pose.pose.position.x, final_waypoint2.pose.pose.position.y-final_waypoint1.pose.pose.position.y, final_waypoint2.pose.pose.position.z-final_waypoint1.pose.pose.position.z]) # progress on vector b # term = (a.b / euclidian_norm(b)**2) * b where a.b is dot product # term = progress * b => progress = term / b => progress = (a.b / euclidian_norm(b)**2) progress = (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]) # position where the car should be: waypoint1 + progress * b error_pos = np.array([final_waypoint1.pose.pose.position.x, final_waypoint1.pose.pose.position.y, final_waypoint1.pose.pose.position.z]) + progress * b # difference vector between where the car should be and where the car currently is error = (error_pos - np.array([current_location.x, current_location.y, current_location.z])) # is ideal track (b) left or right of the car's current heading? dot_product = a[0]*-b[1] + a[1]*b[0] direction = 1.0 if dot_product >= 0: direction = -1.0 else: direction = 1.0 # Cross track error is the squared euclidian norm of the error vector: CTE = direction*euclidian_norm(error)**2 cte = direction * math.sqrt(error[0]*error[0]+error[1]*error[1]+error[2]*error[2]) sample_step = 0.02 if not(self.previous_dbw_enabled) and dbw_enabled: self.previous_dbw_enabled = True self.cte_pid.reset() self.low_pass_filter = LowPassFilter(self.tau, self.ts) else: self.previous_dbw_enabled = False steering = self.cte_pid.step(cte, sample_step) #steering = self.low_pass_filter.filt(steering) return steering
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 get_steering_pid(self, angular_velocity, angular_current, dbw_enabled): angular_error = angular_velocity - angular_current sample_step = 0.02 if not(self.previous_dbw_enabled) and dbw_enabled: self.previous_dbw_enabled = True self.linear_pid.reset() self.low_pass_filter = LowPassFilter(self.tau, self.ts) else: self.previous_dbw_enabled = False steering = self.linear_pid.step(angular_error, sample_step) steering = self.low_pass_filter.filt(steering) return steering
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.min_speed = 5 self.max_lat_accel = max_lat_accel self.previous_dbw_enabled = False self.min_angle = -max_steer_angle self.max_angle = max_steer_angle self.linear_pid = PID(0.9, 0.001, 0.0004, self.min_angle, self.max_angle) #self.cte_pid = PID(0.4, 0.1, 0.1, self.min_angle, self.max_angle) self.cte_pid = PID(0.4, 0.1, 0.2, self.min_angle, self.max_angle) self.tau = 0.2 self.ts = 0.1 self.low_pass_filter = LowPassFilter(self.tau, self.ts)
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 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.decel_limit = decel_limit self.accel_limit = accel_limit 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) # Initialize constants for feed-forward-control self.mass = vehicle_mass + fuel_capacity * GAS_DENSITY self.brake_deadband = brake_deadband self.wheel_radius = wheel_radius rospy.loginfo( "DriveByWire with Feed Forward Control: mass={}kg and wheel_radius={}m" .format(self.mass, self.wheel_radius)) # Create controller object min_speed = 0.1 self.controller = Controller(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, decel_limit, accel_limit, wheel_radius, self.mass, P_THROTTLE, P_BRAKE, D_RESIST) # optional logging to a csv-file self.csv_fields = [ 'time', 'x', 'y', 'v_raw', 'v', 'v_d', 'a', 'v_des', 'v_d_des', 'a_des', 'throttle', 'throttle_des', 'brake', 'brake_des', 'steer', 'torque' ] if RECORD_CSV: base_path = os.path.dirname(os.path.abspath(__file__)) base_path = os.path.dirname(base_path) base_path = os.path.dirname(base_path) base_path = os.path.dirname(base_path) base_path = os.path.join(base_path, 'data', 'records') if not os.path.exists(base_path): os.makedirs(base_path) csv_file = os.path.join(base_path, 'driving_log.csv') rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback) rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.steering_callback) rospy.Subscriber('/vehicle/throttle_report', Float32, self.throttle_callback) rospy.Subscriber('/vehicle/brake_report', Float32, self.brake_callback) self.fid = open(csv_file, 'w') self.csv_writer = csv.DictWriter(self.fid, fieldnames=self.csv_fields) self.csv_writer.writeheader() self.csv_data = {key: 0.0 for key in self.csv_fields} rospy.logwarn("created logfile for manual driving: " + self.fid.name) # Subscribe to all required topics rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback, queue_size=2) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=2) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1) # lowpass filter self.velocity_filt = LowPassFilter(0.1, 1.0 / 50.0) self.acceleration_filt = LowPassFilter(0.1, 1.0 / 50.0) self.throttle_filt = LowPassFilter(0.06, 1.0 / 50.0) self.brake_filt = LowPassFilter(0.06, 1.0 / 50.0) # additional variables self.dbw_enabled = False self.current_linear_velocity = None self.angular_velocity = None self.desired_linear_velocity = None self.desired_angular_velocity = None self.last_loop = None self.current_acceleration = None self.desired_velocity_old = 0.0 self.dbw_ready = False self.acc_ready = False self.loop() def pose_callback(self, msg): self.csv_data['x'] = msg.pose.position.x self.csv_data['y'] = msg.pose.position.y def steering_callback(self, msg): self.csv_data['steer'] = msg.steering_wheel_angle_cmd def throttle_callback(self, msg): self.csv_data['throttle'] = msg.data def brake_callback(self, msg): self.csv_data['brake'] = msg.data def twist_cmd_callback(self, data): # callback of desired velocities self.desired_linear_velocity = data.twist.linear.x if self.desired_linear_velocity < 0: rospy.logwarn( "negative linear.x velocity requested: reverse is not available yet" ) self.desired_linear_velocity = 0.0 self.desired_angular_velocity = data.twist.angular.z if RECORD_CSV: self.csv_data['a_des'] = self.desired_angular_velocity def current_velocity_callback(self, data): # callback of current vehicle velocities if self.current_linear_velocity is not None: self.linear_velocity_old = self.current_linear_velocity else: self.linear_velocity_old = 0 self.current_linear_velocity = self.velocity_filt.filt( data.twist.linear.x) self.angular_velocity = data.twist.angular.z if RECORD_CSV: self.csv_data['v_raw'] = data.twist.linear.x self.csv_data['v'] = self.current_linear_velocity self.csv_data['a'] = self.angular_velocity if not RECORD_TIME_TRIGGERED: self.csv_data['time'] = rospy.get_time() self.csv_writer.writerow(self.csv_data) def dbw_enabled_callback(self, tf): self.dbw_enabled = tf def loop(self): rate = rospy.Rate(50) # Carla wants 50Hz while not rospy.is_shutdown(): now = rospy.get_time() if self.acceleration_calc_ready(): # calculate current acceleration (derivative of velocity) dt = now - self.last_loop acceleration_raw = (self.current_linear_velocity - self.linear_velocity_old) / dt self.current_acceleration = self.acceleration_filt.filt( acceleration_raw) else: self.current_acceleration = 0.0 if self.autopilot_ready(): total_wheel_torque, steering, v_d_des = self.controller.control( self.desired_linear_velocity, self.desired_angular_velocity, self.current_linear_velocity, self.current_acceleration, now - self.last_loop) if total_wheel_torque > 0: # accelerate or coast along at constant speed brake = 0.0 throttle = total_wheel_torque / P_THROTTLE elif total_wheel_torque > -300: # -self.brake_deadband*P_THROTTLE: # a small deadband to avoid braking while coasting if self.desired_linear_velocity <= 1.0: # minimum braking torque in case of a stop brake = CREEPING_TORQUE # reset controller when standing self.controller.reset() else: brake = 0.0 # of course we do not step on the gas while braking throttle = 0.0 else: # we are decelerating (braking) # SIMULATOR: fitted coefficient between throttle and acceleration if self.desired_linear_velocity <= 0.1 and self.current_linear_velocity <= 1.0: brake = CREEPING_TORQUE # reset controller when standing self.controller.reset() else: # mass * acceleration = force | force = torque / radius brake = -total_wheel_torque / P_BRAKE throttle = 0.0 # throttle = self.throttle_filt.filt(throttle) # brake = self.brake_filt.filt(brake) self.publish(throttle, brake, steering) else: # do not publish drive-by-wire commands if we are driving manually self.controller.reset() v_d_des = 0.0 throttle = 0.0 brake = 0.0 total_wheel_torque = 0.0 if RECORD_CSV: self.csv_data['v_d'] = self.current_acceleration self.csv_data['v_des'] = self.desired_linear_velocity self.csv_data['v_d_des'] = v_d_des self.csv_data['throttle_des'] = throttle self.csv_data['brake_des'] = brake self.csv_data['torque'] = total_wheel_torque if RECORD_TIME_TRIGGERED: self.csv_data['time'] = now self.csv_writer.writerow(self.csv_data) self.last_loop = now rate.sleep() if RECORD_CSV: self.fid.close() def autopilot_ready(self): # returns true if the drive-by-wire is fully initialized and ready if not self.dbw_ready: current_okay = self.current_linear_velocity is not None and self.angular_velocity is not None desired_okay = self.desired_linear_velocity is not None and self.desired_angular_velocity is not None timing_okay = self.last_loop is not None self.dbw_ready = current_okay and desired_okay and timing_okay return self.dbw_enabled and self.dbw_ready def acceleration_calc_ready(self): # return True if all data required for the acceleration calculation is available if not self.acc_ready: timing_okay = self.last_loop is not None velocity_okay = self.current_linear_velocity is not None and self.linear_velocity_old is not None self.acc_ready = timing_okay and velocity_okay return self.acc_ready 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)
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, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.min_vel = 0.1 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.yaw_controller = YawController(wheel_base, steer_ratio, self.min_vel, max_lat_accel, max_steer_angle) # Coefficients for PID Controller p = 0.3 i = 0.1 d = 0. min_throttle = 0. max_throttle = 0.2 # initialize PID controller self.throttle_controller = PID(p, i, d, min_throttle, max_throttle) tau = 0.5 ts = .02 self.vel_lpf = LowPassFilter(tau, ts) self.last_time = rospy.get_time() def control(self, dbw_enabled, current_vel, linear_vel, angular_vel): # if dbw not enabled, reset controller if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. # filter current velocity if linear_vel > 2.78: # 2.78 m/s -> 10 km/h vel = self.vel_lpf.filt(current_vel) else: vel = current_vel steering = self.yaw_controller.get_steering(linear_vel, angular_vel, vel) vel_error = linear_vel - 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) if linear_vel < 2.78: # 2.78 m/s -> 10 km/h throttle = 0.25 * throttle brake = 0. if linear_vel == 0. and vel < self.min_vel: throttle = 0. brake = 700 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 if brake < 700.: brake = 700. # rospy.loginfo("Control (%s, %s, %s) -> throttle: %s, brake: %s, steer: %s", linear_vel, current_vel, vel, throttle, brake, steering) return throttle, brake, steering
class Controller(object): #def __init__(self, *args, **kwargs): 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.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() 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.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 = 700 elif throttle < 0.1 and vel_error < 0.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, *args, **kwargs): # # TODO: Implement # 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): self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.5 ki = 0.3 kd = 0.2 mn = 0. # Minimum throttle value mx = 0.4 # Maximum throttle value # kp = 1 # ki = 1 # kd = 1 # mn = 0. # Minimum throttle value # mx = 1 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) # kp = 0.5 # ki = 0.00018 # kd = 5.5 # mn = -math.pi/3 # mx = math.pi/3 # self.steering_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tay = cutoff frequency) ts = .02 # sample time self.vel_lpf = LowPassFilter(tau, ts) self.ang_vel_lpf = LowPassFilter(1, 1) 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, *args, **kwargs): # # TODO: Change the arg, kwarg list to suit your needs # # Return throttle, brake, steer # return 1., 0., 0. def control(self, current_vel, curr_ang_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() # self.steering_controller.reset() return 0., 0., 0 # steering = self.yaw_controller.get_steering(linear_vel , angular_vel , current_vel) current_vel = self.vel_lpf.filt(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. and current_vel < 0.1: throttle = 0 brake = 700 # N*m - to hold the car in place if we are 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 steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # rospy.loginfo("steering=%f", steering) #curr_ang_vel = self.vel_lpf.filt(curr_ang_vel) #ang_vel_error = angular_vel - curr_ang_vel #steering = steering + self.steering_controller.step(ang_vel_error, sample_time) #steering = self.ang_vel_lpf.filt(steering) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): #get the vehicle specific parameters 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 = kwargs['min_speed'] #init controllers self.pid_linear = PID(0.4, 0.0, 0.02, self.decel_limit, self.accel_limit) self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) self.steer_filter = LowPassFilter(tau=2, ts=1) self.throttle_lilter = LowPassFilter(tau=2, ts=1) self.pid_steer = PID(0.12, 0.002, 0.1, -self.max_steer_angle, self.max_steer_angle) def reset(self): self.pid_linear.reset() self.pid_steer.reset() def control(self, *args, **kwargs): # get all control params new_linear_vel = args[0] new_angular_vel = args[1] cur_linear_vel = args[2] cte = args[3] duration = args[4] acceleration_update = steering_update = brake_update = 0.0 #get the strring angle update from yaw controller steering_update = self.yaw_controller.get_steering( new_linear_vel, new_angular_vel, cur_linear_vel) #steering_update = self.steer_filter.filt(steering_update) steering_update += self.pid_steer.step( cte, duration) #use steer PID to correct the calculated steering update #get acceleration update from pid acceleration_update = self.pid_linear.step( (new_linear_vel - cur_linear_vel), duration) acceleration_update = self.throttle_lilter.filt(acceleration_update) #if we were to decelerate, calculate the break update if (acceleration_update < 0.0): brake_update = -1.0 * acceleration_update * ( self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius acceleration_update = 0.0 print("acceleration_update:", acceleration_update, " brake_update:", brake_update, " steering_update:", steering_update) rospy.loginfo( "new control info: throttle: %s, brake: %s, steering: %s" % (acceleration_update, brake_update, steering_update)) return acceleration_update, brake_update, steering_update
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()
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): # Create an object with class YawController self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # Define PID tuning parameters for the throttle controller kp = 0.3 #5, 0.3 ki = 0.1 #0.5, 0.1 kd = 0. # 0.5, 0. mn = 0. # Minimum throttle value mx = 0.2 # Maximum throttle value tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.steer_lpf = LowPassFilter(tau, ts) self.vel_lpf = LowPassFilter(tau, ts) self.t_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.throttle_controller = PID(kp, ki, kd, -5, .4) def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # The function uses the YawController class and PID class to calculate the throttle, steering inputs and applies the brake based on throttle, velocity. # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) vel_error = linear_vel - current_vel steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) steering = self.steer_lpf.filt(steering) self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time acc = self.throttle_controller.step(vel_error, sample_time) acc = self.t_lpf.filt(acc) brake = 0.0 if linear_vel == 0.0: # and current_vel < 0.1: throttle = 0.0 brake = 700 #N-m - to hold the car in place if we stopped at a light. self.throttle_controller.reset() else: # and vel_error < 0: throttle = acc if acc <= 0.0: throttle = 0 decel = -acc if decel < self.brake_deadband: decel = 0 brake = abs(decel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * 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): 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 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): # TODO: Change the arg, kwarg list to suit your needs # check is dbw node enabled, if not, reset throttle controller if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 # otherwise, get current velocity through low pass filter current_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(self.vel_lpf.get())) # get steering from yaw_controller steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # get velocity error vel_error = linear_vel - current_vel self.last_vel = current_vel # get sample time from difference between rospy current time and last time current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # Get sample time for each step of our throttle PID controller throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 # check if our linear velocity is 0 and we are going very slow, we should stop if linear_vel == 0.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. accel ~ 1m/s^2 # else if the car is going faster than we want and our pid is letting up on the throttle # , but we want to slow down, apply brake based on decel*vehicle_mass*wheel_radius # if velocity is very large, then the brake value is very large 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, steer return throttle, brake, steering
class Controller(object): def __init__(self, vehicle): assert isinstance(vehicle, Vehicle) self.vehicle = vehicle self.accel_pid = PID(kp=.4, ki=.1, kd=0., mn=0., mx=1.) self.speed_pid = PID(kp=2., ki=0., kd=0., mn=self.vehicle.deceleration_limit, mx=self.vehicle.acceleration_limit) self.last_velocity = 0. self.curr_fuel = LowPassFilter(tau=60.0, ts=0.1) self.lpf_accel = LowPassFilter(tau=.5, ts=0.02) self.yaw_control = YawController( wheel_base=self.vehicle.wheel_base, steer_ratio=self.vehicle.steer_ratio, min_speed=4. * ONE_MPH, max_lat_accel=self.vehicle.max_lat_acceleration, max_steer_angle=self.vehicle.max_steer_angle) self.last_ts = None def control(self, linear_velocity, angular_velocity, current_velocity): time_elapsed = self.time_elapsed() if time_elapsed > 1. / 5 or time_elapsed < 1e-4: self.reset_pids() self.last_velocity = current_velocity return 0., 0., 0. vehicle_mass = self.vehicle.gross_mass(curr_fuel=self.curr_fuel.get(), fuel_density=GAS_DENSITY) vel_error = linear_velocity - current_velocity if abs(linear_velocity) < ONE_MPH: self.speed_pid.reset() accel_cmd = self.speed_pid.step(vel_error, time_elapsed) min_speed = ONE_MPH * 5. if linear_velocity < .01: accel_cmd = min(accel_cmd, -530. / vehicle_mass / self.vehicle.wheel_radius) elif linear_velocity < min_speed: angular_velocity *= min_speed / linear_velocity linear_velocity = min_speed accel = (current_velocity - self.last_velocity) / time_elapsed _ = self.lpf_accel.filt(accel) self.last_velocity = current_velocity throttle, brake = 0., 0. if accel_cmd >= 0: throttle = self.accel_pid.step(accel_cmd - self.lpf_accel.get(), time_elapsed) else: self.accel_pid.reset() if (accel_cmd < -self.vehicle.brake_deadband) or (linear_velocity < min_speed): brake = -accel_cmd * vehicle_mass * self.vehicle.wheel_radius steering = self.yaw_control.get_steering(linear_velocity, angular_velocity, current_velocity) return throttle, brake, steering def reset_pids(self): self.accel_pid.reset() self.speed_pid.reset() def time_elapsed(self): now = rospy.get_time() if not self.last_ts: self.last_ts = now elapsed, self.last_ts = now - self.last_ts, now return elapsed
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.5 # 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) # Filter out the high-frequency noise in velocity 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, curr_ang_vel, linear_vel, angular_vel, dbw_enabled): # Return throttle, brake, steer # If the dbw is turned off, we will reset our PID controller and return nothing if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. curren_vel = self.vel_lpf.filt( current_vel) # Filter out the high-frequency noise in velocity steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # rospy.logwarn("Angular vel : {0}".format(angular_vel)) # rospy.logwarn("Target vel : {0}".format(linear_vel)) # rospy.logwarn("Target angular vel : {0}\n".format(angular_vel)) # rospy.logwarn("Current vel : {0}".format(current_vel)) # rospy.logwarn("Filtered vel : {0}".format(self.vel_lpf.get())) 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 < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # Brake torque( N*m) = deceleration * mass * wheel radius #rospy.loginfo('Throttle: %s, Brake: %s, Steering: %s', 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): rospy.loginfo('TwistController: Initializing...') self.sampling_rate = 50.0 # get parameters for tunning pids self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband # values, decel and accel limit, for regulating discomfortness self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius # for the steering pid self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.min_speed = 0.1 self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle # steering controller self.yaw_controller = YawController(wheel_base=self.wheel_base, steer_ratio=self.steer_ratio, min_speed=self.min_speed, max_lat_accel = self.max_lat_accel, max_steer_angle = self.max_steer_angle) # throttle controller kp = 0.3 ki = 0.1 kd = 0. min_throttle = 0. max_throttle = 0.2 self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 1 / self.sampling_rate self.low_pass_filter_velocity = LowPassFilter(tau, ts) self.last_time = rospy.get_time() rospy.loginfo('TwistController: Complete to initialize') def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enabled): # Return throttle, brake, steer # if dbw is not enabled, you're in the manual mode and you'll # have to reset the pid controller, particularly, integral # term. if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. # the acceleration value is noisy which you don't want it as # it is; need to smooth it out before using it. current_velocity = self.low_pass_filter_velocity.filt(current_velocity) # get value of steering angle steering = self.yaw_controller.get_steering(linear_velocity, angular_velocity, current_velocity) error_velocity = linear_velocity - current_velocity self.last_velocity = current_velocity current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # get the throttle value throttle = self.throttle_controller.step(error_velocity, sample_time) brake = 0 if linear_velocity == 0. and current_velocity < 0.1: throttle = 0. brake = 700 #N*m elif throttle < 0.1 and error_velocity < 0: throttle = 0. decel = max(error_velocity, 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 # Yaw controller self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # PID controller kp = 0.3 ki = 0.1 kd = 0.3 min_throttle = 0.0 max_throttle = 0.2 self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle) # Low pass filter tau = 0.5 # cut-off 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, proposed_linear_velocity, proposed_angular_velocity, curr_linear_velocity, dbw_enabled): # 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 curr_linear_velocity = self.vel_lpf.filt(curr_linear_velocity) steering = self.yaw_controller.get_steering( proposed_linear_velocity, proposed_angular_velocity, curr_linear_velocity ) velocity_error = proposed_linear_velocity - curr_linear_velocity self.last_velocity = curr_linear_velocity curr_time = rospy.get_time() sample_time = curr_time - self.last_time self.last_time = curr_time throttle = self.throttle_controller.step(velocity_error, sample_time) brake = 0 if proposed_linear_velocity == 0.0 and curr_linear_velocity < 0.1: throttle = 0 brake = 700 elif throttle < 0.1 and velocity_error < 0: throttle = 0 decel = max(velocity_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 #wheel_base, steer_ratio, min_speed, 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 values mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 1.0 / 50 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 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_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 = 499 # To hold the car in place if we are stopped at a light traffic elif throttle < 0.1 and vel_err < 0: throttle = 0 decel = max(vel_err / sample_time, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): # Setup PID controller: self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.pid = PID(PID_P, PID_I, PID_D, PID_LIMIT_INTEGRAL, PID_MIN, PID_MAX) # Setup low pass filter tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) # Setup YawController: 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.) min_speed = 1.0 self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # From https://discussions.udacity.com/t/412339 : vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.torque = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.last_time = rospy.get_time() def control(self, dbw_enabled, last_velocity, last_twist_cmd): throttle = brake = steering = 0.0 if dbw_enabled: # track real time delta: current_time = rospy.get_time() time_delta = current_time - self.last_time self.last_time = current_time # abs because the target velocity from the waypoint_follower seems to flip # between positive and negative velocities on large deviations target_velocity = abs(last_twist_cmd.twist.linear.x) current_vel = self.vel_lpf.filt(last_velocity.twist.linear.x) error = target_velocity - current_vel throttle = self.pid.step(error, time_delta) # from walkthrough: if throttle < 0.05 and error < 1: throttle = 0.0 decel = max(error, self.decel_limit) brake = abs(decel) * self.torque # # to prevent windup # self.pid.reset() # hold the car in place when stopped if current_vel < 1 and target_velocity < 0.001: throttle = 0 brake = 400 # to prevent windup self.pid.reset() if brake < self.brake_deadband: brake = 0 steering = self.yaw_controller.get_steering( target_velocity, last_twist_cmd.twist.angular.z, last_velocity.twist.linear.x) else: # dbw not enabled self.pid.reset() 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 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, 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.001 # important to keep low kd = 0.0 min_throttle = 0.0 max_throttle = 0.2 self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle) 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): # 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_velocity=linear_vel, angular_velocity=angular_vel, current_velocity=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(error=vel_error, sample_time=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^s 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.35 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = 0.21 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 0.02 # Sample time corresponding to 50 Hz 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 = 390 #N-m - stop the car at the signal. 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 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.0 mn = 0.0 # min throttle val mx = 0.2 # max throttle val 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): # 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 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 = 400 # N*m to hold car in place if we stop at a light. Accel > -1m/s2 elif throttle < 0.1 and vel_error < 0.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): 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 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 = 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("current_vel={}, linear_vel={}, vel_error={}".format(current_vel, linear_vel,vel_error)) rospy.logwarn("throttle={}, brake={}, steering={}".format(throttle, brake, steering)) return throttle, brake, steering
class Controller(object): def __init__(self, sampling_rate, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): 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.sampling_rate = sampling_rate tau = LPF_ACCEL_TAU # 1/(2pi*tau) = cutoff frequency ts = 1. / self.sampling_rate # Sample time rospy.logwarn('TwistController: Sampling rate = ' + str(self.sampling_rate)) rospy.logwarn('TwistController: ts = ' + str(ts)) self.prev_vel = 0.0 self.current_accel = 0.0 self.acc_lpf = LowPassFilter(tau, ts) self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius # Initialise PID controller for speed control self.pid_spd_ctl = PID(PID_SPDCTL_P, PID_SPDCTL_I, PID_SPDCTL_D, self.decel_limit, self.accel_limit) # second controller to get throttle signal between 0 and 1 self.accel_pid = PID(PID_ACC_P, PID_ACC_I, PID_ACC_D, 0.0, 0.8) # Initialise Yaw controller for steering control self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # self.last_time = rospy.get_time() def reset(self): #self.accel_pid.reset() self.pid_spd_ctl.reset() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): """Returns (throttle, brake, steer), or None""" throttle, brake, steering = 0.0, 0.0, 0.0 if not dbw_enabled: self.reset() return None sampling_time = 1. / self.sampling_rate vel_error = linear_vel - current_vel # calculate current acceleration and smooth using lpf accel_temp = self.sampling_rate * (self.prev_vel - current_vel) # update self.prev_vel = current_vel self.acc_lpf.filt(accel_temp) self.current_accel = self.acc_lpf.get() # use velocity controller compute desired accelaration desired_accel = self.pid_spd_ctl.step(vel_error, sampling_time) if desired_accel > 0.0: if desired_accel < self.accel_limit: throttle = self.accel_pid.step( desired_accel - self.current_accel, sampling_time) else: throttle = self.accel_pid.step( self.accel_limit - self.current_accel, sampling_time) brake = 0.0 else: throttle = 0.0 # reset just to be sure self.accel_pid.reset() if abs(desired_accel) > self.brake_deadband: # don't bother braking unless over the deadband level # make sure we do not brake to hard if abs(desired_accel) > abs(self.decel_limit): brake = abs(self.decel_limit) * self.brake_torque_const else: brake = abs(desired_accel) * self.brake_torque_const steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # 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 teh 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, *args, **kwargs): # TODO: Implement # First store all local variables 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.brake_deadband = kwargs['brake_deadband'] self.accel_limit = kwargs['accel_limit'] self.decel_limit = kwargs['decel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_tourque_const = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius self.tau = 0.5 self.ts = 0.02 # Some simple controllers to well... control... self.yaw_controller = YawController( wheel_base=self.wheel_base, steer_ratio=self.steer_ratio, min_speed=MIN_SPEED, max_lat_accel=self.max_lat_accel, max_steer_angle=self.max_steer_angle) self.throttle_controller = PID(kp=0.3, ki=0.1, kd=0.0, mn=self.decel_limit, mx=self.accel_limit) self.steering_controller = PID(kp=0.5, ki=0.05, kd=0.1, mn=-self.max_steer_angle, mx=self.max_steer_angle) self.vel_lpf = LowPassFilter(tau=self.tau, ts=self.ts) self.steer_lpf = LowPassFilter(tau=2, ts=1) self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # Return throttle, brake, steer if not dbw_enabled: rospy.logwarn("DBW NOT 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) steering = self.steer_lpf.filt(steering) 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(error=vel_error, sample_time=sample_time) brake = 0 if linear_vel == 0.0 and current_vel < 0.1: throttle = 0 brake = 770 # N*m 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 brake = abs(decel) * self.brake_tourque_const if abs( decel) > self.brake_deadband else 0. # brake = abs(decel) * self.brake_tourque_const rospy.logwarn("Throttle = {}; Brake = {}; Steering = {}".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): # DONE : Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.2 ki = 0.01 kd = 0.2 mn = 0.0 #Minimum throttle value mx = 0.2 #Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 #cut-off freq 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): # DONE : 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.0 brake = 700.0 #7 00Nm to hold the car in place 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 Nm 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
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.decel_limit = decel_limit self.accel_limit = accel_limit 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) # Initialize constants for feed-forward-control self.mass = vehicle_mass + fuel_capacity * GAS_DENSITY self.brake_deadband = brake_deadband self.wheel_radius = wheel_radius rospy.loginfo( "DriveByWire with Feed Forward Control: mass={}kg and wheel_radius={}m" .format(self.mass, self.wheel_radius)) # Create controller object min_speed = 0.1 self.controller = Controller(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, decel_limit, accel_limit, wheel_radius, self.mass, P_THROTTLE, P_BRAKE, D_RESIST) # optional logging to a csv-file self.csv_fields = [ 'time', 'x', 'y', 'v_raw', 'v', 'v_d', 'a', 'v_des', 'v_d_des', 'a_des', 'throttle', 'throttle_des', 'brake', 'brake_des', 'steer', 'torque' ] if RECORD_CSV: base_path = os.path.dirname(os.path.abspath(__file__)) base_path = os.path.dirname(base_path) base_path = os.path.dirname(base_path) base_path = os.path.dirname(base_path) base_path = os.path.join(base_path, 'data', 'records') if not os.path.exists(base_path): os.makedirs(base_path) csv_file = os.path.join(base_path, 'driving_log.csv') rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback) rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.steering_callback) rospy.Subscriber('/vehicle/throttle_report', Float32, self.throttle_callback) rospy.Subscriber('/vehicle/brake_report', Float32, self.brake_callback) self.fid = open(csv_file, 'w') self.csv_writer = csv.DictWriter(self.fid, fieldnames=self.csv_fields) self.csv_writer.writeheader() self.csv_data = {key: 0.0 for key in self.csv_fields} rospy.logwarn("created logfile for manual driving: " + self.fid.name) # Subscribe to all required topics rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback, queue_size=2) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=2) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1) # lowpass filter self.velocity_filt = LowPassFilter(0.1, 1.0 / 50.0) self.acceleration_filt = LowPassFilter(0.1, 1.0 / 50.0) self.throttle_filt = LowPassFilter(0.06, 1.0 / 50.0) self.brake_filt = LowPassFilter(0.06, 1.0 / 50.0) # additional variables self.dbw_enabled = False self.current_linear_velocity = None self.angular_velocity = None self.desired_linear_velocity = None self.desired_angular_velocity = None self.last_loop = None self.current_acceleration = None self.desired_velocity_old = 0.0 self.dbw_ready = False self.acc_ready = False self.loop()
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.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 throggle value mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) # Setup low-pass filter parameters for velocity tau = 0.5 # 1/(2*pi*tau) = cutoff frequency (rad/sec) ts = 0.02 # Sample time (correspond to 50 Hz rate) 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 # Pefrom low-pass filtering on current_vel # Slight deviation from walkthrough, # current_vel will be filtered regardless we are in dbw mode or not. # This will ensure state of low-pass filter will be kept updated to # ensure smoothness of filtered-velocity profile current_vel = self.vel_lpf.filt(current_vel) if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 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.0 and current_vel < 0.1: throttle = 0.0 brake = 700 # N-m, to hold car in place if we are stopped at a light, acccel ~ 1 m/sec^2 # Replace brake value with 700 N-m for Carla elif throttle < 0.1 and vel_error < 0.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, steering
class Controller(object): def __init__(self, **kwargs): self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lateral_acceleration = kwargs['max_lateral_acceleration'] self.max_steer_angle = kwargs['max_steer_angle'] self.vehicle_mass = kwargs['vehicle_mass'] self.wheel_radius = kwargs['wheel_radius'] self.fuel_capacity = kwargs['fuel_capacity'] self.acceleration_limit = kwargs['acceleration_limit'] self.deceleration_limit = kwargs['deceleration_limit'] self.brake_deadband = kwargs['brake_deadband'] # add mass of fuel to vehicle mass self.vehicle_mass = self.vehicle_mass + (self.fuel_capacity * GAS_DENSITY) self.brake_torque = self.vehicle_mass * self.wheel_radius # get max allowed velocity in kmph self.MAX_VELOCITY = rospy.get_param('/waypoint_loader/velocity') # convert velocity to m/s self.MAX_VELOCITY = self.MAX_VELOCITY * 0.2778 self.throttle_PID = PID(THROTTLE_KP, THROTTLE_KI, THROTTLE_KD, -1*self.acceleration_limit, self.acceleration_limit) yaw_controller_init_kwargs = { 'wheel_base' : self.wheel_base, 'steer_ratio' : self.steer_ratio, 'max_lateral_acceleration' : self.max_lateral_acceleration, 'min_speed' : 0.1, 'max_steer_angle' : self.max_steer_angle } self.yaw_controller = YawController(**yaw_controller_init_kwargs) self.throttle_lowpass = LowPassFilter(THROTTLE_TAU,THROTTLE_TS) self.last_time = rospy.get_time() def control(self, **kwargs): ''' controls the vehicle taking into consideration the twist_cmd Params: - `target_linear_velocity` is the desired linear velocity the car should reach - `target_angular_velocity` is the desired angular velocity the car should reach - `current_velocity` is the current velocity of the car - `dbw_enabled` - Boolean, whether auto-drive is enabled or not - `dt` - time in seconds, between each twist_cmd Returns throttle, brake and steer values ''' target_linear_velocity = kwargs['target_linear_velocity'] target_angular_velocity = kwargs['target_angular_velocity'] current_velocity = kwargs['current_velocity'] dbw_enabled = kwargs['dbw_enabled'] dt = kwargs['dt'] rospy.loginfo('Current velocity: {}'.format(current_velocity)) rospy.loginfo('target_linear_velocity : {}'.format(target_linear_velocity)) # pass current_velocity through low pass filter to reduce high freq variations current_velocity = self.throttle_lowpass.filter(current_velocity) # if driver has taken control, reset throttle PID to avoid accumulation # of error if not dbw_enabled: self.throttle_PID.reset() self.throttle_lowpass.reset() error = target_linear_velocity - current_velocity throttle = self.throttle_PID.step(error, dt) steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_velocity) #steer = self.steer_PID.step(steer,dt) #steer = self.steer_lowpass.filter(steer) # if target linear velocity is very less, apply hard brake brake = 0.0 if target_linear_velocity == 0.0 and current_velocity < 0.1: brake = 400 throttle = 0.0 elif throttle < 0.1 and error < 0: throttle = 0.0 decel = min(abs(error/dt), abs(self.deceleration_limit)) brake = decel * self.brake_torque 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 = 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 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)) # 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(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) # rospy.logwarn("Throttle: {}".format(throttle)) 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 < .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 YawController(object): def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.min_speed = 5 self.max_lat_accel = max_lat_accel self.previous_dbw_enabled = False self.min_angle = -max_steer_angle self.max_angle = max_steer_angle self.linear_pid = PID(0.9, 0.001, 0.0004, self.min_angle, self.max_angle) #self.cte_pid = PID(0.4, 0.1, 0.1, self.min_angle, self.max_angle) self.cte_pid = PID(0.4, 0.1, 0.2, self.min_angle, self.max_angle) self.tau = 0.2 self.ts = 0.1 self.low_pass_filter = LowPassFilter(self.tau, self.ts) def get_angle(self, radius, current_velocity): angle = math.atan(self.wheel_base / radius) * self.steer_ratio return max(self.min_angle, min(self.max_angle, angle)) def get_steering_calculated(self, linear_velocity, angular_velocity, current_velocity): """ Formulas: angular_velocity_new / current_velocity = angular_velocity_old / linear_velocity radius = current_velocity / angular_velocity_new angle = atan(wheel_base / radius) * self.steer_ratio """ angular_velocity = current_velocity * angular_velocity / linear_velocity if abs(linear_velocity) > 0. else 0. if abs(current_velocity) > 0.1: max_yaw_rate = abs(self.max_lat_accel / current_velocity) angular_velocity = max(-max_yaw_rate, min(max_yaw_rate, angular_velocity)) return self.get_angle(max(current_velocity, self.min_speed) / angular_velocity, current_velocity) if abs(angular_velocity) > 0. else 0.0; def get_steering_pid(self, angular_velocity, angular_current, dbw_enabled): angular_error = angular_velocity - angular_current sample_step = 0.02 if not(self.previous_dbw_enabled) and dbw_enabled: self.previous_dbw_enabled = True self.linear_pid.reset() self.low_pass_filter = LowPassFilter(self.tau, self.ts) else: self.previous_dbw_enabled = False steering = self.linear_pid.step(angular_error, sample_step) steering = self.low_pass_filter.filt(steering) return steering def get_steering_pid_cte(self, final_waypoint1, final_waypoint2, current_location, dbw_enabled): steering = 0 if final_waypoint1 and final_waypoint2: # vector from car to first way point a = np.array([current_location.x - final_waypoint1.pose.pose.position.x, current_location.y - final_waypoint1.pose.pose.position.y, current_location.z - final_waypoint1.pose.pose.position.z]) # vector from first to second way point b = np.array([final_waypoint2.pose.pose.position.x-final_waypoint1.pose.pose.position.x, final_waypoint2.pose.pose.position.y-final_waypoint1.pose.pose.position.y, final_waypoint2.pose.pose.position.z-final_waypoint1.pose.pose.position.z]) # progress on vector b # term = (a.b / euclidian_norm(b)**2) * b where a.b is dot product # term = progress * b => progress = term / b => progress = (a.b / euclidian_norm(b)**2) progress = (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]) # position where the car should be: waypoint1 + progress * b error_pos = np.array([final_waypoint1.pose.pose.position.x, final_waypoint1.pose.pose.position.y, final_waypoint1.pose.pose.position.z]) + progress * b # difference vector between where the car should be and where the car currently is error = (error_pos - np.array([current_location.x, current_location.y, current_location.z])) # is ideal track (b) left or right of the car's current heading? dot_product = a[0]*-b[1] + a[1]*b[0] direction = 1.0 if dot_product >= 0: direction = -1.0 else: direction = 1.0 # Cross track error is the squared euclidian norm of the error vector: CTE = direction*euclidian_norm(error)**2 cte = direction * math.sqrt(error[0]*error[0]+error[1]*error[1]+error[2]*error[2]) sample_step = 0.02 if not(self.previous_dbw_enabled) and dbw_enabled: self.previous_dbw_enabled = True self.cte_pid.reset() self.low_pass_filter = LowPassFilter(self.tau, self.ts) else: self.previous_dbw_enabled = False steering = self.cte_pid.step(cte, sample_step) #steering = self.low_pass_filter.filt(steering) return steering
class Controller(object): def __init__(self, params): self.yaw_controller = YawController( wheel_base = params['wheel_base'], steer_ratio = params['steer_ratio'], min_speed = params['min_speed'], max_lat_accel = params['max_lat_accel'], max_steer_angle = params['max_steer_angle']) self.prev_linear_velocity = 0.0 self.filter_steer = True if self.filter_steer: self.steer_filter = LowPassFilter(time_interval=0.1, time_constant=1.0) self.filter_throttle = True if self.filter_throttle: self.throttle_filter = LowPassFilter(time_interval=0.1, time_constant=0.1) self.break_constant = 0.1 self.vehicle_mass = params['vehicle_mass'] self.fuel_capacity = params['fuel_capacity'] self.brake_deadband = params['brake_deadband'] self.wheel_radius = params['wheel_radius'] self.sample_rate = params['sample_rate'] self.throttle_pid = PID( 10., .0, 5., 0, 1 ) self.steer_pid = PID( 1.0, .0, 5.0, -params['max_steer_angle'], params['max_steer_angle'] ) # assume tank is full when computing total mass of car self.total_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY def control(self, linear_velocity, angular_velocity, current_velocity, enabled = True): velocity_diff = linear_velocity - current_velocity target_velocity_diff = linear_velocity - self.prev_linear_velocity time_interval = 1.0 / self.sample_rate throttle = 0.0 brake = 0.0 steer = 0.0 if enabled: if target_velocity_diff < 0.0 or linear_velocity < 1.0: # Brake in torque [N*m] acc = velocity_diff/time_interval # Required acceleration brake = max(self.break_constant*math.fabs(acc), 0.19) * self.total_mass * self.wheel_radius self.throttle_pid.reset() else: throttle = self.throttle_pid.step(velocity_diff, time_interval) # Pass the low-pass filter if self.filter_throttle: throttle = self.throttle_filter.filt(throttle) if USE_STEER_PID: steer = self.steer_pid.step(angular_velocity, time_interval) else: steer = self.yaw_controller.get_steering( linear_velocity, angular_velocity, current_velocity ) # Pass the low-pass filter if self.filter_steer: steer = self.steer_filter.filt(steer) # Update the previous target self.prev_linear_velocity = linear_velocity else: self.throttle_pid.reset() self.steer_pid.reset() self.throttle_filter.reset() self.steer_filter.reset() # Logwarn for Debugging PID # run ```rosrun rqt_pt rqt_plot``` and set topics for plotting the actual velocity # rospy.logwarn("Throttle : {}".format(throttle)) # rospy.logwarn(" Brake : {}".format(brake)) # rospy.logwarn(" Steer : {}".format(steer)) # rospy.logwarn("--- ") 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): min_speed = 0.1 self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.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 # Throttle controllers kp = 0.3 ki = 0.1 kd = 0.0 min_throttle = 0.0 max_throttle = 0.2 self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle) # velocity needs to be filtered because its noise tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) self.last_time = rospy.get_time() def control(self, dbw_enabled, current_vel, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. # filt velocity 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 brake = 400 self.throttle_controller.reset() 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, 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.7 ki = 0.00007 kd = 0.1 mn = 0. # Minimum throttle value mx = 0.2 # Maximum throttle value self.throttle_pid_controller = PID(kp, ki, kd, mn, mx) v_tau = 0.5 v_ts = .02 self.vel_lpf = LowPassFilter(v_tau, v_ts) s_tau = 0.5 s_ts = .02 self.steer_lpf = LowPassFilter(s_tau, s_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): # Compute sample time current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # DBW not enabled if not dbw_enabled: self.throttle_pid_controller.reset() # Log if (current_time - self.log_time) > LOGGING_RATE: rospy.logwarn("[CONTROLLER] enabled={}".format(dbw_enabled)) self.log_time = current_time return 0.0, 0.0, 0.0 # Steering controller steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # steering = self.steer_lpf.filt(steering) # Compute linear velocity error vel_error = linear_vel - current_vel self.last_vel = current_vel # Velocity controller throttle = self.throttle_pid_controller.step(vel_error, sample_time) throttle = self.vel_lpf.filt(throttle) brake = 0.0 # Car stopped if linear_vel == 0.0 and current_vel < 0.1: throttle = 0.0 brake = STOP_BRAKE # Car braking 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 # Log if (current_time - self.log_time) > LOGGING_RATE: rospy.logwarn( "[CONTROLLER] current_lin_vel={:.2f}, target_lin_vel={:.2f}, target_ang_vel={:.2f}" .format(current_vel, linear_vel, angular_vel)) rospy.logwarn( "[CONTROLLER] throttle={:.2f}, brake={:.2f}, steering={:.2f}". format(throttle, brake, steering)) self.log_time = current_time 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.0 mn = 0.0 # min throttle mx = 0.5 # max throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # tau = 1/(2*pi*f_cutoff) ts = 0.02 # t_sample 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.0, 0.0 current_vel = self.vel_lpf.filt(current_vel) # some debug logging ... # rospy.logwarn("Target velocity: {0}".format(linear_vel)) # rospy.logwarn("Current velocity: {0}".format(current_vel)) # rospy.logwarn("Angular velocity: {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.0 and current_vel < 0.1: throttle = 0 brake = 400 # [Nm] - to hold at standstill 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 in [Nm] 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): kp = 0.3 ki = 0.01 kd = 0.0 mn = 0. #Minimum throttle value mx = 0.5 #Maximum throttle value self.steering_controller = PID(kp, ki, kd, mn, mx) self.throttle_controller = PID(kp, ki, kd, mn, mx) self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) 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, curr_ang_vel, linear_vel, angular_vel, dbw_enabled): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel steering_estimate = self.yaw_controller.get_steering( linear_vel, angular_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 ang_vel_error = angular_vel - curr_ang_vel steering_correction = self.steering_controller.step( ang_vel_error, sample_time) steering = steering_estimate + steering_correction if linear_vel == 0 and current_vel < 0.1: throttle = 0.0 brake = 400 elif throttle < 0.1 and vel_error < 0: throttle = 0.0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * (self.vehicle_mass + GAS_DENSITY * self.fuel_capacity) * self.wheel_radius return throttle, brake, steering