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