class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, brake_deadband, accel_limit, decel_limit, vehicle_mass, wheel_radius, fuel_capacity, sample_freq): self.dt = 1.0 / sample_freq self.brake_deadband = brake_deadband self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius self.fuel_capacity = fuel_capacity self.decel_limit = decel_limit self.accel_limit = accel_limit self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.accel_controller = PID(kp=0.45, ki=0.02, kd=0.01, mn=decel_limit, mx=accel_limit) self.throttle_controller = PID(kp=1.0, ki=0.001, kd=0.10, mn=0.0, mx=0.2) self.lowpass_filter = LowPassFilter(0.15, self.dt) def control(self, dbw_enabled, target_linear_velocity, target_angular_velocity, current_velocity): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. vel_error = target_linear_velocity - current_velocity raw_accel = self.accel_controller.step(vel_error, self.dt) self.lowpass_filter.filt(raw_accel) accel = self.lowpass_filter.get() brake = 0.0 throttle = 0.0 steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_velocity) if accel > 0.0: throttle = self.throttle_controller.step(accel, self.dt) if target_linear_velocity == 0 and current_velocity < 0.1: throttle = 0 brake = 400 elif throttle < .1 and accel < 0: throttle = 0 accel = max(accel, self.decel_limit) brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius * 2 return throttle, brake, steer
class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, decel_limit, accel_limit, wheel_radius, vehicle_mass ): self.wheel_radius = wheel_radius self.vehicle_mass = vehicle_mass self.YawController = YawController( wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # self.PID = PID(0.9, 0.0005, 0.075, decel_limit, accel_limit) # self.PID = PID(4.0, 0.001, 0.05, decel_limit, accel_limit) self.PID = PID(1.0, 0.0, 0.001, decel_limit, accel_limit) self.low_pass_filer_vel = LowPassFilter(10.0, 1.0) self.lastT = None self.last_dbw_enabled = False def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enabled): if dbw_enabled is True and self.last_dbw_enabled is False: #restart self.lastT = None self.PID.reset() self.last_dbw_enabled = dbw_enabled self.low_pass_filer_vel.filt(linear_velocity) linear_velocity = self.low_pass_filer_vel.get() velocity_error = linear_velocity - current_velocity T = rospy.get_time() dt = T - self.lastT if self.lastT else 0.05 self.lastT = T a = self.PID.step(velocity_error,dt) if a > 0.0: throttle, brake = min(a, 1.0), 0.0 else: throttle, brake = 0.0, self.wheel_radius * self.vehicle_mass * math.fabs(a) steer = self.YawController.get_steering(linear_velocity, angular_velocity, current_velocity) print('--- pid: {} {} {}'.format(velocity_error, throttle, brake)) return throttle, brake, steer
class Controller(object): def __init__(self, car_params): self.car_params = car_params self.lpf_accel = LowPassFilter(tau = 0.5, ts = 0.02) self.accel_pid = PID(kp = 0.4, ki = 0.1, kd = 0.0, mn = 0.0, mx = 1.0) self.speed_pid = PID(kp = 2.0, ki = 0.0, kd = 0.0, mn = car_params.decel_limit, mx = car_params.accel_limit) self.yaw_control = YawController(car_params.wheel_base, car_params.steer_ratio, 4. * ONE_MPH, car_params.max_lat_accel, car_params.max_steer_angle) self.prev_velocity = 0 def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, dbw_enabled, control_period): # TODO: Change the arg, kwarg list to suit your needs accel = (current_linear_velocity - self.prev_velocity) / control_period self.lpf_accel.filt(accel) self.prev_velocity = current_linear_velocity vel_error = proposed_linear_velocity - current_linear_velocity if abs(proposed_linear_velocity) < ONE_MPH: self.speed_pid.reset() accel_cmd = self.speed_pid.step(vel_error, control_period) min_speed = ONE_MPH * 5 if proposed_linear_velocity < 0.01: accel_cmd = min(accel_cmd, -530. / self.car_params.vehicle_mass / self.car_params.wheel_radius) elif proposed_linear_velocity < min_speed: proposed_angular_velocity *= min_speed/proposed_linear_velocity proposed_linear_velocity = min_speed throttle = brake = steer = 0 if dbw_enabled: if accel_cmd >= 0: throttle = self.accel_pid.step(accel_cmd - self.lpf_accel.get(),control_period) else: self.accel_pid.reset() if (accel_cmd < -self.car_params.brake_deadband) or (proposed_linear_velocity < min_speed): brake = -accel_cmd * self.car_params.vehicle_mass * self.car_params.wheel_radius steer = self.yaw_control.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) rospy.loginfo('%s,%s,%s',throttle, brake, steer); else : self.accel_pid.reset() self.speed_pid.reset() # Return throttle, brake, steer return throttle, brake, steer
class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, brake_deadband, accel_limit, decel_limit, vehicle_mass, wheel_radius, fuel_capacity, sample_freq): # TODO: Implement self.dt = 1.0/sample_freq self.brake_deadband = brake_deadband self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius self.fuel_capacity = fuel_capacity self.decel_limit = decel_limit self.accel_limit = accel_limit #initalize the YAW controller, accel controller, throttle controller and low pass filter self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.accel_controller = PID(kp=0.45, ki=0.02, kd=0.01, mn=decel_limit, mx=accel_limit) self.throttle_controller = PID(kp=1.0, ki=0.001, kd=0.10, mn=0.0, mx=0.2) self.lowpass_filter = LowPassFilter(0.15, self.dt) def control(self, dbw_enabled, target_linear_velocity, target_angular_velocity, current_velocity): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer #check if dbw enabled then no need to compute the brake, steering and throttle values, return 0 if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. vel_error = target_linear_velocity - current_velocity raw_accel = self.accel_controller.step(vel_error, self.dt) self.lowpass_filter.filt(raw_accel) accel = self.lowpass_filter.get() brake = 0.0 throttle = 0.0 #Get sterring values after getting angular and current velocity steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_velocity) if accel > 0.0: throttle = self.throttle_controller.step(accel, self.dt) if target_linear_velocity == 0 and current_velocity < 0.1: throttle = 0 # To prevent Carla from moving requires about 700 Nm of torque brake = 400 elif throttle < .1 and accel < 0: throttle = 0 accel = max(accel, self.decel_limit) brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius * 2 return throttle, brake, steer
class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, brake_deadband, accel_limit, decel_limit, vehicle_mass, wheel_radius, fuel_capacity, sample_freq): self.dt = 1.0 / sample_freq self.brake_deadband = brake_deadband self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius self.fuel_capacity = fuel_capacity self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.accel_controller = PID(kp=0.45, ki=0.02, kd=0.01, mn=decel_limit, mx=accel_limit) self.throttle_controller = PID(kp=1.0, ki=0.001, kd=0.10, mn=0.0, mx=1.0) self.lowpass_filter = LowPassFilter(0.15, self.dt) def control(self, target_linear_velocity, target_angular_velocity, current_velocity): vel_error = target_linear_velocity - current_velocity raw_accel = self.accel_controller.step(vel_error, self.dt) self.lowpass_filter.filt(raw_accel) accel = self.lowpass_filter.get() brake = 0.0 throttle = 0.0 steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_velocity) if accel > 0.0: throttle = self.throttle_controller.step(accel, self.dt) if accel <= 0.0: self.throttle_controller.reset() if abs(accel) > self.brake_deadband: # Assumes wheel is point mass brake = abs(accel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius return throttle, brake, steer
class Controller(object): # constructor that takes all the car's caracteristics def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, decel_limit, accel_limit): # TODO: Implement # use the provided YawController and initialize it self.YawController = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # Setup the PID controller and the lowpass # kp, ki and kd were found using trial and error method from the semester project self.PID = PID(4.0, 0.001, 0.05, decel_limit, accel_limit) self.low_pass_filer_vel = LowPassFilter(10.0, 1.0) self.lastTime = None self.last_dbw_enabled = False #Control function def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enabled): # take care of the dbw_enabled. n case it is turned down, reset the PID. if dbw_enabled is True and self.last_dbw_enabled is False: #restart self.lastT = None self.PID.reset() self.last_dbw_enabled = dbw_enabled self.low_pass_filer_vel.filt(linear_velocity) linear_velocity = self.low_pass_filer_vel.get() velocity_error = linear_velocity - current_velocity T = rospy.get_time() dt = T - self.lastTime if self.lastTime else 0.05 self.lastTime = T a = self.PID.step(velocity_error, dt) if a > 0.0: throttle, brake = a, 0.0 else: throttle, brake = 0.0, math.fabs(a) steer = self.YawController.get_steering(linear_velocity, angular_velocity, current_velocity) # print('--- pid: {} {}'.format(velocity_error, a)) return throttle, brake, steer
class Controller(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, accel_limit, decel_limit, loop_frequency, vehicle_mass, wheel_radius): self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_steer_angle = max_steer_angle self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius self.steering_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.throttle_controller = PID(0.15, 0.0, 0.09, mn=decel_limit, mx=accel_limit) self.low_pass_filter = LowPassFilter(12.0, 1) self.last_timestamp = None def control(self, target_angular_velocity, target_linear_velocity, current_angular_velocity, current_linear_velocity, dbw_enabled): # Return throttle, brake, steer if target_linear_velocity < 0.5 or not dbw_enabled: self.reset() self.last_timestamp = rospy.Time.now() return 0., 35., 0. steer = self.steering_controller.get_steering(target_linear_velocity, target_angular_velocity, current_linear_velocity) throttle = 0. brake = 0. current_timestamp = rospy.Time.now() if self.last_timestamp != None: dt = (current_timestamp - self.last_timestamp).nsecs / 1e9 cte = target_linear_velocity - current_linear_velocity acceleration = self.throttle_controller.step(cte, dt) filtvalue = self.low_pass_filter.filt(acceleration) if self.low_pass_filter.ready: acceleration = self.low_pass_filter.get() if acceleration >= 0: throttle = acceleration brake = 0. else: brake = self.vehicle_mass * abs( acceleration) * self.wheel_radius throttle = 0. self.last_timestamp = current_timestamp rospy.loginfo( 'SENDING - [throttle,brake,steer]:[{:.4f},{:.4f},{:.4f}], [cA,cL]:[{:.4f},{:.4f}]m [tA, tL]:[{:.4f},{:.4f}]' .format(throttle, brake, steer, current_angular_velocity, current_linear_velocity, target_angular_velocity, target_linear_velocity)) return throttle, brake, steer def reset(self): """ Reset the controller's state. """ self.throttle_controller.reset() self.low_pass_filter = LowPassFilter(12.0, 1)
class Controller(object): # TODO: Implement 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) # PID hyperparameters kp = 0.3 ki = 0.1 kd = 0 # Throttle values min, max mn = 0. mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) # Low pass filter tau = 0.5 # cutoff-frequence ts = 0.02 # sample time = 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) if IS_DEBUG: rospy.loginfo("Angular vel: {0}".format(angular_vel)) rospy.loginfo("Target vel: {0}".format(linear_vel)) rospy.loginfo("Target ang vel: {0}".format(angular_vel)) rospy.loginfo("Current vel: {0}".format(current_vel)) rospy.loginfo("Filtered vel: {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # Nm --> needed to hold car in place elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) # limit brake value brake = abs(decel) * self.vehicle_mass * self.wheel_radius if IS_DEBUG: rospy.logwarn("vel error: {0}, throttle: {1}, brake: {2}".format( vel_error, throttle, brake)) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): self.last_time = rospy.get_time() self.yaw_control = yaw_controller.YawController( kwargs['wheel_base'], kwargs['steer_ratio'], ONE_MPH, kwargs['max_lat_accel'], kwargs['max_steer_angle']) self.brake_deadband = kwargs['brake_deadband'] self.fuel_capacity = kwargs['fuel_capacity'] self.vehicle_mass = kwargs['vehicle_mass'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] # Throttle PID and filter self.throttle_PID = pid.PID(KP_accel, KI_accel, KD_accel, 0, 1) self.throttle_lpf = LowPassFilter(0.02, 0.03) # Steering filter self.steer_lpf = LowPassFilter(0.015, 0.01) def control(self, desired_velocity_x, desired_vel_angular_z, current_velocity_x, dbw_enabled): actual_time = rospy.get_time() throttle = 0 brake = 0 steer = self.yaw_control.get_steering(desired_velocity_x, desired_vel_angular_z, current_velocity_x) # Feed steering filter steer = self.steer_lpf.filt(steer) if not dbw_enabled: # Reset Throttle PID if dbw is not enable self.throttle_PID.reset() #Update timestamp self.last_time = actual_time return 0., 0., 0. # Calculate error to correct diff_velocity = desired_velocity_x - current_velocity_x diff_time = actual_time - self.last_time self.last_time = actual_time #Limit the new acceleration value accordingly new_accel = min(max(self.decel_limit, diff_velocity), self.accel_limit) # Feed throttle filter self.throttle_lpf.filt(new_accel) throttle = self.throttle_PID.step(self.throttle_lpf.get(), diff_time) # If the difference is positive, them we accelerate if throttle > 0: brake = 0 # If the difference is negative, them we deacelerate else: throttle = 0 # Torque = F * radius -> F = mass * acceleration. Include mass of fuel brake = abs(new_accel * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + DRIVER_WEIGHT) * self.wheel_radius) rospy.loginfo("DV: " + str(desired_velocity_x) + " DA: " + str(desired_vel_angular_z) + " CV: " + str(current_velocity_x) + " new_accel: " + str(new_accel)) rospy.loginfo("Steer: " + str(steer) + " Throttle: " + str(throttle) + " Brake: " + str(brake)) 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 rospy.logwarn( "[Controller Init] vehicle_mass: {0}".format(vehicle_mass)) rospy.logwarn("[Controller Init] decel_limit: {0}".format(decel_limit)) rospy.logwarn("[Controller Init] accel_limit: {0}".format(accel_limit)) rospy.logwarn( "[Controller Init] wheel_radius: {0}".format(wheel_radius)) min_speed = 0.1 self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) #original #original #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) #0530, add steering and throttle controller ################### # Throttle ################### #highway ok #testing lot ok, need use 10kms throttle_kp = 0.3 throttle_ki = 0.1 throttle_kd = 0.005 #testing lot ok, need use 10kms #throttle_kp = 0.8 #throttle_ki = 0.002 #throttle_kd = 0.3 #not good in testing log #throttle_kp = 0.8 #throttle_ki = 0.00 #throttle_kd = 0.06 min_throttle = 0.0 # Minimum throttle value #max_throttle = 0.2 # Maximum throttle value (should be OK for simulation, not for Carla!) #max_throttle = 0.5*accel_limit max_throttle = accel_limit #self.throttle_controller =PID(2.0, 0.4, 0.1, min_throttle, max_throttle) self.throttle_controller = PID(throttle_kp, throttle_ki, throttle_kd, min_throttle, max_throttle) ################### # Steering ################### #highway ok #testing lot ok, need use 10kms steer_kp = 0.4 steer_ki = 0.1 steer_kd = 0.005 #testing lot ok, need use 10kms #steer_kp = 1.15 #steer_ki = 0.001 #steer_kd = 0.1 #not good in testing log #steer_kp = 0.5 #steer_ki = 0.00 #steer_kd = 0.2 min_steer = -max_steer_angle max_steer = max_steer_angle self.steering_controller = PID(steer_kp, steer_ki, steer_kd, min_steer, max_steer) #is for filter out the high frequency noise of volcity tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 #Sample time self.vel_lpf = LowPassFilter(tau, ts) # catch the input self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle self.last_time = rospy.get_time() #pass # this fun will be call 50Hz, from dbw_node def control(self, current_vel, curr_ang_vel, dbw_enabled, target_vel, target_ang_vel, get_first_image): # TODO: Change the arg, kwarg list to suit your needs if not dbw_enabled: rospy.logwarn( "dbw_enabled: {0}, reset controller".format(dbw_enabled)) #we also need to reset the PID controller to prevent #the error accumulating. # If we don't reset that, when we put dbw_enable back on , the PID will # use the wrong error to do something wrong behavior. ## because there is a integral term in PID. self.steering_controller.reset() self.throttle_controller.reset() return 0., 0., 0. if get_first_image == False: rospy.logwarn("get_first_image: {0}, return 0.,750.,0.".format( get_first_image)) return 0., 750., 0. #so, if dbw is enable current_vel = self.vel_lpf.filt(current_vel) rospy.logwarn("Target vel: {0}".format(target_vel)) rospy.logwarn("Target angular vel: {0}\n".format(target_ang_vel)) rospy.logwarn("current_vel: {0}".format(current_vel)) rospy.logwarn("current target_ang_vel: {0}".format(curr_ang_vel)) rospy.logwarn("filtered vel: {0}".format(self.vel_lpf.get())) ############## # time ############## current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time ############ # steering ############ #use Yaw controller to get the steering steering_base = self.yaw_controller.get_steering( target_vel, target_ang_vel, current_vel) #rospy.logwarn("steering: {0}\n".format(steering)) #0530 target_ang_vel_error = target_ang_vel - curr_ang_vel steering_correction = self.steering_controller.step( target_ang_vel_error, sample_time) steering_total = steering_base + steering_correction steering = max(min(self.max_steer_angle, steering_total), -self.max_steer_angle) ############## # throttle ############## #get the vel diff erro between the the vel we want to be (target_vel) v.s. the current_vel # the target_vel comes from twist_cmd, which is the waypoint_follower published, is the target vel # from waypoint updater vel_error = target_vel - current_vel self.last_vel = current_vel #use PID controller to get the proper throttle under the vel_error and the sample time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 ################ #some constrain ################ rospy.logwarn("[some constrain] target_vel: {0}".format(target_vel)) rospy.logwarn("[some constrain] current_vel: {0}".format(current_vel)) rospy.logwarn("[some constrain] vel_error: {0}".format(vel_error)) rospy.logwarn("[some constrain] throttle: {0}".format(throttle)) # if the target vel is 0, and we are very slow <0.1, set the throttle to 0 directly to stop if target_vel == 0. and current_vel < 0.1: rospy.logwarn( "[twist_controller] target vel =0, and current vel is really small, directly STOP" ) throttle = 0 brake = 750 #N*m to hold the car in place if we are stopped at a light. Acceleration 1m/s^2 rospy.logwarn("[directly STOP] throttle: {0}".format(throttle)) rospy.logwarn("[directly STOP] brake: {0}".format(brake)) rospy.logwarn("[directly STOP] steering: {0}".format(steering)) # vel_erro < 0 means the current car vel too fast than we want, and we still put small throttle # we directly stop to add throttle, and get the real brake, because this time # the car still moving, we need the real brake result from the vel, and mass, whell radius #elif throttle < 0.1 and vel_error <0: elif vel_error < 0: rospy.logwarn("[twist_controller] slowing down") throttle = 0 #decel is our desired deacceleration decel = max(vel_error, self.decel_limit) rospy.logwarn("[slowing down] self.decel_limit: {0}".format( self.decel_limit)) rospy.logwarn("[slowing down] vel_error: {0}".format(vel_error)) rospy.logwarn("[slowing down] decel: {0}".format(decel)) # vehicle_mass in kilograms, wheel_radius in meters # use abs is because the simluator, the de-acceleration should be negative, but # the simulator get the positive number as the brake value #brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque N*m brake = abs(decel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius rospy.logwarn("[slowing down] self.wheel_radius: {0}".format( self.wheel_radius)) rospy.logwarn("[slowing down] self.vehicle_mass: {0}".format( self.vehicle_mass)) rospy.logwarn("[slowing down] throttle: {0}".format(throttle)) rospy.logwarn("[slowing down] brake: {0}".format(brake)) rospy.logwarn("[slowing down] steering: {0}".format(steering)) else: rospy.logwarn("[donothing] throttle: {0}".format(throttle)) rospy.logwarn("[donothing] brake: {0}".format(brake)) rospy.logwarn("[donothing] steering: {0}".format(steering)) # Return throttle(min is 0.1 m/s), brake, steer #hard code to keep car move stright #throttle = 1.0 #brake = 0 #steering = 0 rospy.logwarn("[output] throttle: {0}".format(throttle)) rospy.logwarn("[output] brake: {0}".format(brake)) rospy.logwarn("[output] steering: {0}".format(steering)) return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed=0.1, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) # Throttle PID controller parameters # Tuned using Ziegler-Nichols method, temporarily using mx=1.0 # see https://en.wikipedia.org/wiki/PID_controller#Ziegler%E2%80%93Nichols_method # Which gives the parameters Ku=2.1, Tu=1.4s # Which in turn results in: kp = 1.26 ki = 1.8 kd = 0.294 # Throttle range, 0 is minimum. 0.2 max for safety and comfort, real max is 1.0 mn = 0.0 mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) # Lowpass filter for measured velocity, the sensor is noisy tau = 0.5 # 1/(2*pi*tau) = cutoff frequency ts = 0.02 # Sample time at 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): if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 if DEBUG_LOG: rospy.logwarn("Angular vel: {0}".format(angular_vel)) rospy.logwarn("Target velocity: {0}".format(linear_vel)) rospy.logwarn("Target angular velocity: {0}".format(angular_vel)) rospy.logwarn("Current velocity: {0}".format(current_vel)) rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get())) 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 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 we want to stop and are not going too fast, brake with known-good torque for staying still if linear_vel == 0.0 and current_vel < 0.1: throttle = 0 brake = 700 # Nm - according to Slack comments, minimum 550 Nm required, but use 700 to be safer # If no/low throttle was indicated and we want to slow down (decelerate), give zero throttle and brake elif throttle < 0.1 and vel_error < 0.0: throttle = 0.0 decel = max(vel_error, self.decel_limit) # decel_limit is negative as well brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # Braking torque in Nm # return 1., 0., 0. # For debugging only, full gas ahead! 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. mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cut_off frequency ts = 0.02 # sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.wheel_base = wheel_base 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, angular_vel, linear_vel, 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. current_vel = self.vel_lpf.filt(current_vel) debug_messages("Angular vel: ", angular_vel) debug_messages("Linear vel: ", linear_vel) debug_messages("Target Angular vel: ", angular_vel) debug_messages("Current vel: ", current_vel) debug_messages("Filtered vel: ", self.vel_lpf.get()) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # torque required at standstill as per updated walkthrough 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 Nm # rospy.logwarn("braking at {0} Nm".format(brake)) 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.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.last_velocity = 0. self.lpf_fuel = LowPassFilter(60.0, 0.1) self.lpf_accel = LowPassFilter(0.5, 0.02) #self.lpf_angv = LowPassFilter(1.0, 1) self.accel_pid = PID(0.4, 0.1, 0.0, 0.0, 1.0) self.speed_pid = PID(2.0, 0.0, 0.0, self.decel_limit, self.accel_limit) self.yaw_control = YawController(wheel_base, steer_ratio, 4. * ONE_MPH, max_lat_accel, max_steer_angle) self.last_ts = None def set_fuel(self, level): self.lpf_fuel.filt(level) def get_vehicle_mass(self): return self.vehicle_mass + \ self.lpf_fuel.get() / 100.0 * self.fuel_capacity * GAS_DENSITY def time_elasped(self, msg=None): now = rospy.get_time() if self.last_ts is None: self.last_ts = now elasped, self.last_ts = now - self.last_ts, now return elasped def control(self, current_velocity, dbw_enabled, linear_velocity, angular_velocity): #rospy.logwarn("Current Velocity = %f, Linear Velocity = %f, Angular Velocity = %f", current_velocity, linear_velocity, angular_velocity) time_elasped = self.time_elasped() given_av = angular_velocity if time_elasped > 1. / 5 or time_elasped < 1e-4: self.speed_pid.reset() self.accel_pid.reset() self.last_velocity = current_velocity return 0., 0., 0. vehicle_mass = self.get_vehicle_mass() vel_error = linear_velocity - current_velocity #rospy.logwarn("Velocity Error : %s",vel_error) if abs(linear_velocity) < ONE_MPH: self.speed_pid.reset() accel_cmd = self.speed_pid.step(vel_error, time_elasped) #rospy.logwarn("Velocity step : %s",accel_cmd) min_speed = ONE_MPH * 5 if linear_velocity < 0.01: #rospy.logwarn("Current Velocity = %f, Linear Velocity = %f, Angular Velocity = %f", current_velocity, linear_velocity, angular_velocity) #rospy.logwarn("Velocity Error : %s",vel_error) #rospy.logwarn("Velocity step Before: %s",accel_cmd) accel_cmd = min(accel_cmd, -530. / vehicle_mass / self.wheel_radius) #rospy.logwarn("computed accel_cmd : %s",accel_cmd) #rospy.logwarn("Velocity step After: %s",accel_cmd) elif linear_velocity < min_speed: angular_velocity *= min_speed / linear_velocity linear_velocity = min_speed #rospy.logwarn("updated angular_velocity : %s", angular_velocity) #rospy.logwarn("updated linear velocity: %s",linear_velocity) accel = (current_velocity - self.last_velocity) / time_elasped #rospy.logwarn("Current accel = %f, Last = %f, New = %f", accel, self.last_velocity, current_velocity) self.lpf_accel.filt(accel) self.last_velocity = current_velocity throttle, brake, steering = 0., 0., 0. if dbw_enabled: if accel_cmd >= 0: throttle = self.accel_pid.step( accel_cmd - self.lpf_accel.get(), time_elasped) else: self.accel_pid.reset() if (accel_cmd < -self.brake_deadband) or \ (linear_velocity < min_speed): brake = -accel_cmd * vehicle_mass * self.wheel_radius #angular_velocity_filt = self.lpf_angv.filt(angular_velocity) steering = self.yaw_control.get_steering(linear_velocity, angular_velocity, current_velocity) else: self.speed_pid.reset() self.accel_pid.reset() #rospy.logwarn("Throttle = %f, Brake = %f, Steering = %f" , throttle, brake, steering) #rospy.logwarn("given_av = %f, comp_av = %f, filt_av = %f, Steering = %f" , given_av, angular_velocity, angular_velocity_filt, steering) return throttle, brake, steering
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') rospy.loginfo("initing dbw_node") self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) wheel_base = rospy.get_param('~wheel_base', 2.8498) steer_ratio = rospy.get_param('~steer_ratio', 14.8) max_lat_accel = rospy.get_param('~max_lat_accel', 3.) max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) # self.prev_steer_val = None # self.prev_steer_val_set = False # self.tl_distance = -1 # self.prev_tl_distance = -1 # self.red_tl = True # self.tl_count = 0 # TODO: Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/tl_distance', Float32, self.tl_distance_cb) rospy.Subscriber('/go_to_stop_state', Bool, self.go_to_stop_cb) self.dbw = False self.angular_velocity_filter = LowPassFilter(.90, 1) self.velocity_filter = LowPassFilter(.9, 1) self.twist_yaw_filter = LowPassFilter(.2, .96) self.twist_velocity_filter = LowPassFilter(.96, .9) self.steer_filter = LowPassFilter(.2, .90) #self.p_v = [1.187355162, 0.044831144, 0.00295747] # v1.3 #self.p_v = [1.325735147117472, 0.06556341512981727, 0.013549012506233077] # v1.4 #self.p_v = [1.9285529383307387, 0.0007904838169666957, 0.019058015342866958] #self.p_v = [1.181681729, 0.084559526, 0.021058816] v.1.5 #self.p_v = [1.0671285679286078, 0.0011578159618311297, 0.011254946679196659] ## noise 0, 1. self.p_v = [0.9999999999, 0.0, 0.0] ## noise 0 #self.p_v = [0.923685948, 0.004035275, -0.000129007] ## noise 0.05 #self.p_v = [2.8144929000000034, 0.3190704767458041, 0.04469070609966564] # new values based on new brake system self.pidv = pid.PID(self.p_v[0], self.p_v[1], self.p_v[2]) self.throttle = 0. min_speed = .01 # At high speeds, a multiple of 1.2 seems to work a bit # better than 1.0 self.yaw_controller = YawController(wheel_base, 1.2 * steer_ratio, min_speed, 8 * max_lat_accel, max_steer_angle) # contol if the car is going to stop self.go_to_stop = False self.loop() ''' /twist_cmd Type: geometry_msgs/TwistStamped geometry_msgs/TwistStampedstd_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z ''' def go_to_stop_cb(self, msg): self.go_to_stop = msg.data def tl_distance_cb(self, msg): # self.tl_distance = msg.data # if self.prev_tl_distance == msg.data == -1: # self.tl_count += 1 % 1000 # if self.tl_count > 10: self.red_tl = False # else: # self.tl_count = 0 # self.prev_tl_distance = msg.data # self.red_tl = True pass def twist_cmd_cb(self, msg): seq = msg.header.seq (x, y, yaw) = twist_to_xyy(msg) # rospy.loginfo("twist_cmd_cb %d", seq) #print("twist linear: [%f, %f, %f]" % (x, y, yaw)) self.twist_yaw_filter.filt(yaw) # if self.red_tl == True: # vtwist = self.twist_velocity_filter.filt(0.1) # else: vtwist = self.twist_velocity_filter.filt(x) # calculate error between desired velocity and current velocity e = vtwist - self.velocity_filter.get() # feed pid controller with a dt of 0.033 self.throttle = self.pidv.step(e, 0.03) if self.throttle < -3.0: rospy.logerr("Resetting PID !!!") self.pidv.reset() self.throttle = self.pidv.step(e, 0.03) if msg.header.seq % 5 == 0: ts = msg.header.stamp.secs + 1.e-9 * msg.header.stamp.nsecs # rospy.loginfo("tcc %d %f %f %f %f", seq, ts, x, yaw, self.twist_yaw_filter.get()) pass ''' /vehicle/dbw_enabled Type: std_msgs/Bool ''' def dbw_enabled_cb(self, msg): rospy.loginfo("dbw_enabled_cb %s", msg.data) self.dbw = msg.data ''' /current_velocity Type: geometry_msgs/TwistStamped ''' def current_velocity_cb(self, msg): seq = msg.header.seq (x, y, yaw) = twist_to_xyy(msg) # rospy.loginfo("current_velocity_cb %i", seq) # factor = .90 # if yaw != 0.: # self.filtered_angular_velocity = factor*self.filtered_angular_velocity + (1-factor)*yaw # self.filtered_velocity = factor*self.filtered_velocity + (1-factor)*x self.angular_velocity_filter.filt(yaw) self.velocity_filter.filt(x) if msg.header.seq % 5 == 0: ts = msg.header.stamp.secs + 1.e-9 * msg.header.stamp.nsecs # ts seems to always be 0. yaw_per_meter = self.angular_velocity_filter.get( ) / self.velocity_filter.get() # rospy.loginfo("cv %d %f %f %f %f %f", seq, self.velocity_filter.get(), x, self.angular_velocity_filter.get(), yaw, yaw_per_meter) pass def loop(self): # rate = rospy.Rate(50) # 50Hz rate = rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): # TODO: Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) if self.dbw == True: twist = self.twist_yaw_filter.get() #steer = self.yaw_controller.get_steering(self.twist_velocity_filter.get(), twist, self.velocity_filter.get()) steer = self.yaw_controller.get_steering( self.velocity_filter.get(), twist, self.velocity_filter.get()) steer = self.steer_filter.filt(steer) # define throttle command based on pid controller throttle = brake = 0. if self.go_to_stop == False: if abs(self.throttle) > 1.: if self.throttle > 0: throttle = 1.0 else: throttle = 0. #brake = 1.0 * 10 v = self.velocity_filter.get() t = 0.03 # time to brake u = 0.70 # friction coeficcient g = 9.8 # gravity force D = (v * t) + (v**2 / (2. * u * 9.8)) # calculate work needed to stop w = 0.5 * self.vehicle_mass * (v**2 - (v - t)**2) # caculate the force F = w / D brake = (2 * u * F * self.wheel_radius) + self.brake_deadband elif self.throttle < 0: throttle = 0 #brake = (abs(self.throttle) + self.brake_deadband) #* 10 #if brake > 1.: brake = 1. * 1000 v = self.velocity_filter.get() t = 0.03 # time to brake u = 0.70 # friction coeficcient g = 9.8 # gravity force D = (v * t) + (v**2 / (2. * u * 9.8)) # calculate work needed to stop w = 0.5 * self.vehicle_mass * (v**2 - (v - t)**2) # caculate the force F = w / D brake = (2 * u * F * self.wheel_radius) + self.brake_deadband else: throttle = self.throttle + self.brake_deadband if throttle > 1.: throttle = 1. else: v = self.velocity_filter.get() t = 4.0 # time to brake u = 0.70 # friction coeficcient g = 9.8 # gravity force D = (v * t) + (v**2 / (2. * u * 9.8)) # calculate work needed to stop w = 0.5 * self.vehicle_mass * v**2 # caculate the force F = w / D brake = (2 * u * F * self.wheel_radius) + self.brake_deadband #brake *= 10. rospy.loginfo( "[%f] throttle: %f brake: %f steering angle: %f " % (self.throttle, throttle, brake, steer)) # throttle is 0.35, which runs the car at about 40 mph. # throttle of 0.98 will run the car at about 115 mph. rospy.loginfo( "[%f] throttle: %f brake: %f steering angle: %f " % (self.throttle, throttle, brake, steer)) self.publish(throttle, brake, steer) else: self.pidv.reset() rate.sleep() def publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
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. # min throttle value mx = 0.2 # max throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1 / (2pi * tau) = cutoff 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): # 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.loginfo('Angular Velocillty: {0}'.format(angular_vel)) rospy.loginfo('Target Velocity: {0}'.format(linear_vel)) rospy.loginfo('Target Angular Velocity: {0}'.format(angular_vel)) rospy.loginfo('Current Velocity: {0}'.format(current_vel)) rospy.loginfo('Filtered Velocity: {0}'.format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 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 ~ 1 m/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 # pass tau = 0.5 ts = .02 self.vel_lpf = LowPassFilter(tau, ts) kp = 0.3 ki = 0.1 kd = 0. mn = 0. mx = 0.5 self.throttle_controller = PID(kp, ki, kd, mn, mx) wheel_base = kwargs['wheel_base'] steer_ratio = kwargs['steer_ratio'] max_lat_accel = kwargs['max_lat_accel'] max_steer_angle = kwargs['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 = 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.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. proposed_linear_velocity = args[0] proposed_angular_velocity = args[1] current_linear_velocity = args[2] dbw_status = args[3] if not dbw_status: self.throttle_controller.reset() return 0., 0., 0. current_linear_velocity = self.vel_lpf.filt(current_linear_velocity) # http://wiki.ros.org/rospy/Overview/Logging # rospy.logwarn("Target linear velocity: {0}".format(proposed_linear_velocity)) # rospy.logwarn("Target angular velocity: {0}".format(proposed_angular_velocity)) # rospy.logwarn("Current linear velocity: {0}".format(current_linear_velocity)) # rospy.logwarn("filtered linear velocity: {0}".format(self.vel_lpf.get())) steer = self.yaw_controller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) vel_error = proposed_linear_velocity - current_linear_velocity now_time = rospy.get_time() sample_time = now_time - self.last_time self.last_time = now_time throttle = self.throttle_controller.step(vel_error, sample_time) rospy.loginfo( "Target_lv: {0} Target_av: {1} Current_lv: {2} filtered_lv: {3} sample_time: {4}" .format(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, self.vel_lpf.get(), sample_time)) brake = 0 if proposed_linear_velocity == 0. and current_linear_velocity < 0.1: throttle = 0 brake = 400 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.loginfo("throttle: {0} brake: {1} steer: {2}".format(throttle, brake, steer)) return throttle, brake, steer
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.brake_torque_const = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius self.twist = None self.current_velocity = 0.0 self.past_velocity = 0.0 self.current_acc = 0.0 self.lowpass_filter_acc = LowPassFilter(LPF_ACC_TAU, 1.0 / 50) self.pid_vel = PID(PID_P_VEL, PID_I_VEL, PID_D_VEL, self.decel_limit, self.accel_limit) self.pid_acc = PID(PID_P_ACC, PID_I_ACC, PID_D_ACC, 0.0, 0.75) self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, MIN_SPEED, self.max_lat_accel, self.max_steer_angle) #rospy.logerr('twist_controller.self() done') pass def control(self): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer return 0., 0., 0. def control(self, required_vel_linear, required_vel_angular, current_velocity): throttle, brake, steering = 0.0, 0.0, 0.0 velocity_error = required_vel_linear - current_velocity #get current acc using lowpass filter #rospy.logerr('twist_controller: velocity_error=%d',velocity_error) acc_temp = (self.past_velocity - current_velocity) * 50.0 self.past_velocity = current_velocity self.lowpass_filter_acc.filt(acc_temp) self.current_acc = self.lowpass_filter_acc.get( ) #at first, current_acc=0 #rospy.logerr('twist_controller: current_acc=%d',self.current_acc) required_acc = self.pid_vel.step(velocity_error, 1.0 / 50) #rospy.logerr('twist_controller: required_acc=%d',required_acc) if required_acc < 0: throttle = 0 self.pid_acc.reset() if -required_acc > self.brake_deadband: #brake, but not over the brak deadband brake = -self.decel_limit * self.brake_torque_const else: brake = -required_acc * self.brake_torque_const else: if required_acc > self.accel_limit: throttle = self.pid_acc.step( self.accel_limit - self.current_acc, 1.0 / 50) else: throttle = self.pid_acc.step(required_acc - self.current_acc, 1.0 / 50) steering = self.yaw_controller.get_steering(required_vel_linear, required_vel_angular, current_velocity) return throttle, brake, steering def reset(self): self.pid_vel.reset() pass
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, speed_kp, accel_kp, accel_ki, max_lat_accel, accel_tau, 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.last_velocity = 0. self.lpf_fuel = LowPassFilter(60.0, 0.1) self.lpf_accel = LowPassFilter(accel_tau, 0.02) self.accel_pid = PID(accel_kp, accel_ki, 0.0, 0.0, 1.0) self.speed_pid = PID(speed_kp, 0.0, 0.0, self.decel_limit, self.accel_limit) self.yaw_control = YawController(wheel_base, steer_ratio, 4. * ONE_MPH, max_lat_accel, max_steer_angle) self.last_ts = None def set_fuel(self, level): self.lpf_fuel.filt(level) def get_vehicle_mass(self): return self.vehicle_mass + \ self.lpf_fuel.get() / 100.0 * self.fuel_capacity * GAS_DENSITY def time_elasped(self, msg=None): now = rospy.get_time() if self.last_ts is None: self.last_ts = now elasped, self.last_ts = now - self.last_ts, now return elasped def control(self, linear_velocity, angular_velocity, current_velocity, dbw_enbaled): time_elasped = self.time_elasped() if time_elasped > 1. / 5 or time_elasped < 1e-4: self.speed_pid.reset() self.accel_pid.reset() self.last_velocity = current_velocity return 0., 0., 0. vehicle_mass = self.get_vehicle_mass() 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_elasped) min_speed = ONE_MPH * 5 if linear_velocity < 0.01: accel_cmd = min(accel_cmd, -530. / vehicle_mass / self.wheel_radius) elif linear_velocity < min_speed: angular_velocity *= min_speed / linear_velocity linear_velocity = min_speed accel = (current_velocity - self.last_velocity) / time_elasped #rospy.logwarn("Current accel = %f, Last = %f, New = %f", accel, self.last_velocity, current_velocity) self.lpf_accel.filt(accel) self.last_velocity = current_velocity throttle, brake, steering = 0., 0., 0. if dbw_enbaled: if accel_cmd >= 0: throttle = self.accel_pid.step( accel_cmd - self.lpf_accel.get(), time_elasped) else: self.accel_pid.reset() if (accel_cmd < -self.brake_deadband) or \ (linear_velocity < min_speed): brake = -accel_cmd * vehicle_mass * self.wheel_radius steering = self.yaw_control.get_steering(linear_velocity, angular_velocity, current_velocity) else: self.speed_pid.reset() self.accel_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): 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.loginfo("Angular vel: {0}".format(angular_vel)) rospy.loginfo("Target velocity: {0}".format(linear_vel)) rospy.loginfo("Target angular velocity: {0}\n".format(angular_vel)) rospy.loginfo("Current velocity: {0}".format(current_vel)) rospy.loginfo("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 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.0 # maximum throttle angle mn = 0.0 # minimum throttle value mx = 0.3 # 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): # 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) 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) brake = 0 if linear_vel == 0.0 and current_vel < 0.1: throttle = 0 brake = 700 # N.m - to hold the car in place 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, min_speed, *args, **kwargs): # TODO: Implement self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle self.min_speed = min_speed self.previous_linear_velocity = None self.speed_controller = PID(2., 0., 0.) # values provided by DataSpeedInc for MKZ self.accel_controller = PID(0.4, 0.1, 0., mn=0., mx=self.accel_limit) # values provided by DataSpeedInc for MKZ self.accel_lowpassfilter = LowPassFilter(0.5, 0.2) # values provided by DataSpeedInc for MKZ self.steering_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) pass def control(self, target_linear_velocity, target_angular_velocity, current_linear_velocity, current_angular_velocity, control_rate, dbw_is_enabled, *args, **kwargs): # TODO: Change the arg, kwarg list to suit your needs if self.previous_linear_velocity is None: self.previous_linear_velocity = current_linear_velocity else: raw_accel = control_rate * (current_linear_velocity.x - self.previous_linear_velocity.x) # TODO: parameterize: 50 comes from the dbw_node Rate value self.accel_lowpassfilter.filt(raw_accel) velocity_error = target_linear_velocity.x - current_linear_velocity.x accel = self.speed_controller.step(velocity_error, 1./ control_rate) steer = 0. throttle =0. brake = 0. if accel >= 0.: throttle = self.accel_controller.step(accel-self.accel_lowpassfilter.get(), 1./ control_rate) else: # TODO: Check if accel requested for braking is smaller than brake_deadband accel = max(self.decel_limit, accel) brake = -accel * self.vehicle_mass * self.wheel_radius #TODO: Add passanger weights and consider GAS weight steer = self.steering_controller.get_steering(target_linear_velocity.x,target_angular_velocity.z,current_linear_velocity.x) steer = max(-self.max_steer_angle, min(self.max_steer_angle, steer)) if not dbw_is_enabled: self.accel_controller.reset() self.speed_controller.reset() self.previous_linear_velocity = current_linear_velocity # Return throttle, brake, steer 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_vel = 0 self.last_time = rospy.get_time() pass def control(self, linear_vel, angular_vel, current_vel, curr_ang_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer """ Params: linear_vel: proposed linear velocity angular_vel: proposed angular velocity current_vel: current linear velocity curr_ang_vel: current angular velocity dbw_enabled: DBW status """ if not dbw_enabled: # Turn off/Reset PID controller self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) rospy.logwarn("Current Linear Velocity: {0}".format(current_vel)) rospy.logwarn("Filtered Velocity: {0}".format(self.vel_lpf.get())) rospy.logwarn("Current Angular Velocity: {0}".format(curr_ang_vel)) rospy.logwarn("Target Linear Velocity: {0}".format(linear_vel)) rospy.logwarn("Target Angular Velocity {0}".format(angular_vel)) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel, curr_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) 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 Controller(object): def __init__(self): self.mass = rospy.get_param('~vehicle_mass', 1736.35) self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.fuel_density = rospy.get_param('~fuel_density', 2.858) self.vehicle_mass = self.mass + self.fuel_capacity * self.fuel_density self.vehicle_mass_wheel_radius = self.vehicle_mass * self.wheel_radius self.throttle_pid = PID(.4, .1, 0., 0., 1.) self.accel_pid = PID(2., 0., 0., self.decel_limit, self.accel_limit) self.accel_f = LowPassFilter(.5, 0.02) self.yaw_control = YawController(self.wheel_base, self.steer_ratio, 1, self.max_lat_accel, self.max_steer_angle) self.min_speed = 2. self.last_velocity = 0.0 self.last_time = 0 def control(self, linear_velocity, angular_velocity, current_velocity): delta_t = self.delta_time() lv_delta = linear_velocity - current_velocity v_delta = current_velocity - self.last_velocity self.last_velocity = current_velocity if delta_t == 0: self.reset() return 0., 0., 0. if abs(linear_velocity) < ONE_MPH: self.accel_pid.reset() accel_cmd = self.accel_pid.step(lv_delta, delta_t) if linear_velocity < .01: accel_cmd = min(accel_cmd, -530. / self.vehicle_mass / self.wheel_radius) elif linear_velocity < self.min_speed: angular_velocity *= self.min_speed / linear_velocity linear_velocity = self.min_speed throttle = self.get_trottle(accel_cmd, v_delta, delta_t) brake = self.get_brake(accel_cmd, linear_velocity) steering = self.yaw_control.get_steering(linear_velocity, angular_velocity, current_velocity) return throttle, brake, steering def delta_time(self): """ Calculate step time delta current time - previous time. """ current_time = rospy.get_time() if self.last_time == 0: self.last_time = current_time delta_t = current_time - self.last_time self.last_time = current_time return delta_t def get_brake(self, accel_cmd, linear_velocity): if (accel_cmd < -self.brake_deadband) or (linear_velocity < self.min_speed): return -accel_cmd * self.vehicle_mass_wheel_radius return 0.0 def get_trottle(self, accel_cmd, v_delta, delta_t): accel = v_delta / delta_t self.accel_f.filt(accel) if accel_cmd >= 0: return self.throttle_pid.step(accel_cmd - self.accel_f.get(), delta_t) self.throttle_pid.reset() return 0.0 def reset(self): self.accel_pid.reset() self.throttle_pid.reset()
class Controller(object): def __init__(self, *args, **kwargs): self.vehicle_mass = args[0] self.fuel_capacity = args[1] self.brake_deadband = args[2] self.decel_limit = args[3] self.accel_limit = args[4] self.wheel_radius = args[5] self.wheel_base = args[6] self.steer_ratio = args[7] self.max_lat_accel = args[8] self.max_steer_angle = args[9] self.velocity_pid_ = PID(VELOCITY_Kp, VELOCITY_Ki, VELOCITY_Kd, -abs(self.decel_limit), abs(self.accel_limit)) self.accel_pid_ = PID(ACCEL_Kp, ACCEL_Ki, ACCEL_Kd) self.steer_pid_ = PID(STEER_Kp, STEER_Ki, STEER_Ki, -abs(self.max_steer_angle), abs(self.max_steer_angle)) self.steering_cntrl = YawController(self.wheel_base, self.steer_ratio, MIN_SPEED, self.max_lat_accel, self.max_steer_angle) self.accel_lpf_ = LowPassFilter(ACCEL_Tau, ACCEL_Ts) def control(self, *args, **kwargs): current_time = args[0] last_cmd_time = args[1] control_period = args[2] twist_cmd = args[3] current_velocity = args[4] dbw_enabled = args[5] brake_deadband = args[6] cte = args[7] if (current_time - last_cmd_time) > 10 * control_period: self.velocity_pid_.reset() self.accel_pid_.reset() # assuming 2 pax in car plus full tank vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + 2 * WEIGHT_PERSON velocity_error = twist_cmd.twist.linear.x - current_velocity.twist.linear.x if abs(twist_cmd.twist.linear.x) < 1.0 * ONE_MPH: self.velocity_pid_.reset() accel_cmd = self.velocity_pid_.step(velocity_error, control_period) if twist_cmd.twist.linear.x <= 1e-2: accel_cmd = min(accel_cmd, -530 / vehicle_mass / self.wheel_radius) elif twist_cmd.twist.linear.x < MIN_SPEED: twist_cmd.twist.angular.z = twist_cmd.twist.angular.z * MIN_SPEED / twist_cmd.twist.linear.x twist_cmd.twist.linear.x = MIN_SPEED if dbw_enabled: if accel_cmd >= 0: throttle_val = self.accel_pid_.step(accel_cmd - self.accel_lpf_.get(), control_period) else: throttle_val = 0 self.accel_pid_.reset() if accel_cmd < -brake_deadband or twist_cmd.twist.linear.x < MIN_SPEED: brake_val = -accel_cmd * vehicle_mass * self.wheel_radius else: brake_val = 0 steering_val = self.steering_cntrl.get_steering(twist_cmd.twist.linear.x, twist_cmd.twist.angular.z, current_velocity.twist.linear.x) \ + self.steer_pid_.step(cte, control_period) steering_val = max(-abs(self.max_steer_angle), min(abs(self.max_steer_angle), steering_val)) return throttle_val, brake_val, steering_val else: self.velocity_pid_.reset() self.accel_pid_.reset() return 0., 0., 0. def filter_accel_value(self, value): self.accel_lpf_.filt(value) def get_filtered_accel(self): return self.accel_lpf_.get()
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 self.sampling_time = 1. / self.sampling_rate # rospy.logwarn('TwistController: Sampling rate = ' + str(self.sampling_rate)) # rospy.logwarn('TwistController: Sampling time = ' + str(self.sampling_time)) self.prev_vel = 0.0 self.current_accel = 0.0 self.acc_lpf = LowPassFilter(tau, self.sampling_time) 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 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, self.sampling_time) if desired_accel > 0.0: if desired_accel < self.accel_limit: throttle = self.accel_pid.step( desired_accel - self.current_accel, self.sampling_time) else: throttle = self.accel_pid.step( self.accel_limit - self.current_accel, self.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 the car in place if we are stopped at a light. Acceleration - 1m/s^2 # # elif throttle < .1 and vel_error < 0: # throttle = 0 # decel = max(vel_error, self.decel_limit) # brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque N*m return throttle, brake, steering
class Controller(object): #def __init__(self, *args, **kwargs): # TODO: Implement #pass # init in dbw: # self.controller = Controller(vehicle_mass = vehicle_mass, # fuel_capacity = fuel_capacity, # brake_deadband = brake_deadband, # decel_limit = decel_limit, # accel_limit = accel_limit, # wheel_radius = wheel_radius, # wheel_base = wheel_base, # steer_ratio = steer_ratio, # max_lat_accel = max_lat_accel, # max_steer_angle = max_steer_angle) # usage in dbw: #self.throttle, self.brake, self.steering = self.controller.control(self.current_vel, # self.dbw_enabled, # self.linear_vel, # self.angular_vel) 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. # mimnimum throttle value mx = 0.2 # maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) # velocity is noisy, so use low-pass filter tau = 0.5 # 1/ (2*pi*tau) = cutoff-frequence ts = 0.02 # sample time = 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, *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, dbw_enabled, linear_vel, angular_vel): # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. # get lowpass-filtered velocity current_vel = self.vel_lpf.filt(current_vel) if IS_DEBUG: rospy.loginfo("Angular vel: {0}".format(angular_vel)) rospy.loginfo("Target vel: {0}".format(linear_vel)) rospy.loginfo("Target ang vel: {0}".format(angular_vel)) rospy.loginfo("Current vel: {0}".format(current_vel)) rospy.loginfo("Filtered vel: {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 # brake = 400 # Nm --> needed to hold car in place if we are at a light, resulting acceleration = -1 m/s^2 brake = 700 # Nm --> needed to hold car in place if we are at a light, resulting acceleration = -1 m/s^2 elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) # limit brake value brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # torque = N * m = acceleration * mass * radius if IS_DEBUG: rospy.logwarn("vel error: {0}, throttle: {1}, brake: {2}".format( vel_error, throttle, brake)) # problem with this controller: # by the time, car is away from waypoint, waypoint_follower send new commands # 1. waypoint_folloer: make sure, it will update every time; if not following waypoints, do update # 2. change yaw-controller and add some damping terms (current_ang_vel vs target_ang_vel --> if too large...) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = args[0] self.fuel_capacity = args[1] self.brake_deadband = args[2] self.decel_limit = args[3] self.accel_limit = args[4] self.wheel_radius = args[5] self.wheel_base = args[6] self.steer_ratio = args[7] self.max_lat_accel = args[8] self.max_steer_angle = args[9] self.velocity_pid = PID(VELOCITY_Kp, VELOCITY_Ki, VELOCITY_Kd, -abs(self.decel_limit), abs(self.accel_limit)) self.accel_pid = PID(ACCEL_Kp, ACCEL_Ki, ACCEL_Kd) self.steering_cntrl = YawController(self.wheel_base, self.steer_ratio, MIN_SPEED, self.max_lat_accel, self.max_steer_angle) self.accel_lpf = LowPassFilter(ACCEL_Tau, ACCEL_Ts) #pass def control(self, *args, **kwargs): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer #return 1., 0., 0. current_time = args[0] last_cmd_time = args[1] control_period = args[2] twist_cmd = args[3] current_velocity = args[4] dbw_enabled = args[5] if (current_time - last_cmd_time) > 10 * control_period: self.velocity_pid.reset() self.accel_pid.reset() vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + 2 * WEIGHT_PERSON velocity_error = twist_cmd.twist.linear.x - current_velocity.twist.linear.x if abs(twist_cmd.twist.linear.x) < (1.0 * ONE_MPH): self.velocity_pid.reset() accel_cmd = self.velocity_pid.step(velocity_error, control_period) if twist_cmd.twist.linear.x <= 1e-2: accel_cmd = min(accel_cmd, -530 / vehicle_mass / self.wheel_radius) elif twist_cmd.twist.linear.x < MIN_SPEED: twist_cmd.twist.angular.z = twist_cmd.twist.angular.z * MIN_SPEED / twist_cmd.twist.linear.x twist_cmd.twist.linear.x = MIN_SPEED if dbw_enabled: if accel_cmd >= 0: throttle_val = self.accel_pid.step( accel_cmd - self.accel_lpf.get(), control_period) else: throttle_val = 0.0 self.accel_pid.reset() if accel_cmd < -self.brake_deadband or twist_cmd.twist.linear.x < MIN_SPEED: brake_val = -accel_cmd * vehicle_mass * self.wheel_radius else: brake_val = 0.0 steering_val = self.steering_cntrl.get_steering( twist_cmd.twist.linear.x, twist_cmd.twist.angular.z, current_velocity.twist.linear.x ) #+ STEER_Kp * (twist_cmd.twist.angular.z - current_velocity.twist.angular.z) return throttle_val, brake_val, steering_val else: self.velocity_pid.reset() self.accel_pid.reset() return 0.0, 0.0, 0.0 def filter_accel_value(self, value): self.accel_lpf.filt(value) def get_filtered_accel(self): return self.accel_lpf.get()
class Controller(object): def __init__(self, *args, **kwargs): rospy.loginfo('TwistController: Start init') self.sampling_rate = kwargs["sampling_rate"] self.decel_limit = kwargs["decel_limit"] self.accel_limit = kwargs["accel_limit"] # brake_deadband is the interval in which the brake would be ignored # the car would just be allowed to slow by itself/coast to a slower speed self.brake_deadband = kwargs["brake_deadband"] self.vehicle_mass = kwargs["vehicle_mass"] self.fuel_capacity = kwargs["fuel_capacity"] self.wheel_radius = kwargs["wheel_radius"] # bunch of parameters to use for the Yaw (steering) controller 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.delta_t = 1 / self.sampling_rate self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity \ * GAS_DENSITY) * self.wheel_radius self.past_vel_linear = 0.0 self.current_accel = 0.0 self.low_pass_filter_accel = LowPassFilter(LPF_ACCEL_TAU, self.delta_t) # Initialise speed PID, with tuning parameters # Will use this PID for the speed control self.pid_vel_linear = PID(PID_VEL_P, PID_VEL_I, PID_VEL_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.75) # Initialise Yaw controller - this gives steering values using # vehicle attributes/bicycle model # Need to have some minimum speed before steering is applied self.yaw_controller = YawController( wheel_base=self.wheel_base, steer_ratio=self.steer_ratio, min_speed=5.0, max_lat_accel=self.max_lat_accel, max_steer_angle=self.max_steer_angle) rospy.loginfo('TwistController: Complete init') rospy.loginfo('TwistController: Steer ratio = ' + str(self.steer_ratio)) def control(self, required_vel_linear, required_vel_angular, current_vel_linear): throttle, brake, steering = 0.0, 0.0, 0.0 # uncomment for debugging pruposes otherwise we write logs at 50 Hz #rospy.loginfo('TwistController: Control call at ' + str(rospy.get_time())) #rospy.loginfo('TwistController: ' + 'Desired linear/angular = ' + str(required_vel_linear) + "," # + str(required_vel_angular) + ' Current = ' + str(current_vel_linear) + ',' + str(current_vel_angular)) # calculate the difference (error) between desired and current linear velocity velocity_error = required_vel_linear - current_vel_linear # calculate current acceleration and smooth using lpf accel_temp = self.sampling_rate * (self.past_vel_linear - current_vel_linear) # update self.past_vel_linear = current_vel_linear self.low_pass_filter_accel.filt(accel_temp) self.current_accel = self.low_pass_filter_accel.get() # use velocity controller compute desired accelaration desired_accel = self.pid_vel_linear.step(velocity_error, self.delta_t) # TODO think about emergency brake command if desired_accel > 0.0: if desired_accel < self.accel_limit: throttle = self.accel_pid.step( desired_accel - self.current_accel, self.delta_t) else: throttle = self.accel_pid.step( self.accel_limit - self.current_accel, self.delta_t) 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 - yaw controller takes desired linear, desired angular, current linear as params #steering = required_vel_angular * self.steer_ratio steering = self.yaw_controller.get_steering(required_vel_linear, required_vel_angular, current_vel_linear) # uncomment for debugging #if throttle <> 0.0: # rospy.loginfo('TwistController: Accelerating = ' + str(throttle)) #if brake <> 0.0: # rospy.loginfo('TwistController: Braking = ' + str(brake)) if abs(steering) <> 0.0: #rospy.loginfo('TwistController: Steering = ' + str(steering)) rospy.loginfo('Veer: Steering = ' + str(steering) + ', required = ' + str(required_vel_angular)) return throttle, brake, steering def reset(self): self.pid_vel_linear.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): # TODO: Implement # Yaw controller self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # PID controller # mn: minimum throttle # mx: maximun throttle self.pid = PID(kp=0.3, ki=0.1, kd=0.0, mn=0.0, mx=0.2) # Low pass filter # tau: cut-off frequency # ts: sample time self.lowpassfilter = LowPassFilter(tau=0.5, ts=0.02) # Some vehicle properties 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_vel = self.lowpassfilter.get() self.last_time = rospy.get_time() def control(self, linear_velocity, angular_velocity, curr_linear_velocity, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # Reset if dbw not enabled if not dbw_enabled: self.pid.reset() return 0.0, 0.0, 0.0 curr_linear_velocity = self.lowpassfilter.filt(curr_linear_velocity) # Throttle control vel_err = linear_velocity - curr_linear_velocity self.last_vel = curr_linear_velocity curr_time = rospy.get_time() sample_time = curr_time - self.last_time self.last_time = curr_time throttle = self.pid.step(vel_err, sample_time) # Brake control brake = 0.0 # To hold the car from moving if abs(linear_velocity ) < sys.float_info.epsilon and curr_linear_velocity < 0.1: throttle = 0.0 brake = 700.0 # Nm of torque to hold the car # Deceleration elif throttle < 0.1 and vel_err < 0.0: throttle = 0.0 decel = max(vel_err, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque (Nm) # Steering control steering = self.yaw_controller.get_steering(linear_velocity, angular_velocity, curr_linear_velocity) return throttle, brake, steering