class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. mx = 1 self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = .02 self.vel_lpf = LowPassFilter(tau, ts) tau = 0.5 ts = .02 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() # TODO: Implement pass def control(self, current_vel, curr_ang_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) #curr_ang_vel = self.vel_lpf.filt(curr_ang_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel, curr_ang_vel) #rospy.logwarn("Angular Vel: {0}".format(angular_vel)) #rospy.logwarn("Current Angular Vel: {0}".format(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 = 700 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 = 1.0 ki = 0.1 kd = 0.006 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.last_vel = self.vel_lpf 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) 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. Acceleratoin ~ 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, 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): # *args, **kwargs): # TODO: Implement #pass 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 values 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. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 400 # N8m - tp hold the car in place if we are stopped 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*map 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 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, update_rate_hz, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.update_rate_hz = update_rate_hz self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius 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 kp = 0.3 # proportional ki = 0.08 # integral (i.e. bias) kd = 0. # derivative (i.e. low-pass adjustment for kp) mn = 0. # min throttle mx = 1.0 # max throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) # STEERING self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # SMOOTH OUT COMMANDED VELOCITY tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 1.0/update_rate_hz # 0.02 # sample time self.vel_lpf = LowPassFilter(tau, ts) 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 if not dbw_enabled: self.throttle_controller.reset() # Start it over from scratch (should we keep the integral??) return 0.,0.,0. # return nothingness current_vel = self.vel_lpf.filt(current_vel) # Get steering and slow down if we're going to fast to make the turn steering, new_vel = self.yaw_controller.get_steering(linear_vel,angular_vel,current_vel) #rospy.loginfo ("Linear Vel = %s Current Vel = %s New Vel = %s" % (linear_vel,current_vel,new_vel)) overshoot_error = current_vel - new_vel # will be positive if going too fast on turn # penalize velocity error, but not if we need to slow down for the turn vel_error = linear_vel - current_vel - overshoot_error 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 = RESTING_BRAKE_Nm #elif throttle < .1 and vel_error < 0: elif vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius if linear_vel == 0.: brake = max(RESTING_BRAKE_Nm,brake) #brake += RESTING_BRAKE_Nm #rospy.loginfo ("Throttle = %s Brake = %s Steering = %s" % (throttle,brake,steering)) return throttle, brake, steering # Return throttle, brake, steer #return 0.5, 0., 0. return 0., RESTING_BRAKE_Nm, 0.
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # Yaw controller 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. mn = 0. # Minimum throttle value mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() self.last_vel = None def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) 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 # control brake here # if we want to stop the car 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 # if we are going faster than we want 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): self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) ''' # experimental parameters kp = 0.3 ki = 0.1 kd = 0.02 mn = 0. # minimum throttle mx = 0.2 # maximum throttle self.throttle_controller = PID(kp,ki,kd,mn,mx) ''' kp = 0.5 ki = 0.0001 kd = 0.15 mn = decel_limit # minimum throttle mx = 0.5 # maximum throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) # to control the noisy messages in current_velocity via a provided low pass filter tau = 0.05 ts = 0.02 self.velocity_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass 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.last_throttle = 0 self.last_brake = 100 def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # check if car is in manual mode if not dbw_enabled: self.throttle_controller.reset() # disconnect the PID controllet to avoid error accumulating for the Integral term # to avoid erratic behaviours at re-insertion of the controller return 0., 0., 0. # get current velocity current_vel = self.velocity_lpf.filt(current_vel) ## get the comands steer = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) velocity_error = linear_vel - current_vel # self.last_velocity = current_vel ## update internal time current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # get throttle aceeleration = self.throttle_controller.step(velocity_error, sample_time) #the square difference between the proposed velocity (linear_vel) and the current velocity (current_vel) #divided by 2 times the distance of 30 meters. #The positive value that returns this algorithm are the throttle values of the car. smooth_acc = ((linear_vel * linear_vel) - (current_vel * current_vel)) / (2 * 30) if smooth_acc >= 0: throttle = smooth_acc else: throttle = 0 if throttle > 0.5: throttle = 0.5 #smoothing throttle acceleration and deceleration if (throttle > 0.025) and (throttle - self.last_throttle) > 0.005: throttle = max((self.last_throttle + 0.0025), 0.005) if throttle > 0.025 and (throttle - self.last_throttle) < -0.05: throttle = self.last_throttle - 0.05 self.last_throttle = throttle # get brake brake = 0 if linear_vel == 0 and current_vel < 0.1: # 0.1 m/s is the minimum velocity throttle = 0 brake = 700 # N*m - amount necessary to hold the car in place while stopped # like if we're waiting at a traffic light # tha corresponding acceleration is around 1m/s^2 elif throttle < 0.025 and velocity_error < 0.: #if we're stopping/decelerating throttle = 0 decel = max((smooth_acc * 5), self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque N*m #smoothing brake if brake > 100 and (brake - self.last_brake) > 20: brake = max((self.last_brake + 20), 100) if brake > 20 and (brake - self.last_brake) > 20: brake = max((self.last_brake + 20), 20) #rospy.loginfo('brake: %f', brake) #rospy.loginfo('trottle: %f', throttle) self.last_brake = brake return throttle, brake, steer
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # Instance values self.dbw_enabled = True self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # check self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) # check self.brake_deadband = rospy.get_param('~brake_deadband', .1) # check self.decel_limit = -650.0 self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) # check self.wheel_base = rospy.get_param('~wheel_base', 2.8498) # check self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) # check self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) # check self.current_velocity = 0.0 self.current_angular = 0.0 self.proposed_linear = 0.0 self.proposed_angular = 0.0 self.cte = 0.0 self.lp_filter = LowPassFilter(0.5, CONTROL_PERIOD) 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) # NOTE: I pass the dbw_node object to the controller so I don't # have huge ugly param list for initialization. Perhaps # not the best practice... self.controller = Controller(self) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) # rospy.Subscriber('/cross_track_error', Float64, self.cross_track_cb) self.loop() def loop(self): rate = rospy.Rate(CONTROL_RATE) # 50Hz while not rospy.is_shutdown(): throttle, brake, steering = self.controller.control( self.proposed_linear, self.proposed_angular, self.current_velocity, self.current_angular) if self.dbw_enabled: self.publish(throttle, brake, steering) rate.sleep() def publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def twist_cmd_cb(self, msg): # There is a bug in the pure_pursuit code that sometimes returns a negative # linear.x, so we just take the abs() here as suggested on Slack self.proposed_linear = abs(msg.twist.linear.x) self.proposed_angular = msg.twist.angular.z return def current_velocity_cb(self, msg): # lowpass filter the finite difference approximation to the acceleration self.lp_filter.filt( (self.current_velocity - msg.twist.linear.x) / CONTROL_RATE) self.current_velocity = msg.twist.linear.x self.current_angular = msg.twist.angular.z return def dbw_enabled_cb(self, msg): self.dbw_enabled = msg return def cross_track_cb(self, msg): self.cte = msg.data return
class Controller(object): def __init__(self, *args, **kwargs): wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] min_velocity = kwargs['min_velocity'] max_lat_accel = kwargs['max_lat_accel'] max_steer_angle = kwargs['max_steer_angle'] self.decel_limit = kwargs['decel_limit'] accel_limit = kwargs['accel_limit'] self.deadband = kwargs['deadband'] self.yawController = YawController(wheel_base, self.steer_ratio, min_velocity, max_lat_accel, max_steer_angle) #self.correcting_pid = PID(0.065, 0.000775, 0.775, -1.57, 1.57) #self.velocity_pid = PID(0.8, 0.001, 0.2, decel_limit, accel_limit) #self.velocity_pid = PID(1.6, 0.00001, 0.04, decel_limit, accel_limit) Good: best so far #self.velocity_pid = PID(2.0, 0.4, 0.1, decel_limit, accel_limit) Too much bias #self.velocity_pid = PID(0.1, 0.001, 1.0, decel_limit, accel_limit) Not responsive enough #self.velocity_pid = PID(0.9, 0.0005, 0.07, decel_limit, accel_limit) fair #self.velocity_pid = PID(5.0, 0.00001, 0.2, decel_limit, accel_limit) ok #self.velocity_pid = PID(2.0, 0.0, 0.05, decel_limit, accel_limit) not as good as 1.6 #self.velocity_pid = PID(2.0, 0.00001, 0.04, decel_limit, accel_limit) good #self.velocity_pid = PID(0.35, 0., 0., decel_limit, accel_limit) self.velocity_pidHI = PID(0.35, 0., 0., self.decel_limit, accel_limit) self.velocity_pidLO = PID(0.7, 0., 0., self.decel_limit / 2., accel_limit) #self.lowpassFilt = LowPassFilter(accel_limit/2., 0.02) # good self.lowpassFilt = LowPassFilter(0.07, 0.02) self.topVelocity = 0. def control(self, linear_velocity_target, angular_velocity_target, linear_velocity_current): # Throttle if linear_velocity_target > 2.: throttle_correction = self.velocity_pidHI.step( linear_velocity_target - linear_velocity_current, 0.02) else: throttle_correction = self.velocity_pidLO.step( linear_velocity_target - linear_velocity_current, 0.02) throttle_correction = self.lowpassFilt.filt(throttle_correction) if throttle_correction > 0.: brake = 0.0 if linear_velocity_target <= 0.01: throttle = 0. self.velocity_pidLO.reset() # <-- Reset the velocity PID self.velocity_pidHI.reset() else: throttle = throttle_correction # Braking else: throttle = 0. brake = -throttle_correction if brake < self.deadband * self.decel_limit / 2.: brake = 0 # Steering if linear_velocity_target > self.topVelocity: # mitigate rapid turning self.topVelocity = linear_velocity_target if linear_velocity_current > 0.05: steering = self.yawController.get_steering( self.topVelocity, angular_velocity_target, linear_velocity_current ) # self.correcting_pid.step(angular_velocity_target, 0.1) else: steering = 0. # Alternate approach: steering = angular_velocity_target * self.steer_ratio return throttle, brake, steering def reset(self): self.velocity_pidLO.reset() self.velocity_pidHI.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, 1) 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): # TODO: Implement 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/(2*pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) 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.yaw_controller = YawController(self.wheel_base, self.steer_ratio, 0.1, self.max_lat_accel, self.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.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 ''' CASE: dirving by wire not enabled. In this case nothing should be done: throttle = 0, brake=0, steer=0 ''' if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. ''' CASE: dirving by wire enabled ''' current_vel = self.vel_lpf.filt( current_vel ) # filtering the current velocity by using the lower pass to let car run smoothly steering = self.yaw_controller.get_steering( linear_vel, angular_vel, current_vel) # calculate stering 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) # calculate throttle ''' decision for brake ''' 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 return throttle, brake, steering
class Controller(object): def __init__(self, pid_controller_properties, yaw_controller_properties, lowpass_filter_properties): # @done Find suitable values for PID properties self.pid = PID(kp=2.0, ki=0.05, kd=0.005, mn=pid_controller_properties.decel_limit, mx=pid_controller_properties.accel_limit) self.max_brake_torque = pid_controller_properties.max_brake_torque self.max_throttle = pid_controller_properties.max_throttle self.yaw_controller = YawController( yaw_controller_properties.wheel_base, yaw_controller_properties.steer_ratio, yaw_controller_properties.min_speed, yaw_controller_properties.max_lat_accel, yaw_controller_properties.max_steer_angle) # @done Check what are actual correct properties for tau and ts for the lowpass filter self.low_pass_filter_pid = LowPassFilter( lowpass_filter_properties.brake_deadband, lowpass_filter_properties.ts) self.low_pass_filter_yaw_controller = LowPassFilter( lowpass_filter_properties.wheel_radius, lowpass_filter_properties.ts) def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, is_dbw_enabled): throttle = 0.0 brake = 0.0 steer = 0.0 if not is_dbw_enabled: rospy.loginfo( "Gauss - DBW not enabled, resetting PID, throttle, brake and steer" ) self.pid.reset() return throttle, brake, steer # Compute the error for the PID error = proposed_linear_velocity - current_linear_velocity rospy.loginfo("Gauss - Got error for PID: " + str(error)) pid_result = self.pid.step(error=error, sample_time=1.0 / 50.0) rospy.loginfo("Gauss - PID Result: " + str(pid_result)) stopdeadzone = 0.5 if pid_result >= 0.0 and proposed_linear_velocity >= stopdeadzone: # We want to accelerate throttle = self.scale(pid_result, self.max_throttle) brake = 0.0 else: # We want to decelerate throttle = 0.0 brake = self.scale(abs(pid_result), self.max_brake_torque) # Apply low pass filter on braking brake = self.low_pass_filter_pid.filt(brake) steer = self.yaw_controller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) # Apply low pass filter on steering kSteeringFactor = 1.2 steer = self.low_pass_filter_yaw_controller.filt( steer) * kSteeringFactor rospy.loginfo("Gauss - Throttle: " + str(throttle)) rospy.loginfo("Gauss - Brake: " + str(brake)) rospy.loginfo("Gauss - Steering: " + str(steer)) # Return throttle, brake, steer return throttle, brake, steer def scale(self, value, scale): kStretch = 0.5 if value < 0.0: return 0.0 else: return scale * math.tanh(value * kStretch)
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lat_accel = kwargs['max_lat_accel'] self.max_steer_angle = kwargs['max_steer_angle'] self.min_speed = 0.0 self.yaw_contoller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) kp = 0.1 ki = 0.007 kd = 0.0 minth = 0.0 maxth = 0.2 self.throttle_contoller = PID(kp, ki, kd, minth, maxth) tau = 150 ts = 75 self.lpf_velocity = LowPassFilter(tau, ts) # self.lpf_steering = LowPassFilter(tau, ts) self.t0 = rospy.get_time() def control(self, proposed_twist, current_twist, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_contoller.reset() return 0.0, 0.0, 0.0 proposed_linear_velocity = proposed_twist.twist.linear.x proposed_angular_velocity = proposed_twist.twist.angular.z current_linear_velocity = current_twist.twist.linear.x current_angular_velocity = current_twist.twist.angular.z current_linear_velocity = self.lpf_velocity.filt(current_linear_velocity) # Filter current linear velocity err_linear_velocity = proposed_linear_velocity - current_linear_velocity t1 = rospy.get_time() sample_time = t1 - self.t0 self.t0 = t1 throttle = self.throttle_contoller.step(err_linear_velocity, sample_time) brake = 0.0 if proposed_linear_velocity == 0.0 and current_linear_velocity < 0.1: throttle = 0.0 brake = 400 # from walkthrough elif throttle < 0.1 and err_linear_velocity < 0: throttle = 0.0 decel = max(err_linear_velocity, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius steering = self.yaw_contoller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = kwargs['vehicle_mass'] self.fuel_capacity = kwargs['fuel_capacity'] self.brake_deadband = kwargs['brake_deadband'] self.decel_limit = kwargs['decel_limit'] self.accel_limit = kwargs['accel_limit'] self.wheel_radius = kwargs['wheel_radius'] self.wheel_base = kwargs['wheel_base'] self.steer_ratio = kwargs['steer_ratio'] self.max_lat_accel = kwargs['max_lat_accel'] self.max_steer_angle = kwargs['max_steer_angle'] self.min_speed = 0.0 self.yaw_contoller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) kp = 0.1 ki = 0.007 kd = 0.0 minth = 0.0 maxth = 0.2 self.throttle_contoller = PID(kp, ki, kd, minth, maxth) tau = 150 ts = 75 self.lpf_velocity = LowPassFilter(tau, ts) # self.lpf_steering = LowPassFilter(tau, ts) self.t0 = rospy.get_time() def control(self, proposed_twist, current_twist, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_contoller.reset() return 0.0, 0.0, 0.0 proposed_linear_velocity = proposed_twist.twist.linear.x proposed_angular_velocity = proposed_twist.twist.angular.z current_linear_velocity = current_twist.twist.linear.x current_angular_velocity = current_twist.twist.angular.z current_linear_velocity = self.lpf_velocity.filt( current_linear_velocity) # Filter current linear velocity err_linear_velocity = proposed_linear_velocity - current_linear_velocity t1 = rospy.get_time() sample_time = t1 - self.t0 self.t0 = t1 throttle = self.throttle_contoller.step(err_linear_velocity, sample_time) brake = 0.0 if proposed_linear_velocity == 0.0 and current_linear_velocity < 0.1: throttle = 0.0 brake = 400 # from walkthrough elif throttle < 0.1 and err_linear_velocity < 0: throttle = 0.0 decel = max(err_linear_velocity, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius steering = self.yaw_contoller.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) return throttle, brake, steering
class 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, *args, **kwargs): # TODO: Implement #pass kp = 2.01 ki = 0.001 kd = 0.01 kp = 1.6 ki = 0.0 kd = 0.001 #kp = 3.03 #kd = 0.012 mn = -1.0 mx = 1.0 self.pid_controller = PID(kp, ki, kd, mn, mx) ts = 1.0 / 50.0 tau = 1.0 / 1000 #default value tau = (7.0 / 8.0) * ts * 0.632 self.throttle_filter = LowPassFilter(tau, ts) self.steer_filter = LowPassFilter(tau, ts) def control(self, *args, **kwargs): current_linear_velocity = kwargs['current_vel'] target_linear_velocity = kwargs['target_vel'] target_angular_velocity = kwargs['target_angular_vel'] yaw_controller = kwargs['yaw_control'] max_brake = (GAS_DENSITY * kwargs['fuel_capacity'] + kwargs['vehicle_mass']) * \ kwargs['decel_limit'] * kwargs['wheel_radius'] # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer actual_target_vel = target_linear_velocity if 0: if target_linear_velocity < 0.015: #to protect against small movement target_linear_velocity = 0 #rospy.loginfo("target velocity: %f %f %f", actual_target_vel, target_linear_velocity, current_linear_velocity) error = (target_linear_velocity - current_linear_velocity) if 0: #ease on throttle if current is trying to meet the target_linear_velocity #but brake quickly if the current exceeds target if abs(error) < 0.01: #error < 0.05 and error > -0.0001 : error = 0 #rospy.loginfo("error:%f", error) #rospy.loginfo("kd:%f %f %f", self.pid_controller.kp, self.pid_controller.kd, self.pid_controller.ki) noise_throttle = self.pid_controller.step(error, sample_time=1.0 / 50.0) #filter the throttle throttle = self.throttle_filter.filt(noise_throttle) #throttle = noise_throttle # for time being noise_steering = yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_linear_velocity) steering = self.steer_filter.filt(noise_steering) brake = 0.0 actual_th = throttle if throttle < 0: brake = throttle * max_brake throttle = 0 #rospy.loginfo("th:%f steer: %f brake:%f", actual_th, steering, 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): # TODO: Implement ## Initialize Throttle Controller as PID: kp = 0.123757 ## used values from PID project to begin with ki = 0.0 kd = 0.770457 mn = 0 mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) ## Initialize yaw controller:: self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) ## Initialize low pass filter:: tau = 0.5 ts = 0.2 self.lpf = LowPassFilter(tau, ts) ## Initialize parameters:: self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, curr_vel, linear_vel, angular_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # Reset to stop if dbw is not enabled: if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 # Filter out the curr_vel to remove high frequency noise: curr_vel = self.lpf.filt(curr_vel) self.last_vel = curr_vel # Compute Steering using Yaw Controller : steering = self.yaw_controller.get_steering(linear_vel, angular_vel, curr_vel) #rospy.logwarn('linera_Vel $s ,current vel %s, angular_vel: %s', #linear_vel, curr_vel, angular_vel) # Compute inputs for the Throttle controller step function (error, sample_time): vel_err = linear_vel - curr_vel sample_time = rospy.get_time() - self.last_time throttle = self.throttle_controller.step(vel_err, sample_time) #Since Throttle is set now, break should be set to zero brake = 0 # Store curr values of time and velocity into variable self.last_time = rospy.get_time() self.last_vel = curr_vel # Check on braking conditions: # Assumption here is if, linear_vel is zero and curr velocity is <0.1, we are trying to stop: if linear_vel == 0 and curr_vel < 0.1: throttle = 0 brake = 400 # set to max torque to stay at stop # Assumption -> if vel_error is negative, vehicle is moving faster than expected, we need to decelerate by applying brake if throttle < 0.1 and vel_err < 0: throttle = 0 decel = max(vel_err, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius 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'] min_speed = 0 self.s_lpf = LowPassFilter(tau=0.5, ts=1) self.linear_pid = PID(kp=0.8, ki=0, kd=0.05, mn=self.decel_limit, mx=0.5 * self.accel_limit) self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, min_speed, self.max_lat_accel, self.max_steer_angle) self.steering_pid = PID(kp=0.15, ki=0.001, kd=1, mn=-self.max_steer_angle, mx=self.max_steer_angle) def control(self, proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity, cross_track_error, duration_in_seconds): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer linear_velocity_error = proposed_linear_velocity - current_linear_velocity velocity_correction = self.linear_pid.step(linear_velocity_error, duration_in_seconds) brake = 0 throttle = velocity_correction if (throttle < 0): deceleration = abs(throttle) brake = ( self.vehicle_mass + self.fuel_capacity * GAS_DENSITY ) * self.wheel_radius * deceleration if deceleration > self.brake_deadband else 1. throttle = 0.0 predictive_steering = self.yaw_controller.get_steering( proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) corrective_steering = self.steering_pid.step(cross_track_error, duration_in_seconds) steering = predictive_steering + corrective_steering * 0.33 steering = self.s_lpf.filt(steering) return throttle, brake, steering def reset(self): self.linear_pid.reset() self.steering_pid.reset()
class Controller(object): def __init__(self, vehicle_mass, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, max_steer_angle, max_lat_accel, steer_ratio): # TODO: Implement # Steering self.yc = YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel, max_steer_angle) # Throttle kp = 0.3 ki = 0.1 kd = 0 mn = 0 #min throttle mx = 0.4 #max throttle self.tc = PID(kp, ki, kd, mn, mx) # LPF tau = 0.5 ts = 0.02 self.lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass 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, current_linear_velocity, current_angular_velocity, dbw_status): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_status: self.tc.reset() return 0., 0., 0. current_linear_velocity = self.lpf.filt(current_linear_velocity) steering = self.yc.get_steering(proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity) vel_error = proposed_linear_velocity - current_linear_velocity self.last_vel = current_linear_velocity current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.tc.step(vel_error, sample_time) brake = 0 if proposed_linear_velocity == 0 and current_linear_velocity < 0.1: throttle = 0 brake = 700 elif throttle < 0.01 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, *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): 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) # Parameters for the throttle control PID controller # kp = 0.3 #kp = 1.0 #kp = 1.5 #kp = 0.5 #kp = 0.2 #kp = 0.05 #kp = 0.02 #kp = 0.03 #kp = 0.045 #kp = 0.035 #kp = 0.03 #kp = 0.027 #kp = 0.02 #kp = 0.5 #kp = 0.035 ki = 0.1 #ki = 0.3 #ki = 0.0 #ki = 0.02 #ki = 0.005 #ki = 0.01 #ki = 0.005 #ki = 0.008 #ki = 0.007 #ki = 0.006 #ki = 0.0055 #ki = 0.005 #ki = 0.003 #ki = 0.004 #ki = 0.0045 #ki = 0.003 #ki = 0.0 kd = 0.0 #kd = 0.04 #kd = 0.0 #kd = 0.005 #kd = 0.001 #kd = 0.0005 #kd = 0.0 mn = 0.0 # Minimum throttle value # mx = 0.2 # Maximum throttle value # self.throttle_controller = PID(kp, ki, kd, mn, mx) # Low pass filter to reduce the effect of the noise of the incoming velocity # tau = 0.5 # 1/(2pi*tau)_ = cutoff frequency # ts = 0.02 # [s] 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 # Check if the dbw is enabled in order not to erroneously add to the I part of the PID controller # 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) # Calculate the difference between where the vehicle is requested to be vs where to vehicle currently is # vel_error = linear_vel - current_vel self.last_vel = current_vel # Get the sample time for each step of the PID controller # current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0.0 # If the requested velocity is 0 and the vehicle has come to a standstill, apply the brakes # if linear_vel == 0.0 and current_vel < 0.1: throttle = 0.0 brake = 400.0 # [N*m] - To hold the car in place when waiting for the traffic light to turn from red to green. Acceleration ~ 1 [m/s^2] # # If the requested velocity is smaller than the current velocity, apply the brakes with a factor depending on the required deceleration and maximum deceleration # 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 # [N*m] Torque # 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.3 ki = 0.1 kd = 0.0 mn = 0.0 mx = 1.0 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): # Done: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # Video 8: 7.01 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: # When we need to break for traffic light throttle = 0 brake = 400 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 # In case debugging needed then enter log values here return throttle, brake, steering
class Controller(object): def __init__(self, max_speed, 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.4 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = max_speed * 0.005 # 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 + fuel_capacity*GAS_DENSITY self.fuel_capaciy = 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, target_velocity, current_velocity): current_vel = self.vel_lpf.filt(current_velocity.linear.x) steering = self.yaw_controller.get_steering( target_velocity.linear.x, target_velocity.angular.z, current_velocity.linear.x) vel_error = target_velocity.linear.x - 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 target_velocity.linear.x == 0. and current_vel < 0.1: throttle = 0 brake = 400 elif vel_error < 0.: decel = max(vel_error, self.decel_limit) brake = 0.2*abs(decel)*self.vehicle_mass*self.wheel_radius # Torque N*m if vel_error == 0.0 and throttle == 0.0: brake = 0.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): # TODO: Implement self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # 0.1 m/s is the lowest speed of the car # PID gain parameters 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) #the velocity data that's coming from the messages is noisy, so low-pass filter is used to filter out all the high frequency noise in the velocity data tau = 0.5 # 1/(2*pi*tau) = cut-off 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 the vehicle is just sitting and in our pid we have an integral term and the DBW is enabled, then the intergral # term will be just accumulating error # So, if drive-by-wire is not enabled, then reset the controller if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. # Filter the current velocity from /current_velocity topic using the low-pass filter 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 car is supposed to remain stopped # Carla has an automatic transmission, which means the car will roll forward if no brake and no throttle is applied # So, a minimum brake force to prevent Carla from rolling has to be applied 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 # If we need to decelerate 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 # Braking torque in N-m return throttle, brake, steering
class Controller(object): def __init__(self, car_params): # TODO: Implement self.car_params = car_params self.dbw_enabled = False self.prev_time = None self.brake_db = car_params.brake_db self.steer_filter = LowPassFilter(0.9, 0.8) self.yaw_controller = YawController(car_params.wheel_base, car_params.steer_ratio, car_params.min_speed, car_params.max_lat_accel, car_params.max_steer_angle) self.pid = PID(kp=0.7, ki=0.003, kd=0.1, mn=car_params.decel_limit, mx=car_params.accel_limit) def control(self, time, ref_lin_vel, ref_ang_vel, lin_vel, ang_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # TODO_NEHAL - Check if it works without self.dbw_enabled if not self.dbw_enabled and dbw_enabled: self.pid.reset() self.dbw_enabled = dbw_enabled if not dbw_enabled or self.prev_time is None: self.prev_time = time return 0.0, 0.0, 0.0 delta_time = time - self.prev_time self.prev_time = time result = self.pid.step(ref_lin_vel - lin_vel, delta_time) if result > 0: throttle = result brake = 0.0 elif math.fabs(result) > self.brake_db: throttle = 0.0 brake = (self.car_params.vehicle_mass + (self.car_params.fuel_capacity * GAS_DENSITY)) * -result * self.car_params.wheel_radius else: throttle = 0.0 brake = 0.0 if ref_lin_vel == 0 and lin_vel < 0.5: throttle = 0.0 temp_brake_val = (self.car_params.vehicle_mass + (self.car_params.fuel_capacity * GAS_DENSITY) ) * -1.0 * self.car_params.wheel_radius brake = max(brake, temp_brake_val) steer = self.yaw_controller.get_steering(ref_lin_vel, ref_ang_vel, lin_vel) steer = self.steer_filter.filt(steer) # 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): 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 = min(MAX_BRAKE, (abs(decel) * self.vehicle_mass * self.wheel_radius)) # Torque N*m if (current_time - self.log_time) > LOGGING_THROTTLE_FACTOR: self.log_time = current_time rospy.logwarn( "POSE: current_vel={:.2f}, linear_vel={:.2f}, vel_error={:.2f}" .format(current_vel, linear_vel, vel_error)) rospy.logwarn( "POSE: throttle={:.2f}, brake={:.2f}, steering={:.2f}".format( throttle, brake, steering)) return throttle, brake, steering
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) brake_deadband = rospy.get_param('~brake_deadband', .1) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) wheel_radius = rospy.get_param('~wheel_radius', 0.2413) wheel_base = rospy.get_param('~wheel_base', 2.8498) steer_ratio = rospy.get_param('~steer_ratio', 14.8) max_lat_accel = rospy.get_param('~max_lat_accel', 3.) max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) # TODO: Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) self.pid = PID(-0.9, -0.001, -0.07, decel_limit, accel_limit) self.lowpass_filter = LowPassFilter( 1.0, 2.0) # the less the value, the more smooth self.yaw_controller = YawController(wheel_base, steer_ratio, 0, max_lat_accel, max_steer_angle) self.brake_factor = vehicle_mass * wheel_radius self.brake_deadband = brake_deadband # TODO: Subscribe to all the topics you need to # init the vars self.current_velocity = None self.twist_cmd = None self.dbw_enabled = True # For debugging, use true self.last_timestamp = rospy.get_time() # subscribe rospy.Subscriber("/current_velocity", TwistStamped, self.current_velocity_cb) rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cmd_cb) rospy.Subscriber("/dbw_enabled", Bool, self.dbw_enabled_cb) rospy.loginfo("### Controller initialized Yeah") self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # TODO: Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) # init the vars ref_v = 0. cur_v = 0. err_v = 0. ref_yaw = 0. dt = 0.01 thro_cmd = 0. brake_cmd = 0. steer_cmd = 0. if self.twist_cmd and self.current_velocity: ref_v = self.twist_cmd.twist.linear.x ref_yaw = self.twist_cmd.twist.angular.z cur_v = self.current_velocity.twist.linear.x err_v = cur_v - ref_v dt = rospy.get_time() - self.last_timestamp self.last_timestamp = rospy.get_time() rospy.loginfo("CurV: %f, RefV: %f" % (cur_v, ref_v)) rospy.loginfo("Change the errors: " + str(err_v)) if self.dbw_enabled: acc = self.pid.step(err_v, dt) # get the acc if acc >= 0: thro_cmd = self.lowpass_filter.filt(acc) brake_cmd = 0. else: thro_cmd = 0. brake_cmd = self.brake_factor * ( -acc) + self.brake_deadband steer_cmd = self.yaw_controller.get_steering( ref_v, ref_yaw, cur_v) self.publish(thro_cmd, brake_cmd, steer_cmd) rospy.loginfo("Publish:" + str(thro_cmd) + " " + str(steer_cmd)) else: self.pid.reset() rate.sleep() def publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def twist_cmd_cb(self, msg): self.twist_cmd = msg rospy.loginfo("Get twist cmd:" + str(self.twist_cmd.twist)) def current_velocity_cb(self, msg): self.current_velocity = msg def dbw_enabled_cb(self, msg): self.dbw_enabled = msg
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement 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.2 # max throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) self.brake_controller = PID(0.3, 0.0, 0.0, 0.0, 700) tau = 0.5 # 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, linear_vel, dbw_enabled, current_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(current_vel, angular_vel, linear_vel) vel_error = -(linear_vel - current_vel) self.last_vel = current_vel print(vel_error) current_time = rospy.get_time() sampled_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sampled_time) brake = 0 #self.brake_controller.step(-vel_error, sampled_time) if linear_vel == 0.0 and current_vel < 0.1: 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 return throttle, brake, steering
class Controller(object): def __init__(self, **ros_param): """ :param ros_param: Note: sample time (sec) is based on the dbw node frequency 50Hz low_pass filter: val = w * cur_val + (1 - w) * prev_val w is 0 ~ 1 """ # used ros_param self.vehicle_mass = ros_param['vehicle_mass'] # self.fuel_capacity = ros_param['fuel_capacity'] self.brake_deadband = ros_param['brake_deadband'] self.decel_limit = ros_param['decel_limit'] self.accel_limit = ros_param['accel_limit'] self.wheel_radius = ros_param['wheel_radius'] self.last_time = rospy.get_time() # low pass filter for velocity self.vel_lpf = LowPassFilter(0.5, .02) # Init yaw controller min_speed = 0.1 # I think min_speed self.steer_controller = YawController(min_speed, **ros_param) self.throttle_lpf = LowPassFilter(0.05, 0.02) # w = 0.28 # Init throttle PID controller # TODO: tweaking kp = 0.5 ki = 0.005 kd = 0.1 acc_min = 0. acc_max = 0.5 #self.accel_limit self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max) def control(self, target_linear_velocity, target_angular_velocity, cur_linear_velocity, dbw_status): # Check input info is ready if not dbw_status: self.throttle_controller.reset() return 0., 0., 0. # dbw enabled: control! cur_linear_velocity = self.vel_lpf.filt(cur_linear_velocity) # get steer value steering = self.steer_controller.get_steering(target_linear_velocity, target_angular_velocity, cur_linear_velocity) # get throttle (could be < 0 so it will be updated by `get brake` as well) vel_err = target_linear_velocity - cur_linear_velocity current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_err, sample_time) # get brake brake = 0 if target_linear_velocity == 0. and cur_linear_velocity < 0.1: throttle = 0 brake = 400 # N * m - hold the car in place for the red traffic light elif throttle < .1 and vel_err < 0.: throttle = 0. decel_velocity = max(vel_err, self.decel_limit) # attention this value is < 0 # if less than brake_deaband, we don't need to add brake # The car will deceleration by friction just release peddle if abs(decel_velocity) > self.brake_deadband: brake = abs(decel_velocity) * self.vehicle_mass * self.wheel_radius else: brake = 0 return throttle, brake, steering def reset(self): self.throttle_controller.reset()
class Controller(object): def __init__(self, vehicle_mass, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # Controller paramters are from "DBW Walkthrough" tau = 0.5 ts = 0.02 self.current_velocity_lpf = LowPassFilter(tau, ts) kp = 0.3 ki = 0.1 kd = 0.0 throttle_min = 0.0 throttle_max = 0.4 self.throttle_pid = PID(kp, ki, kd, throttle_min, throttle_max) self.steering_yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) self.vehicle_mass = vehicle_mass self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius # For PID self.last_time = rospy.get_time() def control(self, dbw_enabled, cmd_linear_velocity, cmd_angular_velocity, current_velocity): if not dbw_enabled: self.throttle_pid.reset() return 0.0, 0.0, 0.0 velocity_error = None current_velocity_filtered = None current_velocity_filtered = self.current_velocity_lpf.filt( current_velocity) velocity_error = cmd_linear_velocity - current_velocity_filtered current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_pid.step(velocity_error, sample_time) brake = 0 # Close enough to being stationary if cmd_linear_velocity == 0.0 and current_velocity_filtered < 0.1: throttle = 0 brake = 700 # To prevent Carla from moving requires about 700 Nm of torque. # Want to slow down the car, but just letting off the throttle is not enough if velocity_error < 0 and throttle < 0.1: throttle = 0 decel = abs( velocity_error / 1.0) # a = dv/dt is in m/s^2, so divide dv (m/s) by 1 second decel_limit = abs(self.decel_limit) # torque = radius * force = radius * mass * acceleration if decel < decel_limit: brake = self.wheel_radius * self.vehicle_mass * decel else: brake = self.wheel_radius * self.vehicle_mass * decel_limit steering = self.steering_yaw_controller.get_steering( cmd_linear_velocity, cmd_angular_velocity, current_velocity) return throttle, brake, steering
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_cap, brake_deadband, decel_lmt, accel_lmt, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_ctrl = YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0.0 mn = 0.0 # Minimum throttle value mx = 0.4 # Maximum throttle value self.throttle_ctrl = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2*pi*tau) = cutoff frequency ts = 0.02 # sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_cap = fuel_cap self.brake_deadband = brake_deadband self.decel_lmt = decel_lmt self.accel_lmt = accel_lmt self.wheel_radius = wheel_radius self.last_time = rospy.get_time() self.last_vel = 0 def control(self, current_vel, angular_vel, linear_vel, dbw_enabled): # Return throttle, brake, steer if not dbw_enabled: self.throttle_ctrl.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_ctrl.get_steering(linear_vel, angular_vel, current_vel) vel_err = linear_vel - current_vel self.last_vel = current_vel curr_time = rospy.get_time() sample_time = curr_time - self.last_time self.last_time = curr_time throttle = self.throttle_ctrl.step(vel_err, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # to hold the car in place elif throttle < .1 and vel_err < 0: throttle = 0 decel = max(vel_err, self.decel_lmt) brake = abs( decel) * self.vehicle_mass * self.wheel_radius #Torqur N*m return throttle, brake, steering
class Controller(object): def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min_speed max_lat_accel, max_steer_angle, ) # PID coefficients kp = 0.3 ki = 0.1 kd = 0.0 # For convenience (no real limits on throttle): mn_th = 0.0 # Minimal throttle mx_th = 0.2 # Maximal throttle self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th) tau = 0.5 # 1 / (2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.wheel_radius = wheel_radius self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 filtered_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn( # '\nAngular vel: {}\n' # 'Target velocity: {}\n' # 'Current velocity: {}\n' # 'Filtered velocity: {}\n' # .format(angular_vel, linear_vel, current_vel, filtered_vel) # ) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel) vel_error = linear_vel - filtered_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0 and filtered_vel < 0.1: throttle = 0 brake = 400 # [N*m] -- to hold the car in place if we are stopped # at a light. Acceleration ~ 1 m/s/s elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque [N*m] return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): 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 = 0.1 self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.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.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 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 abs(linear_vel) < 0.0001 and abs(current_vel) < 0.1: throttle = 0. brake = 700 # Nm - 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 = -1. * decel * self.vehicle_mass * self.wheel_radius # Nm - Torque return throttle, brake, steering