class TwistController(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
self.wheel_base = kwargs['wheel_base']
self.vehicle_mass = kwargs['vehicle_mass']
self.fuel_capacity = kwargs['fuel_capacity']
steer_ratio = kwargs['steer_ratio']
min_speed = 0.
max_lat_accel = kwargs['max_lat_accel']
accel_limit = kwargs['accel_limit']
decel_limit = kwargs['decel_limit']
self.max_steer_angle = kwargs['max_steer_angle']
self.brake_deadband = kwargs['brake_deadband']
self.max_throttle_percentage = kwargs['max_throttle_percentage']
self.max_braking_percentage = kwargs['max_braking_percentage']
self.yaw_controller = YawController(
self.wheel_base, steer_ratio, min_speed, max_lat_accel, self.max_steer_angle)
self.current_linear_velocity_lowpassfilter = LowPassFilter(150, 75)
self.steer_lowpassfilter = LowPassFilter(150, 75)
self.throttle_pid = PID(0.1,0.007,0)
def control(self, proposed_twist, current_twist, dbw_enabled):
# Return throttle, brake, steer
proposed_linear_velocity = abs(proposed_twist.linear.x)
proposed_angular_velocity = proposed_twist.angular.z
current_linear_velocity = current_twist.linear.x
current_angular_velocity = current_twist.angular.z
current_linear_velocity = self.current_linear_velocity_lowpassfilter.filt(current_linear_velocity)
delta_linear_velocity = proposed_linear_velocity - current_linear_velocity
brake = 0.
throttle = 0.
if dbw_enabled:
throttle = self.throttle_pid.step(delta_linear_velocity, 0.02)
throttle = min(throttle, self.max_throttle_percentage)
if (throttle < 0.05 and current_linear_velocity < 1) or throttle < 0:
if current_linear_velocity > 1:
brake = 3250 * self.max_braking_percentage
else:
brake = 3250 * 0.01
steer = self.yaw_controller.get_steering(
proposed_linear_velocity, proposed_angular_velocity, current_linear_velocity)
steer = self.steer_lowpassfilter.filt(steer)
steer = max(-self.max_steer_angle, min(steer, self.max_steer_angle))
else:
throttle = 0.
brake = 0.
steer = 0.
self.steer_lowpassfilter.reset()
self.throttle_pid.reset()
rospy.loginfo("throttle:" + str(throttle) + "brake:" +
str(brake) + "steer" + str(steer))
return throttle, brake, steer
class Controller(object):
def __init__(self, *args, **kwargs):
self.vehicle_mass = args[0]
self.fuel_capacity = args[1]
self.brake_deadband = args[2]
self.decel_limit = args[3]
self.accel_limit = args[4]
self.wheel_radius = args[5]
self.wheel_base = args[6]
self.steer_ratio = args[7]
self.max_lat_accel = args[8]
self.max_steer_angle = args[9]
self.pid_velocity = PID(0.05, 0.0, 0.01)
self.pid_steering = PID(10.0, 0.0, 1.0)
self.lowpass_velocity = LowPassFilter(13)
self.lowpass_steering = LowPassFilter(4)
def control(self, *args, **kwargs):
""" vehicle controller
arguments:
- proposed linear velocity
- proposed angular velocity (radians)
- current linear velocity
- elapsed time
- dbw_enabled status
"""
target_vel = args[0]
target_steer = args[1]
cur_vel = args[2]
time_elapsed = args[3]
dbw_enabled = args[4]
vel_error = target_vel - cur_vel
velocity = self.pid_velocity.step(vel_error, time_elapsed)
if (velocity < 0.005) and (velocity > -0.005):
velocity = 0.0
velocity = self.lowpass_velocity.filt(velocity)
steer_error = target_steer
steer = self.pid_steering.step(steer_error, time_elapsed)
steer = self.lowpass_steering.filt(steer)
if DEBUG:
rospy.logerr('ctrl velocity: {}'.format(velocity))
rospy.logerr('ctrl steer: {}'.format(steer))
throttle = max(velocity, 0.0)
brake = math.fabs(
min(0.0, velocity)
) * 10000 # TODO: tune this multiplier or find a better way...
# Return throttle, brake, steer
return throttle, brake, steer
def pid_reset(self):
self.pid_velocity.reset()
self.pid_steering.reset()
self.lowpass_steering.reset()
self.lowpass_velocity.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,
stop_brake_torque):
self.yaw_controller = YawController(wheel_base, steer_ratio, ONE_MPH,
max_lat_accel, max_steer_angle)
kp = 0.9
ki = 0.007
kd = 0.2
mn = 0.0 # Minimum throttle.
mx = 0.2 # Maximum throttle.
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau_throttle = 0.5
ts_throttle = 0.02 # Sample time.
self.throttle_lpf = LowPassFilter(tau_throttle, ts_throttle)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.fuel_mass = self.fuel_capacity * GAS_DENSITY
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.stop_brake_torque = stop_brake_torque
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()
self.throttle_lpf.reset()
return 0, 0, 0
current_vel = self.throttle_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.0 and current_vel < 0.1:
throttle = 0
brake = self.stop_brake_torque # Nm
elif throttle < 0.1 and vel_error < 0:
throttle = 0.0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * (self.vehicle_mass + self.fuel_mass
) * self.wheel_radius # Torque N*m
return throttle, brake, steering
class Controller(object):
def __init__(self, vehicle):
self.loglevel = rospy.get_param('/loglevel', 3)
self.max_throttle = rospy.get_param('/max_throttle', 0.8)
self.full_stop_brake_keep = rospy.get_param('~full_stop_brake_keep',
1200)
self.full_stop_brake_limit = rospy.get_param('~full_stop_brake_limit',
0.1)
self.brake_deceleration_start = rospy.get_param(
'~brake_deceleration_start', -0.3)
self.vehicle = vehicle
self.yaw_controller = YawController(vehicle, 0.1)
self.throttle_controller = PID(0.4, 0.003, 0.04, 0, self.max_throttle)
self.velocity_filter = LowPassFilter(0.2, 0.2)
self.throttle_filter = LowPassFilter(1.8, 0.2)
self.last_time = rospy.get_time()
self.throttle_filter.reset(
0
) # initialize it for throttle so we will not have large throttle at begining
def reset(self):
self.throttle_controller.reset()
self.last_time = rospy.get_time()
def control(self, target_linear_velocity, target_angular_velocity,
current_velocity):
velocity = self.velocity_filter.filt(current_velocity)
steering = self.yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
velocity)
error = target_linear_velocity - velocity
current_time = rospy.get_time()
dt = current_time - self.last_time
throttle_raw = self.throttle_controller.step(error, dt)
throttle = self.throttle_filter.filt(throttle_raw)
self.last_time = current_time
brake = 0
if target_linear_velocity <= self.full_stop_brake_limit and velocity < 1: # target velocity is 0, and current velocity is small
throttle = 0
brake = self.full_stop_brake_keep
elif error < self.brake_deceleration_start: # target velocity is lower than current velocity, apply brake
throttle = 0
decel = max(error, self.vehicle.decel_limit)
brake = -decel * self.vehicle.mass * self.vehicle.wheel_radius
if self.loglevel >= 4:
rospy.loginfo(
"Control (%s, %s, %s, %s) -> throttle: %s <- %s, steer: %s, brake: %s",
target_linear_velocity, current_velocity, velocity, dt,
throttle, throttle_raw, 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):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle);
kp = 0.2
ki = 0.01
kd = 0.09
mn = decel_limit
mx = accel_limit
self.pid_controller = PID(kp, ki, kd, mn, mx);
self.lpf = LowPassFilter(0.5, 0.02)
self.last_time = rospy.get_time();
self.decel_limit = decel_limit
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
def control(self, dbw_enabled, current_vel, linear_vel, angular_vel):
if not dbw_enabled:
self.pid_controller.reset()
self.lpf.reset()
return 0., 0., 0
if (current_vel <= linear_vel):
current_vel = self.lpf.filt(current_vel)
cur_time = rospy.get_time()
time_diff = cur_time - self.last_time
self.last_time = cur_time
vel_diff = linear_vel - current_vel
throttle = self.pid_controller.step(vel_diff, time_diff)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)
brake = 0.0
if linear_vel == 0.0 and current_vel < 0.1:
brake = 400
throttle = 0.0
elif vel_diff < 0:
throttle = 0.0
decel = max(self.decel_limit, vel_diff)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
if (vel_diff < 0):
rospy.loginfo("t {0} b {1} s {2} vel {3} {4}".format(throttle, brake, steering, current_vel, linear_vel))
return throttle, brake, steering
class Controller(object):
def __init__(self, *args, **kwargs):
self.pid = PID(
kwargs['kp'], kwargs['ki'],
kwargs['kd']) #, mn, mx) these numvers are for the saturation
self.yaw_control = YawController(kwargs['wheel_base'],
kwargs['steer_ratio'],
kwargs['min_speed'],
kwargs['max_lat_accel'],
kwargs['max_steer_angle'])
self.steer_low_pass_filter = LowPassFilter(kwargs['tau'], kwargs['ts'])
self.speed_low_pass_filter = LowPassFilter(kwargs['tau'], kwargs['ts'])
self.vehicle_mass = kwargs['vehicle_mass']
self.wheel_radius = kwargs['wheel_radius']
self.last_t = time.time()
def reset(self):
self.pid.reset()
self.steer_low_pass_filter.reset()
self.speed_low_pass_filter.reset()
def control(self, reference, measured):
# dt
t = time.time()
dt = t - self.last_t
self.last_t = t
# vel error
error_vel = reference.linear.x - measured.linear.x
vel_control = self.pid.step(error_vel, dt)
vel_control = self.speed_low_pass_filter.filt(vel_control)
throttle = vel_control
brake = 0.
if vel_control < -0.1 and error_vel < 0:
throttle = 0
brake = abs(vel_control) * self.vehicle_mass * self.wheel_radius
elif reference.linear.x < 0.0001 and measured.linear.x < 0.1:
throttle = 0
brake = 700
steer = self.yaw_control.get_steering(reference.linear.x,
reference.angular.z,
measured.linear.x)
steer = self.steer_low_pass_filter.filt(steer)
return throttle, brake, steer
class TwistController(object):
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle,
decel_limit):
# PID brake controller
self.pid_brake = PID(0.5, 0, 0, mn=0, mx=-decel_limit)
self.low_pass_brake = LowPassFilter(0.3, STEP_TIME)
# PID throttle controller
self.pid_gas = PID(0.5, 0, 0, mn=0, mx=0.3)
self.low_pass_gas = LowPassFilter(0.5, STEP_TIME)
min_speed = 0.5
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
def control(self, current_long_velocity, desired_long_velocity,
desired_angular_velocity):
steer_angle = self.yaw_controller.get_steering(
desired_long_velocity, desired_angular_velocity,
current_long_velocity)
velocity_error = desired_long_velocity - current_long_velocity
if velocity_error < 0:
# set gas to 0 and apply brakes
gas_pos = 0.0
self.low_pass_gas.reset()
brake_pos_new = self.pid_brake.step(-velocity_error, STEP_TIME)
brake_pos = self.low_pass_brake.filt(brake_pos_new)
else:
# set brakes to 0 and apply gas
gas_pos_new = self.pid_gas.step(velocity_error, STEP_TIME)
gas_pos = self.low_pass_gas.filt(gas_pos_new)
brake_pos = 0.0
self.low_pass_brake.reset()
return gas_pos, brake_pos, steer_angle
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.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_base
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
# Controller
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
self.throttle_controller = PID(0.3, 0.1, 0.1, 0, 0.5)
# Lowpass filter
self.vel_lpf = LowPassFilter(0.5, 0.05)
self.yaw_lpf = LowPassFilter(0.5, 0.05)
self.last_vel = 0
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()
self.vel_lpf.reset()
self.yaw_lpf.reset()
return 0.,0.,0.
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)
steering = self.yaw_lpf.filt(steering)
velocity_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(velocity_error, sample_time)
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 700
elif throttle < 0.1 and velocity_error < 0:
throttle = 0
decel = max(velocity_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
else:
brake = 0
# Return throttle, brake, steer
return throttle, brake, steering
class Controller(object):
# def __init__(self, *args, **kwargs):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = .3
ki = 0.1
kd = 0.1
mn = 0. # minimum throttle value
mx = 0.5 # maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# low pass for velocity
#tau = 1./6.28/.1
#ts = 1.
tau = 0.5
ts = 0.2
self.vel_lpf = LowPassFilter(tau, ts)
# low pass for steering
#tau = 1./6.28/.5
#ts = 1.
self.steer_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()
# pass
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
# def control(self, *args, **kwargs):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
#reset low pass filter
self.vel_lpf.reset()
self.steer_lpf.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filt(current_vel)
# debug
# 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_raw = self.yaw_controller.get_steering(
linear_vel, angular_vel, current_vel)
# additional steering filter
steering = self.steer_lpf.filt(steering_raw)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
# rospy.loginfo("sample_time: {0}".format(sample_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 # Torque N*m
# return 1., 0., 0.
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, accel_limit,
decel_limit, brake_deadband, max_lat_accel, max_steer_angle,
vehicle_mass, fuel_capacity, wheel_radius):
# TODO: Implement
self.steer_yaw = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.accel_limit = accel_limit
self.decel_limit = decel_limit
self.brake_deadband = brake_deadband
self.accel = 0.0
self.total_vehicle_mass = vehicle_mass + fuel_capacity * GAS_DENSITY
self.wheel_radius = wheel_radius
self.accel_pid = PID(0.6, 0.0, 0.0, decel_limit, accel_limit)
self.accel_lpf = LowPassFilter(0.5, 0.5)
self.prev_time = None
def control(self, velocity, angular_velocity, target_velocity,
target_angular_velocity):
now = time.time()
if self.prev_time:
if target_velocity < 0.001:
# If the target_velocity is a stop then apply the brake to hold position.
# If the calculated accel is 0 due to 0 error the car will move.
# Limit from https://discussions.udacity.com/t/what-is-the-range-for-the-brake-in-the-dbw-node/412339
throttle = 0.0
brake = -1809.0
else:
dt = now - self.prev_time
velocity_error = target_velocity - velocity
throttle, brake = self.acceleration(velocity_error, dt)
else:
throttle = 0.0
brake = 0.0
steer = self.steer_yaw.get_steering(target_velocity,
target_angular_velocity, velocity)
self.prev_time = now
return throttle, abs(brake), steer
def acceleration(self, velocity_error, dt):
accel = self.accel_pid.step(velocity_error, dt)
accel = self.accel_lpf.filt(accel)
if accel <= 0.0 and accel > -self.brake_deadband:
throttle = 0.0
brake = 0.0
elif accel > 0.0:
# Acceleration required.
throttle = min(accel, self.accel_limit)
brake = 0.0
else:
# Braking required. Brake values passed to publish should be in units of torque (N*m).
# The correct values for brake can be computed using the desired acceleration,
# weight of the vehicle, and wheel radius.
# Note that https://discussions.udacity.com/t/what-is-the-range-for-the-brake-in-the-dbw-node/412339
# suggests the `accel` value is limited by self.decel_limit rather than the valye of `brake`.
throttle = 0.0
brake = self.total_vehicle_mass * max(
accel, self.decel_limit) * self.wheel_radius
return throttle, brake
def reset(self):
self.prev_time = None
self.accel_pid.reset()
self.accel_lpf.reset()
self.accel = 0.0
class Controller(object):
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle,
vehicle_mass, wheel_radius, fuel_capacity, accel_limit,
decel_limit):
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.vehicle_mass = vehicle_mass + fuel_capacity * GAS_DENSITY
self.wheel_radius = wheel_radius
self.accel_limit = accel_limit
self.decel_limit = decel_limit
self.acc_pid = PID(2.5, 0., 0., -1., 1.)
self.acc_filter = LowPassFilter(3., 1.)
self.steer_filter = LowPassFilter(1., 1.)
self.dbw_enabled = False
def control(self, tgt_linear, tgt_angular, cur_linear, cur_angular,
dbw_enabled):
if not dbw_enabled:
if self.dbw_enabled:
self.dbw_enabled = False
self.acc_pid.reset()
self.acc_filter.reset()
self.steer_filter.reset()
return 0., 0., 0.
dt = 0.02 # in seconds (~ 50 Hz)
v = max(0., cur_linear)
vel_error = tgt_linear - v
# Get the angle from the yaw_controller
yawcontroller = YawController(self.wheel_base, self.steer_ratio,
ONE_MPH, self.max_lat_accel,
self.max_steer_angle)
steer_raw = yawcontroller.get_steering(tgt_linear, tgt_angular,
cur_linear)
steer = self.steer_filter.filt(steer_raw)
acc_raw = self.acc_pid.step(vel_error, dt)
acc = self.acc_filter.filt(acc_raw)
# If dbw was just activated, we wait for the next call
if not self.dbw_enabled:
self.dbw_enabled = True
return 0., 0., 0.
# drag force due to air
F_drag = 0.4 * v * v
# rolling resistance
c_rr = .01 + 0.005 * pow(v / 28., 2)
F_rr = c_rr * self.vehicle_mass * 9.8
torque = (acc * self.vehicle_mass + F_drag + F_rr) * self.wheel_radius
max_torque = 1000.
# there is a constant bias of throttle we need to correct for
torque -= 0.02 * max_torque
if acc > 0:
throttle = min(.25, max(torque, 0.) / max_torque)
brake = 0.
else:
brake = max(0., -torque)
# for idle state, we apply a small constant brake :
if tgt_linear < 0.01:
brake = 10.
throttle = 0.0
# rospy.logdebug("throttle : %s %s %s %s", throttle, acc, cur_linear, brake)
# Return throttle, brake, steer
return throttle, brake, steer
class Controller(object):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit,
wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
kp = 0.55
ki = -0.0001
kd = -0.005
mn = 0. # Minimum throttle value
mx = accel_limit # Maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.05 # 1/(2pi*tau) = cutoff frequency
ts = .02 # Sample time
self.curr_vel_lpf = LowPassFilter(tau, ts)
self.curr_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.wheel_base = wheel_base
self.last_time = rospy.get_time()
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()
self.curr_vel_lpf.reset()
self.curr_ang_vel_lpf.reset()
self.last_time = rospy.get_time()
return 0., 0., 0.
current_vel = self.curr_vel_lpf.filt(current_vel)
curr_ang_vel = self.curr_ang_vel_lpf.filt(curr_ang_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)
vel_error = linear_vel - current_vel #+ abs(angular_vel - curr_ang_vel) * self.wheel_base
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.2:
throttle = 0.
steering = 0.
brake = 400. #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.06:
throttle = 0.
decel = max(vel_error * 0.5, self.decel_limit)
brake = abs(decel) * (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * 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, sampling_rate):
# TODO: Implement
# pass
# member variables
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.sampling_rate = sampling_rate
# update vehicle mass to take into account fuel capacity
self.vehicle_mass = self.vehicle_mass + (self.fuel_capacity *
GAS_DENSITY)
# Yaw Controller for Steering
self.yaw_controller = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
# PID Controller for Throttle
# this control gains are found experimentally
# kp = THROTTLE_KP
# ki = THROTTLE_KI
# kd = THROTTLE_KD
# min_throttle = MIN_THROTTLE
# max_throttle = MAX_THROTTLE
# self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle)
self.throttle_controller = PID(THROTTLE_KP, THROTTLE_KI, THROTTLE_KD,
-1 * self.accel_limit, accel_limit)
# Velocity Low-Pass Filter
tau = LPF_TAU # 1/(2*pi*tau) = cutoff frequency
ts = LPF_TS # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
# self.last_time = rospy.get_time()
self.last_linear_vel = 0.
def control(self, proposed_linear_vel, proposed_angular_vel,
current_linear_vel, dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
# if the Drive-By-Wire mode is off (safety driver takes control of the car), we are not in autonomous mode, so
# we don't want the integral term of PID to controller to accumulate errors, so reset it when the dbw mode
# is not enabled
if not dbw_enabled:
self.throttle_controller.reset()
self.vel_lpf.reset()
return 0., 0., 0.
# clean the current velocity by passing it through the low-pass filter to get rid of the high frequency noises
current_linear_vel = self.vel_lpf.filt(current_linear_vel)
# get the steering from the Yaw Controller
steering = self.yaw_controller.get_steering(proposed_linear_vel,
proposed_angular_vel,
current_linear_vel)
# get velocity error between proposed and current velocities
vel_error = proposed_linear_vel - current_linear_vel
self.last_linear_vel = current_linear_vel
# # get sample time
# current_time = rospy.get_time()
# sample_time = current_time = self.last_time
# self.last_time = current_time
# sample time = 1/rate_hw (50)
sample_time = 1 / float(self.sampling_rate)
# get the throttle from the Throttle PID controller
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
# if proposed vel is 0 and current vel is small, we probably want to stop
if proposed_linear_vel == 0. and current_linear_vel < 0.1:
throttle = 0.
# braking torque in N*m - to hold the car in place if we are stopped at a light. Acceleration ~1m/s^2.
# This works for the simulator, but to hold Carla stationary, this has to be changed to ~700 Nm of torque
brake = 400
# if throttle is small and velocity error is negative, we are probably going faster than we want to and our pid
# is letting up on the throttle, but we want to slow down
elif throttle < 0.1 and vel_error < 0.:
throttle = 0.
decel = max(vel_error, self.decel_limit)
# calculate braking torque (N*m) as desired acceln/decel * vehicle mass * wheel radius. decel here is -ve.
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
rospy.loginfo(
'proposed_linear_vel: {0} proposed_angular_vel: {1} current_linear_vel: {2}'
.format(proposed_linear_vel, proposed_angular_vel,
current_linear_vel))
rospy.loginfo('Throttle: {0} Brake: {1} Steering: {2}'.format(
throttle, brake, steering))
return throttle, brake, steering
class Controller(object):
def __init__(self, **kwargs):
self.wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
self.max_lateral_acceleration = kwargs['max_lateral_acceleration']
self.max_steer_angle = kwargs['max_steer_angle']
self.vehicle_mass = kwargs['vehicle_mass']
self.wheel_radius = kwargs['wheel_radius']
self.fuel_capacity = kwargs['fuel_capacity']
self.acceleration_limit = kwargs['acceleration_limit']
self.deceleration_limit = kwargs['deceleration_limit']
self.brake_deadband = kwargs['brake_deadband']
# add mass of fuel to vehicle mass
self.vehicle_mass = self.vehicle_mass + (self.fuel_capacity * GAS_DENSITY)
self.brake_torque = self.vehicle_mass * self.wheel_radius
# get max allowed velocity in kmph
self.MAX_VELOCITY = rospy.get_param('/waypoint_loader/velocity')
# convert velocity to m/s
self.MAX_VELOCITY = self.MAX_VELOCITY * 0.2778
self.throttle_PID = PID(THROTTLE_KP, THROTTLE_KI, THROTTLE_KD,
-1*self.acceleration_limit, self.acceleration_limit)
yaw_controller_init_kwargs = {
'wheel_base' : self.wheel_base,
'steer_ratio' : self.steer_ratio,
'max_lateral_acceleration' : self.max_lateral_acceleration,
'min_speed' : 0.1,
'max_steer_angle' : self.max_steer_angle
}
self.yaw_controller = YawController(**yaw_controller_init_kwargs)
self.throttle_lowpass = LowPassFilter(THROTTLE_TAU,THROTTLE_TS)
self.last_time = rospy.get_time()
def control(self, **kwargs):
'''
controls the vehicle taking into consideration the twist_cmd
Params:
- `target_linear_velocity` is the desired linear velocity the car should reach
- `target_angular_velocity` is the desired angular velocity the car should reach
- `current_velocity` is the current velocity of the car
- `dbw_enabled` - Boolean, whether auto-drive is enabled or not
- `dt` - time in seconds, between each twist_cmd
Returns throttle, brake and steer values
'''
target_linear_velocity = kwargs['target_linear_velocity']
target_angular_velocity = kwargs['target_angular_velocity']
current_velocity = kwargs['current_velocity']
dbw_enabled = kwargs['dbw_enabled']
dt = kwargs['dt']
rospy.loginfo('Current velocity: {}'.format(current_velocity))
rospy.loginfo('target_linear_velocity : {}'.format(target_linear_velocity))
# pass current_velocity through low pass filter to reduce high freq variations
current_velocity = self.throttle_lowpass.filter(current_velocity)
# if driver has taken control, reset throttle PID to avoid accumulation
# of error
if not dbw_enabled:
self.throttle_PID.reset()
self.throttle_lowpass.reset()
error = target_linear_velocity - current_velocity
throttle = self.throttle_PID.step(error, dt)
steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity,
current_velocity)
#steer = self.steer_PID.step(steer,dt)
#steer = self.steer_lowpass.filter(steer)
# if target linear velocity is very less, apply hard brake
brake = 0.0
if target_linear_velocity == 0.0 and current_velocity < 0.1:
brake = 400
throttle = 0.0
elif throttle < 0.1 and error < 0:
throttle = 0.0
decel = min(abs(error/dt), abs(self.deceleration_limit))
brake = decel * self.brake_torque
return throttle, brake, steer
class Controller(object):
def __init__(self, params):
self.yaw_controller = YawController(
wheel_base = params['wheel_base'],
steer_ratio = params['steer_ratio'],
min_speed = params['min_speed'],
max_lat_accel = params['max_lat_accel'],
max_steer_angle = params['max_steer_angle'])
self.prev_linear_velocity = 0.0
self.filter_steer = True
if self.filter_steer:
self.steer_filter = LowPassFilter(time_interval=0.1,
time_constant=1.0)
self.filter_throttle = True
if self.filter_throttle:
self.throttle_filter = LowPassFilter(time_interval=0.1,
time_constant=0.1)
self.break_constant = 0.1
self.vehicle_mass = params['vehicle_mass']
self.fuel_capacity = params['fuel_capacity']
self.brake_deadband = params['brake_deadband']
self.wheel_radius = params['wheel_radius']
self.sample_rate = params['sample_rate']
self.throttle_pid = PID(
10.,
.0,
5.,
0,
1
)
self.steer_pid = PID(
1.0,
.0,
5.0,
-params['max_steer_angle'],
params['max_steer_angle']
)
# assume tank is full when computing total mass of car
self.total_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY
def control(self, linear_velocity, angular_velocity, current_velocity,
enabled = True):
velocity_diff = linear_velocity - current_velocity
target_velocity_diff = linear_velocity - self.prev_linear_velocity
time_interval = 1.0 / self.sample_rate
throttle = 0.0
brake = 0.0
steer = 0.0
if enabled:
if target_velocity_diff < 0.0 or linear_velocity < 1.0:
# Brake in torque [N*m]
acc = velocity_diff/time_interval # Required acceleration
brake = max(self.break_constant*math.fabs(acc), 0.19) * self.total_mass * self.wheel_radius
self.throttle_pid.reset()
else:
throttle = self.throttle_pid.step(velocity_diff, time_interval)
# Pass the low-pass filter
if self.filter_throttle:
throttle = self.throttle_filter.filt(throttle)
if USE_STEER_PID:
steer = self.steer_pid.step(angular_velocity, time_interval)
else:
steer = self.yaw_controller.get_steering(
linear_velocity,
angular_velocity,
current_velocity
)
# Pass the low-pass filter
if self.filter_steer:
steer = self.steer_filter.filt(steer)
# Update the previous target
self.prev_linear_velocity = linear_velocity
else:
self.throttle_pid.reset()
self.steer_pid.reset()
self.throttle_filter.reset()
self.steer_filter.reset()
# Logwarn for Debugging PID
# run ```rosrun rqt_pt rqt_plot``` and set topics for plotting the actual velocity
# rospy.logwarn("Throttle : {}".format(throttle))
# rospy.logwarn(" Brake : {}".format(brake))
# rospy.logwarn(" Steer : {}".format(steer))
# rospy.logwarn("--- ")
return throttle, brake, steer
class Controller(object):
def __init__(self, steer_ratio, decel_limit, accel_limit, max_steer_angle,
wheel_base, max_let_accel, max_throttle_percent,
max_braking_percent):
self.steer_ratio = steer_ratio
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.max_steer_angle = max_steer_angle
self.max_let_accel = max_let_accel
self.wheel_base = wheel_base
self.max_braking_percent = max_braking_percent
# We use proportional factors between Carla and the simulator.
# The simulator only was used for calibration. Max values for Carla were extracted through
# "dbw_test.py" and provided rosbag
calib_throttle = max_throttle_percent / 0.4
calib_brake = max_braking_percent / 100
self.throttle_pid = PID(0.1 * calib_throttle, 0.001 * calib_throttle,
0, 0, max_throttle_percent)
self.brake_pid = PID(60. * calib_brake, 1. * calib_brake, 0, 0,
max_braking_percent)
tau = 0.1
self.throttle_filter = LowPassFilter(tau, DT)
self.brake_filter = LowPassFilter(tau, DT)
self.steer_filter = LowPassFilter(tau, DT)
self.last_time = 0
self.DT = DT
self.brakeLatch = False
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
0.5, self.max_let_accel,
self.max_steer_angle)
def control(self, target_linear_velocity, target_angular_velocity,
current_linear_velocity, dbw_status, log_handle):
'''Defines target throttle, brake and steering values'''
# Update DT
new_time = rospy.get_rostime()
if self.last_time: # The first time, we are not able to calculate DT
self.DT = (new_time - self.last_time).to_sec()
self.last_time = new_time
if dbw_status:
velocity_error = target_linear_velocity - current_linear_velocity
# if we're going too slow, release the brakes (if they are applied)
# This essentially adds hysterisis: the brakes are only enabled if our velocity error is negative, and only
# released if the velocity error is positive 2 or greater.
if velocity_error > 1:
self.brakeLatch = False
if not self.brakeLatch:
throttle = self.throttle_pid.step(velocity_error, self.DT)
brake = 0
self.brake_pid.reset()
self.brake_filter.reset()
# if we go too fast and we cannot decrease throttle, we need to start braking
if (velocity_error < 0
and throttle is 0) or (velocity_error < -1):
self.brakeLatch = True
else:
# We are currently braking
throttle = 0
self.throttle_pid.reset()
self.throttle_filter.reset()
brake = self.brake_pid.step(-velocity_error, self.DT)
# If we're about to come to a stop, clamp the brake command to some value to hold the vehicle in place
if current_linear_velocity < .1 and target_linear_velocity == 0:
throttle = 0
brake = self.max_braking_percent
# implement steering controller
steer_target = self.yaw_controller.get_steering(
target_linear_velocity, target_angular_velocity,
current_linear_velocity)
# filter commands
throttle = self.throttle_filter.filt(throttle)
brake = self.brake_filter.filt(brake)
steering = self.steer_filter.filt(steer_target)
else:
self.brakeLatch = False
self.throttle_pid.reset()
self.brake_pid.reset()
self.throttle_filter.reset()
self.brake_filter.reset()
self.steer_filter.reset()
throttle, brake, steering = 0, 0, 0
pid_throttle, velocity_error = 0, 0
# Log data
throttle_P, throttle_I, throttle_D = self.throttle_pid.get_PID()
brake_P, brake_I, brake_D = self.brake_pid.get_PID()
steer_P, steer_I, steer_D = 0, 0, 0
self.log_data(log_handle, throttle_P, throttle_I, throttle_D, brake_P,
brake_I, brake_D, velocity_error, self.DT,
int(self.brakeLatch))
return throttle, brake, steering
def log_data(self, log_handle, *args):
log_handle.write(','.join(str(arg) for arg in args) + ',')
class Controller(object):
""" Twist controller to predict throttle, brake, and steer."""
def __init__(self, *args, **kwargs):
self.decel_limit = kwargs.get('decel_limit')
self.accel_limit = kwargs.get('accel_limit')
self.max_steer_angle = kwargs.get('max_steer_angle')
# Vehicle mass, fuel mass and wheel radius are required to calculate braking torque
self.vehicle_mass = kwargs.get('vehicle_mass')
self.wheel_radius = kwargs.get('wheel_radius')
self.fuel_mass = kwargs.get(
'fuel_capacity') * GAS_DENSITY # Assuming a full tank of gas
# Controllers
self.yaw_controller = YawController(
wheel_base=kwargs.get('wheel_base'),
steer_ratio=kwargs.get('steer_ratio'),
min_speed=kwargs.get('min_speed'),
max_lat_accel=kwargs.get('max_lat_accel'),
max_steer_angle=kwargs.get('max_steer_angle'))
self.throttle_filter = LowPassFilter(0.96, 1.0)
self.brake_filter = LowPassFilter(0.96, 1.0)
self.steer_filter = LowPassFilter(0.96, 1.0)
self.acceleration = 0
self.timestamp = None
def control(self, *args, **kwargs):
# For steering, we need the vehicle velocity (angular and linear)
# For velocity, we need the current velocity and target velocity to calculate the error
current_velocity = kwargs.get('current_velocity')
linear_velocity = kwargs.get('linear_velocity')
angular_velocity = kwargs.get('angular_velocity')
target_velocity = kwargs.get('target_velocity')
throttle, brake, steer = 0., 0., 0.
# If this is the first time control() is called
if self.timestamp is None:
self.timestamp = rospy.get_time()
return throttle, brake, steer
current_timestamp = rospy.get_time()
delta_time = current_timestamp - self.timestamp
timestamp = current_timestamp
# Velocity is in meters per second
current_velocity = current_velocity
velocity_error = target_velocity - current_velocity
# Make sure we have a valid delta_time. We don't want to divide by zero.
# Since we're publishing at 50Hz, the expected delta_time should be around 0.02
if delta_time > 0.01:
if velocity_error > 10:
if self.acceleration < 0: self.acceleration = 0
self.acceleration = 1.
elif velocity_error > 2.5:
self.acceleration += 0.224
self.acceleration = min(self.acceleration, self.accel_limit)
elif velocity_error >= 0:
self.acceleration = 0.
elif velocity_error > -10:
self.acceleration -= 0.224
self.acceleration = max(self.acceleration, self.decel_limit)
else:
if self.acceleration > 0: self.acceleration = 0
self.acceleration = -1
acceleration = self.acceleration
# Throttle when acceleration is positive
# Brake when acceleration is negative
if acceleration >= 0:
throttle = acceleration
brake = 0.
else:
# Brake calculations (Brake value is in Torque (N * m))
# (i.e. how much torque would we need to reduce our acceleration by x?)
# Braking Torque = Force * Distance from point of rotation
# * Distance from point of rotation = Wheel radius
# * Force = Mass * target deceleration
# * Mass = Vehicle mass + Fuel mass (assuming full tank)
deceleration = abs(acceleration)
brake = (self.vehicle_mass +
self.fuel_mass) * deceleration * self.wheel_radius
throttle = 0.
steer = self.yaw_controller.get_steering(linear_velocity,
angular_velocity,
current_velocity)
# Apply low pass filters to the throttle and brake values to eliminate jitter
throttle = min(max(self.throttle_filter.filt(throttle), 0),
self.accel_limit)
# Only apply smoothing if we are actually braking
if brake != 0.0:
brake = self.brake_filter.filt(brake)
else:
self.brake_filter.reset()
steer = self.steer_filter.filt(steer)
# rospy.logout('Throttle=%f, Brake=%f, Steer=%f, Acceleration=%f', throttle, brake, steer, self.acceleration)
return throttle, brake, steer
class Controller(object):
"""
Drive-by-Wire Twist controller
"""
def __init__(self, vehicle_mass=1736.35, fuel_capacity=13.5, brake_deadband=.1, decel_limit=-5, accel_limit=1.,
wheel_radius=0.2413, wheel_base=2.8498, steer_ratio=14.8, max_lat_accel=3., max_steer_angle=8.):
"""
Twist controller initialization
"""
# limits
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
# max torque corresponding to 1.0 throttle
self.max_acceleration = 1.5
self.max_acc_torque = self.vehicle_mass * self.max_acceleration * self.wheel_radius
# max brake torque corresponding to deceleration limit
self.max_brake_torque = self.vehicle_mass * abs(self.decel_limit) * self.wheel_radius
# timestamp of the last update
self.last_update = 0
self.max_velocity = 50
self.steer_pid = PID(.4,0,.7, mn=-2.5, mx=2.5)
self.steer_lowpass = LowPassFilter(1e-4)
self.throttle_pid = PID(1.,0.,.5, mn=0, mx=1)
self.throttle_lowpass = LowPassFilter(5e-2)
self.brake_pid = PID(1.,0.,1.,mn=0,mx=1)
self.brake_lowpass = LowPassFilter(5e-2)
# if CTE is too high, this factor will be reduced
self.speed_factor = 1.
def reset(self, time, cte):
"""
reset internal states and timestamp
"""
self.last_update = time
self.steer_pid.reset()
self.steer_lowpass.reset()
self.brake_pid.reset()
self.brake_lowpass.reset()
self.throttle_pid.reset()
self.throttle_lowpass.reset()
def control(self, timestamp, target_velocity, current_velocity, cte):
"""
Control vehicle
:args: timestamp, target v, current v, cte
:return: throttle, brake, steering angle
"""
t_delta = timestamp-self.last_update
if cte>0:
cte = max(0., cte-0.1)
elif cte<0:
cte = min(0., cte+0.1)
# adjust speed if CTE is increasing
if abs(cte) > .75:
self.speed_factor = max(.4, self.speed_factor*.99)
elif abs(cte) < .4:
self.speed_factor = min(1., self.speed_factor/.95)
throttle, brake, steer = 0., 0., 0.
# target_velocity *= self.speed_factor ## @!!!
# slow down dramatically if vehicle too close to lane line
# if abs(cte) > 1.2:
# target_velocity = min(3., target_velocity)
"""
if target_velocity > 0:
max_acceleration = (target_velocity-current_velocity) / t_delta
max_torque = self.vehicle_mass * DEFAULT_GEAR_RATIO * self.wheel_radius * max_acceleration
acceleration_normalization = max_torque / (self.accel_limit * DEFAULT_GEAR_RATIO * DEFAULT_WEIGHT * DEFAULT_WHEEL_RADIUS * THROTTLE_ADJUSTMENT)
if target_velocity < 20:
acceleration_normalization = 1.0
print "!!!!an=", acceleration_normalization
throttle = self.throttle_pid.step(
(target_velocity-current_velocity)/self.max_velocity, t_delta)
throttle *= max(5.,current_velocity)*self.max_velocity / acceleration_normalization
else:
throttle = 0
if (target_velocity == 0 and current_velocity > 0) or (current_velocity * .99 > target_velocity):
max_deceleration = (target_velocity-current_velocity) / t_delta
max_torque = self.vehicle_mass * self.wheel_radius * max_deceleration
deceleration_normalization = max_torque / (self.decel_limit * DEFAULT_WEIGHT * DEFAULT_WHEEL_RADIUS * BRAKE_ADJUSTMENT)
if target_velocity < 20:
deceleration_normalization = 1.0
print "!!!!dn=", deceleration_normalization
brake = self.brake_pid.step(
(current_velocity-target_velocity)/self.max_velocity, t_delta)
brake *= current_velocity*self.max_velocity
brake = min(10, brake / deceleration_normalization)
else:
# if no brake is needed then reset, otherwise low pass
# filter increases latency
self.brake_lowpass.reset()
self.brake_lowpass.filt(0., 0.)
brake = 0
"""
delta_t = 1.0 / 2.0
# convert current velocity from m/s to mph
current_velocity = current_velocity * 3.6 / 1.6093
# make lowest speed 2mph for faster approach towards stop line
if target_velocity > 0.1 and target_velocity < 2.:
target_velocity = 2
delta_v = target_velocity - current_velocity
# approximate and limit acceleration
acceleration = delta_v / delta_t
if acceleration > 0:
acceleration = min(acceleration, self.accel_limit)
else:
acceleration = max(acceleration, self.decel_limit)
if abs(acceleration) > self.brake_deadband:
# avoid deadband
# approximate torque, 1:1 gear ratio
torque = self.wheel_radius * acceleration * self.vehicle_mass
if torque >= 0:
throttle = min(torque / self.max_acc_torque, 1.)
else:
brake = min(self.max_brake_torque, abs(torque))
steer = self.steer_pid.step(cte, t_delta) * self.steer_ratio / DEFAULT_STEER_RATIO # PID normalized to simulator steer ratio
# rospy.loginfo("thr {:.2f} brake {:.2f} steer {:.2f}".format(throttle, brake, steer))
self.last_update = timestamp
# WARNING: low-pass filter on throttle/brake make vehicle move unnaturally at low speeds. Avoid!
# throttle = self.throttle_lowpass.filt(throttle, t_delta)
# brake = self.brake_lowpass.filt(brake, t_delta)*100
# reduce steering magnitude as velocity increases
steer = self.steer_lowpass.filt(steer/(current_velocity*1.+0.01), t_delta)*10
return throttle, brake, steer
class Controller(object):
def __init__(self, default_update_interval, wheel_base, steer_ratio,
min_speed, max_lat_accel, max_steer_angle, max_deceleration,
max_throttle, fuel_capacity, vehicle_mass, wheel_radius,
dyn_velo_proportional_control, dyn_velo_integral_control,
dyn_braking_proportional_control,
dyn_braking_integral_control):
self.current_timestep = None
self.previous_acceleration = 0.
self.max_throttle = max_throttle
self.default_update_interval = default_update_interval
self.velocity_increase_limit_constant = 0.25
self.velocity_decrease_limit_constant = 0.05
self.braking_to_throttle_threshold_ratio = 4. / 3.
self.manual_braking_upper_velocity_limit = 1.4
self.prev_manual_braking_torque = 0
self.manual_braking_torque_to_stop = 700
self.manual_braking_torque_up_rate = 300
self.lpf_tau_throttle = 0.3
self.lpf_tau_brake = 0.3
self.lpf_tau_steering = 0.4
self.manual_braking = False
self.max_braking_torque = (vehicle_mass + fuel_capacity * GAS_DENSITY
) * abs(max_deceleration) * wheel_radius
rospy.logwarn('max_braking_torque = {:.1f} N'.format(
self.max_braking_torque))
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.setup_pid_controllers(dyn_velo_proportional_control,
dyn_velo_integral_control,
dyn_braking_proportional_control,
dyn_braking_integral_control)
self.throttle_lpf = LowPassFilter(self.lpf_tau_throttle,
default_update_interval)
self.brake_lpf = LowPassFilter(self.lpf_tau_brake,
default_update_interval)
self.steering_lpf = LowPassFilter(self.lpf_tau_steering,
default_update_interval)
def setup_pid_controllers(self, velo_p, velo_i, braking_p, braking_i):
rospy.loginfo(
"Initializing PID controllers with velo_P: {}, velo_I: {}, braking_P: {}, braking_I: {}"
.format(velo_p, velo_i, braking_p, braking_i))
# create velocity pid controller thresholded between min and max
# acceleration values
self.velocity_pid_controller = PID(velo_p, velo_i, 0, 0, 1)
# create acceleration pid controller thresholded between 0% and 100%
# for throttle
self.braking_pid_controller = PID(braking_p, braking_i, 0.0, 0.0,
10000)
def control(self, target_linear_velocity, target_angular_velocity,
current_linear_velocity, is_decelerating):
# compute timestep
timestep = self.compute_timestep()
velocity_error = target_linear_velocity - current_linear_velocity
if (target_linear_velocity == 0 and current_linear_velocity == 0):
# reset integrators if we're at a stop
self.reset()
limit_constant = self.velocity_increase_limit_constant if velocity_error > 0 else self.velocity_decrease_limit_constant
error_thresh = limit_constant * current_linear_velocity
throttle_command = 0
brake_command = 0
control_mode = "Coasting"
if is_decelerating and (target_linear_velocity <
self.manual_braking_upper_velocity_limit
and current_linear_velocity <
self.manual_braking_upper_velocity_limit):
# vehicle is coming to a stop or is at a stop; apply fixed braking torque
# continuously, even if the vehicle is stopped
self.manual_braking = True
brake_command = self.prev_manual_braking_torque
# Ramp up manual braking torque
if brake_command < self.manual_braking_torque_to_stop:
brake_command += self.manual_braking_torque_up_rate
# Clip manual brake torque to braking_torque_to_full_stop
brake_command = min(brake_command,
self.manual_braking_torque_to_stop)
self.velocity_pid_controller.reset()
control_mode = "Manual braking"
elif velocity_error < -1 * max(
limit_constant * current_linear_velocity, 0.1):
# use brake if we want to slow down somewhat significantly
self.manual_braking = False
brake_command = self.braking_pid_controller.step(
-velocity_error, timestep) if velocity_error < (
-1 * limit_constant *
self.braking_to_throttle_threshold_ratio *
current_linear_velocity) or (
velocity_error < 0
and current_linear_velocity < 2.5) else 0
self.velocity_pid_controller.reset()
control_mode = "PID braking"
elif not is_decelerating or (
current_linear_velocity > 5 and
velocity_error > -1 * limit_constant * current_linear_velocity
) or (current_linear_velocity < 5
and velocity_error > limit_constant * current_linear_velocity):
# use throttle if we want to speed up or if we want to slow down
# just slightly
# reset brake lpf to release manually held brake quickly
if self.manual_braking:
self.brake_lpf.reset()
self.manual_braking = False
throttle_command = self.velocity_pid_controller.step(
velocity_error, timestep)
self.braking_pid_controller.reset()
control_mode = "PID throttle"
# apply low pass filter and maximum limit on brake command
filtered_brake = min(self.max_braking_torque,
self.brake_lpf.filt(brake_command))
# do not apply throttle if any brake is applied
if filtered_brake < 50:
# brake is released, ok to apply throttle
filtered_brake = 0
else:
# brake is still applied, don't apply throttle
throttle_command = 0
self.velocity_pid_controller.reset()
# apply low pass filter and maximum limit on throttle command
filtered_throttle = min(self.max_throttle,
self.throttle_lpf.filt(throttle_command))
# Store final brake torque command for next manual braking torque calc
self.prev_manual_braking_torque = filtered_brake
rospy.loginfo(
'%s: current linear velocity %.2f, target linear velocity %.2f, is_decelerating %s, throttle_command %.2f, brake_command %.2f, error %.2f, thresh %.2f',
control_mode, current_linear_velocity, target_linear_velocity,
is_decelerating, filtered_throttle, filtered_brake, velocity_error,
error_thresh)
# Return throttle, brake, steer
return (filtered_throttle, filtered_brake,
self.steering_lpf.filt(
self.yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_linear_velocity)))
def reset(self):
self.last_timestep = None
self.velocity_pid_controller.reset()
self.braking_pid_controller.reset()
def compute_timestep(self):
last_timestep = self.current_timestep
self.current_timestep = time.time()
if last_timestep == None:
last_timestep = self.current_timestep - self.default_update_interval
return self.current_timestep - last_timestep
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.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_base
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(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.1
mn = 0 # Minimum throttle value
mx = 0.5 # Maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5
ts = 0.05
self.vel_lpf = LowPassFilter(tau, ts)
self.yaw_lpf = LowPassFilter(tau, ts)
self.last_vel = 0
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()
self.vel_lpf.reset()
self.yaw_lpf.reset()
return 0., 0., 0.
# Apply low-pass filter
current_vel = self.vel_lpf.filt(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
steering = self.yaw_lpf.filt(steering)
velocity_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(velocity_error, sample_time)
brake = 0
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 700 # To prevent Carla from moving requires about 700 Nm of torque.
elif throttle < 0.1 and velocity_error < 0:
throttle = 0
decel = max(velocity_error, self.decel_limit)
brake = abs(
decel
) * self.vehicle_mass * self.wheel_radius # Torque Force * radius = (a * m) * r
# Return throttle, brake, steer
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, steer_ratio, max_speed, min_speed,
accel_limit, decel_limit, max_lat_accel, max_steer_angle,
brake_deadband):
# TODO: Implement
self.max_speed = max_speed
self.accel_limit = accel_limit
self.decel_limit = decel_limit
self.max_steer_angle = max_steer_angle
self.brake_deadband = brake_deadband
# Use respective PID for brake & throttle, since they give different acceleration
self.throttle_pid = PID(4.0, 0.5, 0.0)
self.brake_pid = PID(440.0, 0.0, 0.0)
self.yaw_control = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.filter = LowPassFilter(0.2, 0.1)
self.last_timestamp = None
'''
Params:
target_v - desired linear velocity
target_w - desired angular velocity
current_v - current linear velocity
dbw_enabled - drive by wire enabled (ignore error in this case)
'''
def control(self, target_v, target_w, current_v, dbw_enabled):
#if current_v.x < 0.2:
# rospy.loginfo("target_v %s, current_v %s", target_v.x, current_v.x)
# sys.stdout.flush()
# TODO: Change the arg, kwarg list to suit your needs. Return throttle, brake, steer
if self.last_timestamp is None or not dbw_enabled:
self.last_timestamp = rospy.get_time()
# Reset throttle PID, steer filter
self.throttle_pid.reset()
self.brake_pid.reset()
self.filter.reset()
return 0., 0., 0.
# Car is stopped or approaching stopped
if not abs(target_v.x - self.max_speed) < 1e-5 and current_v.x < 10:
self.last_timestamp = rospy.get_time()
# Reset throttle PID, steer filter
self.throttle_pid.reset()
self.brake_pid.reset()
self.filter.reset()
return 0, 10000, 0
error = min(target_v.x, self.max_speed) - current_v.x
dt = rospy.get_time() - self.last_timestamp
# braking
if error < 0:
brake = self.brake_pid.step(-1.0 * error, dt)
brake = max(1, brake)
#rospy.loginfo("Controller: current_v_v %s, error %s, brake %s", current_v.x, error, brake)
#sys.stdout.flush()
# Reset throttle PID
self.throttle_pid.reset()
throttle = 0.0
else:
# PID
throttle = self.throttle_pid.step(error, dt)
throttle = min(max(throttle, self.decel_limit), self.accel_limit)
# Reset brake PID
self.brake_pid.reset()
brake = 0.0
# when brake , no steering
if error < 0: #not abs(target_v.x - self.max_speed) < 1e-5 :
steering = 0.
self.filter.reset()
else:
steering = self.yaw_control.get_steering(target_v.x, target_w.z,
current_v.x)
steering = self.filter.filt(steering)
steering = min(max(steering, -1.0 * self.max_steer_angle),
self.max_steer_angle)
self.last_timestamp = rospy.get_time()
#rospy.loginfo("Controller: target_v %s, error %s, brake %s", target_v.x, error, brake)
#sys.stdout.flush()
return throttle, brake, steering
class Controller(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, accel_limit, decel_limit, loop_frequency,
vehicle_mass, wheel_radius):
# TODO: Implement
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_steer_angle = max_steer_angle
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.steering_controller = YawController(wheel_base, steer_ratio,
min_speed, max_lat_accel,
max_steer_angle)
self.throttle_controller = PID(0.15,
0.0,
0.09,
mn=decel_limit,
mx=accel_limit)
self.low_pass_filter = LowPassFilter(12.0, 1)
self.last_timestamp = None
def control(self, target_angular_velocity, target_linear_velocity,
current_angular_velocity, current_linear_velocity,
dbw_enabled):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.reset()
return 0., 0., 0.
steer = self.steering_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_linear_velocity)
throttle = 0.
brake = 0.
current_timestamp = rospy.Time.now()
if self.last_timestamp != None:
dt = (current_timestamp - self.last_timestamp).nsecs / 1e9
cte = target_linear_velocity - current_linear_velocity
acceleration = self.throttle_controller.step(cte, dt)
filtvalue = self.low_pass_filter.filt(acceleration)
if self.low_pass_filter.ready:
acceleration = self.low_pass_filter.get()
if acceleration > 0:
throttle = acceleration
else:
brake = self.vehicle_mass * abs(
acceleration) * self.wheel_radius
self.last_timestamp = current_timestamp
rospy.loginfo(
'SENDING - [throttle,brake,steer]:[{:.4f},{:.4f},{:.4f}], [cA,cL]:[{:.4f},{:.4f}]m [tA, tL]:[{:.4f},{:.4f}]'
.format(throttle, brake, steer, current_angular_velocity,
current_linear_velocity, target_angular_velocity,
target_linear_velocity))
return throttle, brake, steer
def reset(self):
"""
Reset the controller's state.
"""
self.throttle_controller.reset()
self.low_pass_filter.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):
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
# global debug counter
self.debug_counter = 0
# yaw controller
self.yaw_controller = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
# throttle controller
self.throttle_controller = PID(kp, ki, kd, mn, mx)
self.vel_lpf = LowPassFilter(tau, ts)
self.last_time = rospy.get_time()
"""
:@ linear_vel : target linear velocity
:@ angular_vel: target angular velocity
"""
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel, cte):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
if not dbw_enabled:
self.throttle_controller.reset()
self.vel_lpf.reset()
return 0., 0., 0.
# Begin to calculate Throttle Value
current_vel = self.vel_lpf.filt(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 control
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0.
# steering control
angular_vel = (
1.5) * angular_vel # (*) adjust (decrease) radius of pure pursuit
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
steering = max(-self.max_steer_angle,
min(self.max_steer_angle, steering))
# If target linear velocity = 0, then go very slow
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 throttle is really small and velocity error < 0 (i.e. we're going faster than we want to be)
elif throttle < 0.1 and vel_error < 0.:
throttle = 0.
decel = max(vel_error, self.decel_limit) # a negative number
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius # Torque N*m
#rospy.logwarn( "########### debug_count: {0} ###############".format(self.debug_counter) )
#rospy.logwarn( "steering: {0}".format(steering) )
#rospy.logwarn( "throttle: \t {0}".format(throttle) )
#rospy.logwarn( "\n")
self.debug_counter = self.debug_counter + 1
return throttle, brake, steering
class TwistController(object):
def __init__(self):
self.linear_velocity_pid = PID(kp=ACCEL_SENSITIVITY * 1.25,
ki=0.003,
kd=0.0,
mn=LINEAR_PID_MIN,
mx=LINEAR_PID_MAX)
self.low_pass_filter = LowPassFilter(
8.0, 2.0) #(b,a) normalized, (0 , 1) = all pass
self.yaw_controller = None
pass
def control(self, current_linear_velocity, target_linear_velocity,
steer_sensitivity, target_angular_velocity):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
#if target_angular_velocity > 0.025:
# rospy.logerr('')
# rospy.logerr('target_linear_velocity.org {: f}'.format(target_linear_velocity))
# v = self.yaw_controller.get_max_linear_velocity(current_linear_velocity, target_angular_velocity)
# rospy.logerr('max_linear_velocity(tangent) {: f}'.format(v))
# target_linear_velocity = min ( v, target_linear_velocity)
if target_angular_velocity > 0.025 and target_linear_velocity > 0.1:
target_linear_velocity = max(
target_linear_velocity / SHARP_TURN_FACTOR, 0.1)
sample_time = SAMPLE_TIME
brake = 0.0
if target_linear_velocity > 0.1:
cte_linear = target_linear_velocity - current_linear_velocity
throttle = self.linear_velocity_pid.step(cte_linear, sample_time)
throttle = self.low_pass_filter.filt(throttle)
if throttle <= 0.0:
brake = throttle * -1 * 10000
throttle = 0.0
else:
self.linear_velocity_pid.reset()
self.low_pass_filter.reset()
throttle = 0.0
brake = MAX_BRAKE_VALUE
brake = min(brake, MAX_BRAKE_VALUE)
#rospy.logerr('target {: f}, current {: f}, throttle {: f}, brake: {: f}'.format(target_linear_velocity, current_linear_velocity, throttle, brake))
#rospy.loginfo('target {: f}, current {: f}, throttle {: f}, brake: {: f}'.format(target_linear_velocity, current_linear_velocity, throttle, brake))
# normalized steering : -1/+1
# normalized steering = steer_angle * 2 / max_steer_angle
# steer_angle = wheel_angle * steer_ratio
# normalized steering = wheel_angle * { steer_ratio * 2 / max_steer_angle }
# steer_sensitivity = steer_ratio * 2 / max_steer_angle
#
# normalized steerting = wheel_angle * steer_sensitivity
steer = self.yaw_controller.get_steering(target_linear_velocity,
target_angular_velocity,
current_linear_velocity)
#rospy.logerr("======================= steer angle: %s", steer)
return throttle, brake, steer
