class LongController(object):
def __init__(self, vehicle_mass, brake_deadband, decel_limit, accel_limit,
wheel_radius):
self.n_wheel_accel = 2 # 2 wheel drive
self.n_wheel_deccel = 4 # 4 wheel brake
self.vehicle_mass = vehicle_mass
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.max_force = vehicle_mass * accel_limit # max throttle force
self.min_force = vehicle_mass * decel_limit # max brake force (negative!)
self.force_brake_deadband = -1.0 * vehicle_mass * brake_deadband
# This is from the simulator!!!- perhaps not valid for real vehicle
#
# throttle at 0.25 pedel position
# v_start: 3.12 km/h v_end: 20.50 km/h --> delta_v = 4.83 m/s
# t_start: 50.85 s t_end: 62.15 s -> delta_t = 11.3 s
# --> accel: 0.43 m/s^2
#
# throttle at 0.50 pedel position
# v_start: 3.72 km/h v_end: 21.00 km/h --> delta_v = 4.80 m/s
# t_start: 696.17 s t_end: 698.87 s -> delta_t = 2.7 s
# --> accel: 1.78 m/s^2
#
# throttle at 0.40 pedel position
# v_start: 3.24 km/h v_end: 20.00 km/h --> delta_v = 4.66 m/s
# t_start: 94.71 s t_end: 100.61 s -> delta_t = 2.7 s
# --> accel: 0.79 m/s^2
# acceleration is not linear
# guess for vehicle
# throttle 1.0 --> accel 3 m/s^2
self.max_accel = 3.0 #
self.ref_force_throttle = vehicle_mass * self.max_accel # force at 100% throttle position
# init low pass filter for acceleration
self.accel_LowPassFilter = LowPassFilter(FILT_TAU_ACCEL, TS)
self.last_spd = 0.0
self.last_target_accel = 0.0
self.last_expected_spd = 0.0
self.last_brake_actv = False # brake active
self.last_force = 0.0
# init PID
self.accel_PID = PID(kp=2.0 * vehicle_mass * wheel_radius,
ki=0.015 * vehicle_mass * wheel_radius,
kd=0.2 * vehicle_mass * wheel_radius)
def control(self, target_spd, current_spd, delta_t):
# calulate filter acceleration of vehicle
accel_raw = (current_spd - self.last_spd) / delta_t
accel_filt = self.accel_LowPassFilter.filt(accel_raw)
# calculate speed error
spd_err = target_spd - current_spd
# calulate target acceleration from speed error
# could be tuned
if target_spd > 0.05:
k_accel = 0.2
else:
k_accel = 1.0
if (abs(spd_err) < 1.0):
target_accel = k_accel * spd_err
else:
target_accel = k_accel * spd_err * spd_err * spd_err
# limit jerk
accel_change_limit = LONG_JERK_LIMIT * delta_t
target_accel = max(
min(target_accel, self.last_target_accel + accel_change_limit),
self.last_target_accel - accel_change_limit)
# check for min and max allowed acceleration
target_accel = max(min(target_accel, self.accel_limit),
self.decel_limit)
trq = 0.0
force_feedForward = 0.0
force_PID = 0.0
# calculate an expected speed from last target values
# this can be used in PID to large errors, when target_spd jumps
# it's smooth and near the expected behavior
expected_spd = self.last_expected_spd + target_accel * delta_t
expected_spd = max(min(target_spd, expected_spd), 0.0)
if target_spd > expected_spd:
expected_spd = max(current_spd, expected_spd,
self.last_expected_spd + 0.1 * delta_t)
elif target_spd < expected_spd:
expected_spd = min(current_spd, expected_spd,
self.last_expected_spd - 0.1 * delta_t)
# assert valid range
expected_spd = max(min(target_spd, expected_spd), 0.0)
if target_spd > 0.05 or current_spd > 1.0:
# overall resistance (tuned in simulator)
force_resistance = 4 * current_spd * current_spd + 40 * current_spd + 40
# calculate force from target_accel using mass
# F = m * a
force_feedForward = target_accel * self.vehicle_mass + force_resistance
# use PID to get better control performance
expected_err = expected_spd - current_spd
force_PID = self.accel_PID.step(expected_err,
delta_t,
mn=MIN_NUM,
mx=MAX_NUM)
# calulate overall torque
force = force_feedForward + force_PID
else: # hold vehicle
force = min(self.last_force, self.min_force / 2)
self.accel_PID.freeze()
# calulate throttle
# published throttle is a percentage [0 ... 1]!!!
if force > 0.:
throttle = force / self.ref_force_throttle * 1.0
brake = 0.0
self.last_brake_actv = False
elif self.last_brake_actv:
throttle = 0.0
brake = -force * self.wheel_radius / self.n_wheel_accel
self.last_brake_actv = True
elif force < self.force_brake_deadband or (force < 0.
and target_spd < 1.0):
throttle = 0.0
brake = -force * self.wheel_radius / self.n_wheel_accel
self.last_brake_actv = True
else:
throttle = 0.0
brake = 0.0
self.last_brake_actv = False
# output for debug
#rospy.logwarn("accel_raw: %f" % accel_raw + "; accel_filt: %f" % accel_filt + "; target_accel: %f" % target_accel + "; current_spd: %f" % current_spd)
#rospy.logwarn("target_spd: {0:.2f} km/h, current_spd: {1:.2f} km/h, expected_spd: {2:.2f} km/h, target_accel: {3:.2f} m/ss, accel: {4:.2f} m/ss, throttle: {5:.2f}, brake: {6:.2f} Nm, force_PID: {7:.2f} N".format(target_spd * 3.6, current_spd * 3.6, expected_spd*3.6, target_accel, accel_filt, throttle, brake, force_PID))
# write values for next iteration
self.last_spd = current_spd
self.last_target_accel = target_accel
self.last_expected_spd = expected_spd
self.last_force = force
return throttle, brake
def reset(self, current_spd):
self.accel_PID.reset()
self.last_spd = current_spd
self.last_target_accel = 0.0
self.last_expected_spd = current_spd
self.last_force = 0.0
class Controller(object):
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
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.min_steer_angle = -max_steer_angle
self.max_steer_angle = max_steer_angle
self.min_force = vehicle_mass * accel_limit # max brake force
self.force_brake_deadband = -1.0 * vehicle_mass * brake_deadband
self.vehicle_accel_sensitivity = vehicle_mass * wheel_radius
# force at 100% throttle position
self.ref_force_throttle = vehicle_mass * 3.0
# store all the previsous steer, speed and acceleration values
self.last_speed = 0.0
self.last_target_accel = 0.0
self.last_expected_speed = 0.0
self.last_force = 0.0
self.last_steer = 0.0
# brakes are not applied initially
self.last_brake_applied = False
# init feed forward yaw-rate control
self.yawControl = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
# initialize acceleration and steering PID
self.accel_PID = PID(kp=2.0 * self.vehicle_accel_sensitivity,
ki=0.015 * self.vehicle_accel_sensitivity,
kd=0.2 * self.vehicle_accel_sensitivity)
self.steer_PID = PID(kp=0.05, ki=0.0005, kd=0.5)
def control(self, waypoints, pose, current_speed, target_speed, target_yaw,
delta_t):
throttle, brake = self.control_speed(current_speed, target_speed,
delta_t)
steer = self.control_steer(waypoints, pose, current_speed,
target_speed, target_yaw, delta_t)
return throttle, brake, steer
def control_speed(self, current_speed, target_speed, delta_t):
if target_speed > 0.05 or current_speed > 1.0:
# overall resistance (tuned in simulator)
force_resistance = 4 * current_speed**2 + 40 * current_speed + 40
# use PID to get better control performance
CTE = self.speed_CTE(current_speed, target_speed, delta_t)
force_PID = self.accel_PID.step(CTE, delta_t)
# calculate force from target_accel using mass: F = m * a
force_feedForward = self.last_target_accel * self.vehicle_mass + force_resistance
# calulate overall torque
force = force_feedForward + force_PID
else:
# apply minimum force if vehicle is near the target speed
force = min(self.last_force, self.min_force / 2)
self.accel_PID.freeze()
# Initialiaze the throttle, brake and last_brake_applied
throttle = 0.0
brake = 0.0
self.last_brake_applied = False
# calulate throttle
# published throttle is a percentage [0 ... 1]!!!
if force > 0.0:
throttle = force / self.ref_force_throttle
elif force < self.force_brake_deadband or target_speed < 1.0:
brake = -force * self.wheel_radius / NUM_WHEEL_ACCELERATION
self.last_brake_applied = True
# store last_speed and last_force for next iteration
self.last_speed = current_speed
self.last_force = force
return throttle, brake
def speed_CTE(self, current_speed, target_speed, delta_t):
# calculate speed error
speed_error = target_speed - current_speed
# calulate target acceleration from speed error
target_accel = 0.3 if target_speed > 0.05 else 1.0
if abs(speed_error) < 1.0:
target_accel *= speed_error
else:
target_accel *= speed_error**3
# check for min and max allowed acceleration
target_accel = max(min(target_accel, self.accel_limit),
self.decel_limit)
self.last_target_accel = target_accel
# calculate CTE and return it
return speed_error
def control_steer(self, waypoints, pose, current_speed, target_speed,
target_yaw, delta_t):
if current_speed < MIN_SPEED:
self.steer_PID.freeze()
return self.last_steer
# feed forward control to drive curvature of road
steer_feedForward = self.yawControl.get_steering(
target_speed, target_yaw, current_speed)
# limit steering angle
steer_feedForward = max(min(steer_feedForward, self.max_steer_angle),
self.min_steer_angle)
# PID control
CTE = self.steer_CTE(waypoints, pose)
steer_PID = self.steer_PID.step(CTE, delta_t)
# steering command
steer = steer_feedForward + steer_PID
self.last_steer = steer
return steer
def steer_CTE(self, waypoints, pose):
# transfrom waypoints into vehicle coordinates
car_theta = 2 * np.arccos(pose.orientation.w)
# Constraining the angle in [-pi, pi)
if car_theta > np.pi:
car_theta = -(2 * np.pi - car_theta)
num_waypoints = len(waypoints)
car_x = pose.position.x
car_y = pose.position.y
# transform waypoints in vehicle coordiantes
wp_car_coords_x = np.zeros(num_waypoints)
wp_car_coords_y = np.zeros(num_waypoints)
for idx, waypoint in enumerate(waypoints):
wp_x = waypoint.pose.pose.position.x
wp_y = waypoint.pose.pose.position.y
wp_car_coords_x[idx] = (wp_y - car_y) * math.sin(car_theta) - (
car_x - wp_x) * math.cos(car_theta)
wp_car_coords_y[idx] = (wp_y - car_y) * math.cos(car_theta) - (
wp_x - car_x) * math.sin(car_theta)
# get waypoint which should be used for controller input#
# get the first waypoint in front of car. The waypoints are already in order as
# they are coming out of /waypoint_updater topic
viewRange = 10.0
first_wp_idx = -1
last_wp_idx = -1
first_wp_found = False
for idx, wp_car_coord_x in enumerate(wp_car_coords_x):
if not first_wp_found and wp_car_coord_x >= 0.0:
first_wp_idx = idx
first_wp_found = True
elif wp_car_coord_x >= viewRange:
last_wp_idx = idx
break
# Check if we have enough waypoints to move forward We need over 5 waypoints
if first_wp_idx < 0 or last_wp_idx - first_wp_idx < 4:
rospy.logerr("twist_controller: Invalid Waypoints")
return 0.0
# Interpolate waypoints (already transformed to vehicle coordinates) to a polynomial of 3rd degree
coeffs = np.polyfit(wp_car_coords_x[first_wp_idx:last_wp_idx + 1],
wp_car_coords_y[first_wp_idx:last_wp_idx + 1], 3)
# distance to track is polynomial at car's position x = 0
return np.poly1d(coeffs)(0.0)
def reset(self, current_speed):
self.steer_PID.reset()
self.accel_PID.reset()
self.last_speed = current_speed
self.last_target_accel = 0.0
self.last_expected_speed = current_speed
self.last_force = 0.0
class LatController(object):
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle):
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.min_speed = min_speed
self.max_lat_accel = max_lat_accel
self.min_angle = -max_steer_angle
self.max_angle = max_steer_angle
# init feed forward yaw-rate control
self.yawControl = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
# init PID
self.steer_PID = PID(kp=0.05, ki=0.0005, kd=0.5)
self.last_steer = 0.0
def control(self, target_spd, target_yawRate, current_spd, waypoints, pose,
delta_t):
steer = self.last_steer
if current_spd > self.min_speed:
# feed forward control to drive curvature of road
steer_feedForward = self.yawControl.get_steering(
target_spd, target_yawRate, current_spd)
# limit steering angle
steer_feedForward = max(min(steer_feedForward, self.max_angle),
self.min_angle)
# PID control
CTE = self.calc_CTE(waypoints, pose)
steer_PID = self.steer_PID.step(
CTE,
delta_t,
mn=self.min_angle - steer_feedForward,
mx=self.max_angle - steer_feedForward)
# steering command
steer = steer_feedForward + steer_PID
self.last_steer = steer
else:
self.steer_PID.freeze()
return steer
def reset(self):
self.steer_PID.reset()
def transfromWPcarCoord(self, waypoints, pose):
n = len(waypoints)
# get car's x and y position and heading angle
car_x = pose.position.x
car_y = pose.position.y
# get orientation
s = pose.orientation.w # quaternion scalar
v1 = pose.orientation.x # vector part 1
v2 = pose.orientation.y # vector part 2
v3 = pose.orientation.z # vector part 3
# now obtaining orientation of the car (assuming rotation about z: [0;0;1])
car_theta = 2 * np.arccos(s)
# Constraining the angle in [-pi, pi)
if car_theta > np.pi:
car_theta = -(2 * np.pi - car_theta)
#car_theta = pose.orientation.z
# transform waypoints in vehicle coordiantes
wp_carCoord_x = np.zeros(n)
wp_carCoord_y = np.zeros(n)
for i in range(n):
wp_x = waypoints[i].pose.pose.position.x
wp_y = waypoints[i].pose.pose.position.y
wp_carCoord_x[i] = (wp_y - car_y) * math.sin(car_theta) - (
car_x - wp_x) * math.cos(car_theta)
wp_carCoord_y[i] = (wp_y - car_y) * math.cos(car_theta) - (
wp_x - car_x) * math.sin(car_theta)
return wp_carCoord_x, wp_carCoord_y
def calc_CTE(self, waypoints, pose):
# transfrom waypoints into vehicle coordinates
wp_carCoord_x, wp_carCoord_y = self.transfromWPcarCoord(
waypoints, pose)
# get waypoint which should be used for controller input
viewRange = 10.0
n_points_min = 5
idxFirstWP, idxLastWP = self.findWPs(wp_carCoord_x, viewRange,
n_points_min)
# do some checks if enough waypoints in the front of the car are available
if (idxFirstWP < 0):
rospy.logerr(
"dbw_node: No waypoint in front of car for lateral control received!"
)
return 0.0
elif (idxLastWP < 0):
rospy.logerr(
"dbw_node: Not enough waypoints for a view range of %f m!" %
viewRange)
return 0.0
elif (idxLastWP + 1 - idxFirstWP < n_points_min):
rospy.logerr("dbw_node: Resolution of waypoints not high enough!")
return 0.0
else:
# Interpolate waypoints (already transformed to vehicle coordinates) to a polynomial of 3rd degree
coeffs = np.polyfit(wp_carCoord_x[idxFirstWP:idxLastWP + 1],
wp_carCoord_y[idxFirstWP:idxLastWP + 1], 3)
p = np.poly1d(coeffs)
# distance to track is polynomial at car's position x = 0
CTE = p(0.0)
return CTE
def findWPs(self, wp_carCoord_x, viewRange, n_points_min):
# this function return first waypoint in front of car
# it is asumed that wayponts are already ordered (by waypoint_updater)
# if return value is negative then no point in front of car is found!
# transfrom waypoint in vehcile coordnates
idxFirstWP = -1
idxLastWP = -1
n_pts = len(wp_carCoord_x)
flagFirstWP = False
for i in range(n_pts):
if not flagFirstWP and wp_carCoord_x[i] >= 0.0:
idxFirstWP = i
flagFirstWP = True
elif wp_carCoord_x[i] >= viewRange:
idxLastWP = i
break
# if view range to less than check if enough points are available
#rospy.logwarn("idxFirstWP: %f" % idxFirstWP + "idxLastWP: %f" % idxLastWP )
#if idxFirstWP>=0 and idxLastWP<0 and n_pts-idxFirstWP >= n_points_min:
# idxLastWP = n_pts-1 # zero based indexing
return idxFirstWP, idxLastWP
