Skip to content
Permalink
Browse files

mostly tested, and awesome

  • Loading branch information...
Gernby committed Apr 3, 2019
1 parent 9520ca9 commit 4ef7f480d6746a4644409eb8c6a936c6220d7e2b
@@ -620,6 +620,7 @@ struct PathPlan {
mpcAngles @14 :List(Float32);
mpcRates @15 :List(Float32);
mpcTimes @16 :List(Float32);
laneProb @17 :Float32;
valid @9 :Bool;
paramsValid @10 :Bool;
modelValid @12 :Bool;
@@ -11,6 +11,12 @@
from selfdrive.controls.lib.vehicle_model import VehicleModel
from common.realtime import set_realtime_priority, Ratekeeper
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv, calc_poly_curvature
import importlib
from collections import defaultdict, deque
from fastcluster import linkage_vector
from selfdrive.controls.lib.vehicle_model import VehicleModel
from cereal import car
from common.params import Params

try:
from selfdrive.kegman_conf import kegman_conf
@@ -36,10 +42,11 @@ 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=False, poller=poller)
liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=True, 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)
#model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller)
model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller)
#frame = messaging.sub_sock(context, service_list['frame'].port, addr=ipaddress, conflate=True, poller=poller)
#sensorEvents = messaging.sub_sock(context, service_list['sensorEvents'].port, addr=ipaddress, conflate=True, poller=poller)
#carControl = messaging.sub_sock(context, service_list['carControl'].port, addr=ipaddress, conflate=True, poller=poller)
@@ -53,7 +60,7 @@ def dashboard_thread(rate=200):
carControl = "disabled"
sensorEvents = "disabled"
frame = "disabled"
model = "disabled"
#model = "disabled"
live20 = "disabled"

_model = None
@@ -82,6 +89,9 @@ def dashboard_thread(rate=200):
prev_l_curv = None
prev_r_curv = None
prev_p_curv = None
prev_l_sum = 0
prev_r_sum = 0
prev_p_sum = 0

current_rate = rate
rk = Ratekeeper(current_rate, print_delay_threshold=np.inf)
@@ -90,6 +100,10 @@ def dashboard_thread(rate=200):
monoTimeOffset = 0
receiveTime = 0

CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
VM = VehicleModel(CP)
_path_pinv = compute_path_pinv()

while 1:
#try:
#receiveTime = int(time.time() * 1000000000)
@@ -123,6 +137,7 @@ def dashboard_thread(rate=200):
actual_angle_change_noise = ((99. * actual_angle_change_noise) + (math.pow(abs_angle_change, 2.))) / 100.
last_desired = l100.live100.angleSteersDes
last_actual = l100.live100.angleSteers
v_curv = l100.live100.curvature

