Skip to content

Commit

Permalink
improved exit filter AND added rate-based feedforward
Browse files Browse the repository at this point in the history
  • Loading branch information
@Gernby
Gernby committed Apr 3, 2019
1 parent 4c445d7 commit df2f4f2
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 32 deletions.
39 changes: 23 additions & 16 deletions dashboard.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
except:
pass

def dashboard_thread(rate=200):
def dashboard_thread(rate=300):
set_realtime_priority(5)

USER = ''
Expand All @@ -42,7 +42,7 @@ def dashboard_thread(rate=200):
vEgo = 0.0
pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller)
#pathPlan = None
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=False, poller=poller)
#liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller)
liveParameters = None
#live20 = messaging.sub_sock(context, service_list['live20'].port, addr=ipaddress, conflate=True, poller=poller)
Expand Down Expand Up @@ -217,8 +217,11 @@ def dashboard_thread(rate=200):
p_poly = model_polyfit(md.path.points, _path_pinv)
l_poly = model_polyfit(md.leftLane.points, _path_pinv)
r_poly = model_polyfit(md.rightLane.points, _path_pinv)
p_curv = calc_poly_curvature(p_poly)
l_curv = calc_poly_curvature(l_poly)
r_curv = calc_poly_curvature(r_poly)

far_pinv = [_path_pinv[0][25:50],_path_pinv[1][25:50],_path_pinv[2][25:50],_path_pinv[3][25:50]]
'''far_pinv = [_path_pinv[0][25:50],_path_pinv[1][25:50],_path_pinv[2][25:50],_path_pinv[3][25:50]]
near_pinv = [_path_pinv[0][0:30],_path_pinv[1][0:30],_path_pinv[2][0:30],_path_pinv[3][0:30]]
p_poly_far = model_polyfit(map(float, md.path.points)[25:50], far_pinv) # predicted path
Expand All @@ -229,29 +232,29 @@ def dashboard_thread(rate=200):
l_poly_near = model_polyfit(map(float, md.leftLane.points)[0:30], near_pinv) # left line
r_poly_near = model_polyfit(map(float, md.rightLane.points)[0:30], near_pinv) # right line
p_curv = calc_poly_curvature(p_poly)
l_curv = calc_poly_curvature(l_poly)
r_curv = calc_poly_curvature(r_poly)
p_curv1 = calc_poly_curvature(p_poly_far)
l_curv1 = calc_poly_curvature(l_poly_far)
r_curv1 = calc_poly_curvature(r_poly_far)
p_curv2 = calc_poly_curvature(p_poly_near)
l_curv2 = calc_poly_curvature(l_poly_near)
r_curv2 = calc_poly_curvature(r_poly_near)
'''
#left_curv = calc_poly_curvature([lp[0], lp[9], lp[19], lp[29], lp[39],lp[49]])
#path_curv = calc_poly_curvature([pp[0], pp[9], pp[19], pp[29], pp[39],pp[49]])
#path_curv = calc_poly_curvature(md.path.points)
#right_curv = calc_poly_curvature([rp[0], rp[9], rp[19], rp[29], rp[39],rp[49]]) #calc_poly_curvature(md.rightLane.points)
#print(left_curv, path_curv, right_curv)
for i in range(1,50):
for i in range(5,50,5):
influxLineString += (",l%d=%1.3f,p%d=%1.3f,r%d=%1.3f" % (i, md.leftLane.points[i], i, md.path.points[i], i, md.rightLane.points[i]))
#influxLineString += (",left_curv=%1.1f %s\n" % (left_curv, receiveTime))
influxLineString += (",vEgo=%1.1f,vCurv=%1.5f,lstd=%1.2f,rstd=%1.2f,pstd=%1.2f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,pProb=%1.2f,rProb=%1.2f,p_curv1=%1.5f,l_curv1=%1.5f,r_curv1=%1.5f,p_curv2=%1.5f,l_curv2=%1.5f,r_curv2=%1.5f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \
(vEgo, v_curv, md.leftLane.std,md.rightLane.std,md.path.std,l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.path.prob, md.rightLane.prob,p_curv1,l_curv1,r_curv1,p_curv2,l_curv2,r_curv2, v_curv, p_curv,l_curv, r_curv, receiveTime))
#influxLineString += (",vEgo=%1.1f,vCurv=%1.5f,lstd=%1.2f,rstd=%1.2f,pstd=%1.2f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,pProb=%1.2f,rProb=%1.2f,p_curv1=%1.5f,l_curv1=%1.5f,r_curv1=%1.5f,p_curv2=%1.5f,l_curv2=%1.5f,r_curv2=%1.5f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \
# (vEgo, v_curv, md.leftLane.std,md.rightLane.std,md.path.std,l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.path.prob, md.rightLane.prob,p_curv1,l_curv1,r_curv1,p_curv2,l_curv2,r_curv2, v_curv, p_curv,l_curv, r_curv, receiveTime))
influxLineString += (",vEgo=%1.1f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,rProb=%1.2f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \
(vEgo, l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.rightLane.prob,v_curv, p_curv,l_curv, r_curv, receiveTime))

frame_count += 1

'''model = (
'''model = (
frameId = 19786,
path = (
points = [-0.002040863, 0.0048789978, 0.0024032593, -0.029251099, -0.050567627, -0.071716309, -0.10424805, -0.14196777, -0.18005371, -0.20825195, -0.2277832, -0.26391602, -0.31420898, -0.38085938, -0.43212891, -0.47900391, -0.51318359, -0.56494141, -0.62646484, -0.68212891, -0.73632812, -0.74951172, -0.82519531, -0.89648438, -0.97265625, -1.0615234, -1.1464844, -1.2412109, -1.3369141, -1.4462891, -1.5488281, -1.6445312, -1.7460938, -1.8544922, -1.9658203, -2.0820312, -2.2089844, -2.3320312, -2.484375, -2.6152344, -2.7265625, -2.8554688, -2.984375, -3.1425781, -3.2636719, -3.4160156, -3.5566406, -3.6835938, -3.8222656, -3.9746094],
Expand Down Expand Up @@ -293,11 +296,15 @@ def dashboard_thread(rate=200):
if sample_str != "":
sample_str += ","
a = _pathPlan.pathPlan.mpcAngles

sample_str = ("lane_width=%1.2f,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" %
(_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3],
_pathPlan.pathPlan.cProb, _pathPlan.pathPlan.lProb, _pathPlan.pathPlan.rProb, a[1], a[2], a[3],
a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18]))
p = _pathPlan.pathPlan

#sample_str = ("lane_width=%1.2f,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" %
# (_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3],
# _pathPlan.pathPlan.cProb, _pathPlan.pathPlan.lProb, _pathPlan.pathPlan.rProb, a[1], a[2], a[3],
# a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18]))
sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f" %
(p.laneWidth, p.lPoly[0], p.rPoly[0], p.cPoly[0], p.dPoly[0], p.lPoly[1], p.rPoly[1], p.cPoly[1], p.dPoly[1], p.lPoly[2], p.rPoly[2], p.cPoly[2], p.dPoly[2],p.lPoly[3], p.rPoly[3], p.cPoly[3], p.dPoly[3],
p.cProb, p.lProb, p.rProb, a[1], a[2], a[3]))
influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime)
sample_str = ""
frame_count += 1
Expand Down Expand Up @@ -518,7 +525,7 @@ def dashboard_thread(rate=200):
# print(identifier)
# pass

