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