influxLineString += ("opData,sources=capnp ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" %
(angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes, l100.live100.dampAngleRateDes, l100.live100.dampAngleSteers, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid,
@@ -177,8 +192,65 @@ def dashboard_thread(rate=200):
driverMonitoringOn = false,
alertType = "",
alertSound = "" ) )'''
#elif socket is model:
# _model = messaging.recv_one(socket)
elif socket is model:
_model = messaging.recv_one(socket)
md = _model.model
if vEgo > 0: # and l100.live100.active:
influxLineString += ("opLines,sources=capnp ")
influxLineString += ("l0=%1.3f,p0=%1.3f,r0=%1.3f" % (md.leftLane.points[0], md.path.points[0], md.rightLane.points[0]))
lp = md.leftLane.points
rp = md.rightLane.points
pp = md.path.points

p_sum = np.sum(md.path.points)
l_sum = np.sum(md.leftLane.points)
r_sum = np.sum(md.rightLane.points)

l_change = l_sum - prev_l_sum
r_change = r_sum - prev_r_sum
p_change = p_sum - prev_p_sum
prev_l_sum = l_sum
prev_r_sum = r_sum
prev_p_sum = p_sum


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)

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
l_poly_far = model_polyfit(map(float, md.leftLane.points)[25:50], far_pinv) # left line
r_poly_far = model_polyfit(map(float, md.rightLane.points)[25:50], far_pinv) # right line

p_poly_near = model_polyfit(map(float, md.path.points)[0:30], near_pinv) # predicted path
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):
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))

frame_count += 1

'''model = (
frameId = 19786,
path = (
@@ -446,7 +518,7 @@ def dashboard_thread(rate=200):
# print(identifier)
# pass

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

if __name__ == "__main__":
@@ -343,7 +343,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
"dampAngleSteers": float(LaC.dampened_angle_steers),
"curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
"curvature": VM.calc_curvature((LaC.dampened_angle_steers - path_plan.pathPlan.angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
@@ -57,6 +57,7 @@ def __init__(self, CP):
self.rough_steers_rate = 0.0
self.prev_angle_steers = 0.0
self.calculate_rate = True
self.lane_prob_reset = False

KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
@@ -128,6 +129,12 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
if not steer_override:
self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing

if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
self.dampened_desired_angle = path_plan.angleSteers
self.lane_prob_reset = True
elif path_plan.laneProb > 0.0:
self.lane_prob_reset = False

if CP.steerControlType == car.CarParams.SteerControlType.torque:

steers_max = get_steer_max(CP, v_ego)
@@ -13,8 +13,20 @@ def __init__(self):
self.r_poly = [0., 0., 0., 0.]
self.l_avg_poly = [0., 0., 0., 0.]
self.r_avg_poly = [0., 0., 0., 0.]
self.p_curv = 0.0
self.v_avg_curv = 0.0
self.p_avg_curv_far = 0.0
self.l_avg_curv_far = 0.0
self.r_avg_curv_far = 0.0
self.p_avg_curv_near = 0.0
self.l_avg_curv_near = 0.0
self.r_avg_curv_near = 0.0
self.l_sum_avg = 0.0
self.r_sum_avg = 0.0

self.c_prob = 0.
self.l_sum = 0
self.p_sum = 0
self.r_sum = 0
self.last_model = 0.
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
self._path_pinv = compute_path_pinv()
@@ -24,13 +36,27 @@ def __init__(self):
self.lane_width = 3.7
self.l_prob = 0.
self.r_prob = 0.
self.lane_prob= 0.

def update(self, v_ego, md):
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
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line

'''
#print(self._path_pinv[0:4][25:50])
far_pinv = [self._path_pinv[0][25:50],self._path_pinv[1][25:50],self._path_pinv[2][25:50],self._path_pinv[3][25:50]]
near_pinv = [self._path_pinv[0][0:30],self._path_pinv[1][0:30],self._path_pinv[2][0:30],self._path_pinv[3][0:30]]
p_poly_far = model_polyfit(map(float, md.model.path.points)[25:50], far_pinv) # predicted path
l_poly_far = model_polyfit(map(float, md.model.leftLane.points)[25:50], far_pinv) # left line
r_poly_far = model_polyfit(map(float, md.model.rightLane.points)[25:50], far_pinv) # right line
p_poly_near = model_polyfit(map(float, md.model.path.points)[0:30], near_pinv) # predicted path
l_poly_near = model_polyfit(map(float, md.model.leftLane.points)[0:30], near_pinv) # left line
r_poly_near = model_polyfit(map(float, md.model.rightLane.points)[0:30], near_pinv) # right line
'''

# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET
r_poly[3] += CAMERA_OFFSET
@@ -49,8 +75,10 @@ def update(self, v_ego, md):
(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.2, 1.0], [1.0, 0.0])
lane_prob = interp(lane_width_diff, [0.1, 0.5], [1.0, 0.0])


'''
l_divergence = (l_poly[2] - self.l_avg_poly[2])
r_divergence = (self.r_avg_poly[2] - r_poly[2])
self.p_curv = ((9.0 * self.p_curv) + calc_poly_curvature(p_poly)) / 10.0
@@ -71,9 +99,62 @@ def update(self, v_ego, md):
#print(" left curv prob %1.2f" % curv_prob)
l_prob *= curv_prob
p_prob *= curv_prob
left = md.model.leftLane
right = md.model.rightLane
l_sum = left.points[0:10]
r_sum = right.points[0:10]
#l_diverging = (l_sum - self.l_sum_avg) > abs(r_sum - self.r_sum_avg) and (v_curv > self.v_avg_curv)
#r_diverging = (self.l_sum_avg - r_sum) > abs(l_sum - self.l_sum_avg) and (v_curv < self.v_avg_curv)
'''

steer_compensation = v_curv * v_ego
total_left_divergence = md.model.leftLane.points[5] - md.model.leftLane.points[0] + steer_compensation
total_right_divergence = -(md.model.rightLane.points[5] - md.model.rightLane.points[0] + 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):
l_prob *= lane_prob
p_prob *= lane_prob # (1.0 - (1.0 - lane_prob) / 2.0)
if lane_prob == 0.0:
r_prob *= 1.5
elif 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 # (1.0 - (1.0 - lane_prob) / 2.0)
if lane_prob == 0.0:
l_prob *= 1.5 #(1.0 + (1.0 - lane_prob))

'''if lane_prob == 0 and self.l_prob == 0.0 and self.r_prob > 0.4 and r_prob > 0.4:
l_prob = 0.0
elif lane_prob == 0 and self.r_prob == 0.0 and self.l_prob > 0.4 and l_prob > 0.4:
r_prob = 0.0
'''

'''if (l_curv_diff_far) > (l_curv_diff_near) or (v_curv > self.v_avg_curv and self.r_poly[3] > r_poly[3]) or self.l_prob == 0.0: # and left.std > right.std:
l_prob *= lane_prob
if (p_curv_diff_far) > (p_curv_diff_near):
p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
print(l_prob, r_prob, p_prob)
elif (r_curv_diff_far) < (r_curv_diff_near) or (v_curv < self.v_avg_curv and self.l_poly[3] < l_poly[3]): # and left.std < right.std:
r_prob *= lane_prob
if (p_curv_far - self.p_avg_curv_far) < (p_curv_far - self.p_avg_curv_near):
p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
print(" right")
elif (l_curv_near - self.l_avg_curv_near) > (r_curv_near - self.r_avg_curv_near): # and left.std > right.std:
l_prob *= lane_prob
if (p_curv_near - self.p_avg_curv_near) > (r_curv_near - self.r_avg_curv_near):
p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
print(l_prob, r_prob, p_prob)
elif (r_curv_near - self.r_avg_curv_near) < (l_curv_near - self.l_avg_curv_near): # and left.std < right.std:
r_prob *= lane_prob
if (r_curv_near - self.r_avg_curv_near) < (p_curv_near - self.p_avg_curv_near):
p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
print(" right")
self.l_avg_poly[2] = ((9.0 * self.l_avg_poly[2]) + l_poly[2]) / 10.0
self.r_avg_poly[2] = ((9.0 * self.r_avg_poly[2]) + r_poly[2]) / 10.0
#self.v_avg_curv += 0.25 * (v_curv - self.v_avg_curv)
self.l_sum_avg += 0.25 * (l_sum - self.l_sum_avg)
self.r_sum_avg += 0.25 * (r_sum - self.r_sum_avg)
'''

self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
@@ -83,6 +164,7 @@ def update(self, v_ego, md):
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)

self.lane_prob = lane_prob
self.r_poly = r_poly
self.r_prob = r_prob

@@ -53,11 +53,12 @@ def setup_mpc(self, steer_rate_cost):
def update(self, CP, VM, CS, md, live100, live_parameters):
v_ego = CS.carState.vEgo
angle_steers = live100.live100.dampAngleSteers
v_curv = live100.live100.curvature
active = live100.live100.active

angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage

self.MP.update(v_ego, md)
self.MP.update(v_ego, md, v_curv)

# Run MPC
self.angle_steers_des_prev = live100.live100.dampAngleSteersDes
@@ -131,6 +132,7 @@ def update(self, CP, VM, CS, md, live100, live_parameters):
plan_send.pathPlan.mpcAngles = map(float, self.mpc_angles)
plan_send.pathPlan.mpcRates = map(float, self.mpc_rates)
plan_send.pathPlan.mpcTimes = map(float, self.mpc_times)
plan_send.pathPlan.laneProb =float(self.MP.lane_prob)
plan_send.pathPlan.valid = bool(plan_valid)
plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)

@@ -132,6 +132,7 @@ def radard_thread(gctx=None):
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
v_curv = l100.live100.curvature
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride

@@ -147,7 +148,7 @@ def radard_thread(gctx=None):
last_md_ts = md.logMonoTime

# *** get path prediction from the model ***
MP.update(v_ego, md)
MP.update(v_ego, md, v_curv)

# run kalman filter only if prob is high enough
if MP.lead_prob > 0.7:

0 comments on commit 4ef7f48

Please sign in to comment.
You can’t perform that action at this time.