def main(rate=100):
def main(rate=300):
dashboard_thread(rate)

if __name__ == "__main__":
Expand Down
5 changes: 4 additions & 1 deletion selfdrive/car/honda/carstate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,9 @@ def get_can_signals(CP):
("STANDSTILL", 50)]

if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1),
("LEAD_DISTANCE", "RADAR_HUD", 0)]
checks += [("RADAR_HUD", 50)]
else:
signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1),
("DOOR_OPEN_FR", "DOORS_STATUS", 1),
Expand Down Expand Up @@ -196,6 +198,7 @@ def update(self, cp, cp_cam):
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
self.lead_distance= cp.vl["RADAR_HUD"]['LEAD_DISTANCE']
else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
Expand Down
36 changes: 29 additions & 7 deletions selfdrive/controls/lib/latcontrol.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,15 @@
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)

def apply_deadzone(angle, deadzone):
if angle > deadzone:
angle -= deadzone
elif angle < - deadzone:
angle += deadzone
else:
angle = 0.
return angle


class LatControl(object):
def __init__(self, CP):
Expand Down Expand Up @@ -50,14 +59,16 @@ def __init__(self, CP):
self.total_actual_projection = max(0.0, CP.steerReactTime + CP.steerDampTime)
self.actual_smoothing = max(1.0, CP.steerDampTime / _DT)
self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT)
self.dampened_angle_steers = 0.0
self.ff_angle_factor = 0.5
self.ff_rate_factor = 1.0
self.dampened_desired_angle = 0.0
self.steer_counter = 1.0
self.steer_counter_prev = 0.0
self.rough_steers_rate = 0.0
self.prev_angle_steers = 0.0
self.calculate_rate = True
self.lane_prob_reset = False
self.feed_forward = 0.0

KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
Expand Down Expand Up @@ -123,14 +134,16 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM

else:
projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
self.dampened_desired_angle = (((self.desired_smoothing - 1.) * self.dampened_desired_angle) + projected_desired_angle) / self.desired_smoothing
self.dampened_desired_angle += ((projected_desired_angle - self.dampened_desired_angle) / self.desired_smoothing)

projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
if not steer_override:
self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing
self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing)

if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
if path_plan.laneWidth > 3.7: self.dampened_desired_angle = path_plan.angleSteers
if path_plan.lPoly[3] - path_plan.rPoly[3] > 3.9:
print(self.dampened_desired_angle, path_plan.angleSteers)
self.dampened_desired_angle = path_plan.angleSteers
self.lane_prob_reset = True
elif path_plan.laneProb > 0.0:
self.lane_prob_reset = False
Expand All @@ -142,11 +155,20 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
self.pid.neg_limit = -steers_max
deadzone = 0.0

steer_feedforward = (self.dampened_desired_angle - path_plan.angleOffset) * v_ego**2
#print("sr %1.3f ff %1.3f offset %1.3f damp_des_angle %1.3f damp_des_rate %1.3f " % ( VM.sR, steer_feedforward, path_plan.angleOffset, self.dampened_desired_angle))
angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 0.5) * v_ego**2
rate_feedforward = self.ff_rate_factor * path_plan.rateSteers * v_ego**2
rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward))
rate_same_direction = (rate_feedforward > 0) == (angle_feedforward > 0)
rate_away_from_zero = ((angle_steers - path_plan.angleOffset) > 0 == (rate_feedforward > 0))

if rate_more_significant and rate_same_direction: # and rate_away_from_zero:
self.feed_forward += ((rate_feedforward - self.feed_forward) / self.desired_smoothing)
#print(self.feed_forward)
else:
self.feed_forward += ((angle_feedforward - self.feed_forward) / self.desired_smoothing)

output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)

self.sat_flag = self.pid.saturated
self.prev_angle_steers = float(angle_steers)
Expand Down
28 changes: 20 additions & 8 deletions selfdrive/controls/lib/model_parser.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
import math
from common.numpy_fast import interp
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv, calc_poly_curvature
from common.numpy_fast import interp, clip
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv

CAMERA_OFFSET = 0.06 # m from center car to camera

Expand All @@ -20,6 +20,14 @@ def __init__(self):
self.r_prob = 0.
self.lane_prob= 0.

def fix_polys(self, winner_points, path_points):
step_size = winner_points[1] - winner_points[0]
path_points[0] = clip(path_points[0], winner_points[0] - self.lane_width / 2.0, winner_points[0] + self.lane_width / 2.0)
for i in range(1,50):
winner_points[i] = winner_points[i-1] + step_size
path_points[i] = path_points[i-1] + step_size
return model_polyfit(winner_points, self._path_pinv), model_polyfit(path_points, self._path_pinv)

def update(self, v_ego, md, v_curv=0.0):
if md is not None:
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path
Expand All @@ -44,22 +52,26 @@ def update(self, v_ego, md, v_curv=0.0):
(1 - self.lane_width_certainty) * speed_lane_width

lane_width_diff = abs(self.lane_width - current_lane_width)
lane_prob = interp(lane_width_diff, [0.1, 0.5], [1.0, 0.0])
lane_prob = interp(lane_width_diff, [0.3, 0.31], [1.0, 0.0])

steer_compensation = v_curv * v_ego
total_left_divergence = (md.model.leftLane.points[5] - md.model.leftLane.points[0]) * r_prob + steer_compensation
total_right_divergence = -((md.model.rightLane.points[5] - md.model.rightLane.points[0]) * l_prob + steer_compensation)

if (total_left_divergence > abs(total_right_divergence) and (self.lane_prob > 0 or self.r_prob > 0)) or (self.lane_prob == 0 and self.l_prob == 0):
if (r_prob > 0.5 and total_left_divergence > abs(total_right_divergence) \
and (self.lane_prob > 0 or self.r_prob > 0)) or (self.lane_prob == 0 and self.l_prob == 0):
l_prob *= lane_prob
p_prob *= lane_prob
if lane_prob == 0.0:
r_prob *= 2.0
elif total_right_divergence > abs(total_left_divergence) or (self.lane_prob == 0 and self.r_prob == 0):
p_prob = 0.5
r_prob *= 1.5
r_poly, p_poly = self.fix_polys(map(float, md.model.rightLane.points), map(float, md.model.path.points))
elif (l_prob > 0.5 and total_right_divergence > abs(total_left_divergence)) \
or (self.lane_prob == 0 and self.r_prob == 0):
r_prob *= lane_prob
p_prob *= lane_prob
if lane_prob == 0.0:
p_prob = 0.5
l_prob *= 1.5
l_poly, p_poly = self.fix_polys(map(float, md.model.leftLane.points), map(float, md.model.path.points))

self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
Expand Down

0 comments on commit df2f4f2

Please sign in to comment.