diff --git a/openpilot/starpilot/lateral/lane_centering.py b/openpilot/starpilot/lateral/lane_centering.py index 501740a2a5..cabe3092c6 100644 --- a/openpilot/starpilot/lateral/lane_centering.py +++ b/openpilot/starpilot/lateral/lane_centering.py @@ -133,6 +133,40 @@ def _at(path: tuple[np.ndarray, np.ndarray], distance: float) -> float | None: return float(np.interp(distance, x, y)) +def _single_boundary_target(model: Any, paths, qualified, lookahead: float, offset: float) -> float | None: + """Keep the confident trajectory clear of one line; an edge only bounds it. + + A road edge may include a shoulder or another lane, so its midpoint is never + a centering target. No width or confidence is assigned to the missing line. + """ + known_left = qualified[0] + line_path = paths[0 if known_left else 1] + model_path, std_path = paths[2:4] + if line_path is None or model_path is None or std_path is None: + return None + edge_index = 1 if known_left else 0 + edge_std = _finite_number(model.roadEdgeStds[edge_index]) + if edge_std is None or not 0.0 <= edge_std <= MAX_LANE_STD: + return None + edge_path = _path(model.roadEdges[edge_index]) + if edge_path is None: + return None + target = None + for distance in (lookahead * 0.25, lookahead * 0.5, lookahead): + line, edge = _at(line_path, distance), _at(edge_path, distance) + model_y, path_std = _at(model_path, distance), _at(std_path, distance) + if line is None or edge is None or model_y is None or path_std is None or not 0.0 <= path_std <= E2E_MAX_PATH_STD: + return None + left, right = (line, edge) if known_left else (edge, line) + if not MIN_LANE_WIDTH <= right - left <= MAX_LANE_WIDTH or not left < model_y < right: + return None + # Project the existing trajectory onto the clearance interval, without + # steering toward the unknown lane center or an edge/shoulder midpoint. + target = clip_scalar(model_y + clip_scalar(offset, -MAX_OFFSET, MAX_OFFSET), + left + MIN_CENTER_TO_LINE, right - MIN_CENTER_TO_LINE) + return target + + def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None: """Return a finite raw curvature correction, or None for unqualified model data.""" speed = _finite_number(speed_mps) @@ -146,23 +180,38 @@ def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: stds = model.laneLineStds if len(lines) < 3 or len(probs) < 3 or len(stds) < 3: return None + qualified = [] for i in (1, 2): prob = _finite_number(probs[i]) std = _finite_number(stds[i]) - if prob is None or std is None or not MIN_LANE_PROB <= prob <= 1.0 or not 0.0 <= std <= MAX_LANE_STD: + if prob is None or std is None or not 0.0 <= prob <= 1.0 or std < 0.0: return None + qualified.append(prob >= MIN_LANE_PROB and std <= MAX_LANE_STD) + if not any(qualified): + return None left_path, right_path, model_path = (_path(lines[1]), _path(lines[2]), _path(model.position)) if _paths is None else _paths[:3] - if left_path is None or right_path is None or model_path is None: + if model_path is None: return None lookahead = clip_scalar(speed, 8.0, 35.0) - left, right, model_y = _at(left_path, lookahead), _at(right_path, lookahead), _at(model_path, lookahead) - if left is None or right is None or model_y is None: + model_y = _at(model_path, lookahead) + if model_y is None: return None - width = right - left - if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH: - return None - safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE)) - target_y = (left + right) * 0.5 + clip_scalar(offset, -safe_offset, safe_offset) + if all(qualified): + if left_path is None or right_path is None: + return None + left, right = _at(left_path, lookahead), _at(right_path, lookahead) + if left is None or right is None: + return None + width = right - left + if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH: + return None + safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE)) + target_y = (left + right) * 0.5 + clip_scalar(offset, -safe_offset, safe_offset) + else: + std_path = _path_xy(model.position.x, model.position.yStd) if _paths is None else _paths[3] + target_y = _single_boundary_target(model, (left_path, right_path, model_path, std_path), qualified, lookahead, offset) + if target_y is None: + return None error = target_y - model_y error_abs = abs(error) error = math.copysign(max(0.0, error_abs - CENTER_ERROR_DEADBAND), error) diff --git a/openpilot/starpilot/lateral/tests/fixtures/route_bd_single_boundary.json b/openpilot/starpilot/lateral/tests/fixtures/route_bd_single_boundary.json new file mode 100644 index 0000000000..4aa50f9e97 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/route_bd_single_boundary.json @@ -0,0 +1 @@ +{"route":"8bc6f0b166ab516e/000000bd--ba5df1c904","build":"0427ba9fbf69b6a91922f7e2ef74652a5a32695c","modelMonoTime":46926621993657,"routeSeconds":2105.067442536,"speedMps":14.231454849243164,"model":{"position":{"x":[0.0,0.139892578125,0.5625,1.267578125,2.251953125,3.52734375,5.09765625,6.9296875,9.0859375,11.53125,14.328125,17.40625,20.796875,24.546875,28.59375,32.9375,37.625,42.65625,47.96875,53.59375,59.53125,65.75,72.3125,79.3125,86.625,94.0,101.8125,109.9375,118.1875,127.1875,135.875,145.125,154.875],"y":[0.0,-0.0005102157592773438,-0.0020389556884765625,-0.004131317138671875,-0.006168365478515625,-0.00728607177734375,-0.007335662841796875,-0.0077667236328125,-0.00995635986328125,-0.01392364501953125,-0.0172576904296875,-0.0193634033203125,-0.022247314453125,-0.0242462158203125,-0.0264129638671875,-0.0267791748046875,-0.0250701904296875,-0.024139404296875,-0.020294189453125,-0.0176544189453125,-0.014984130859375,-0.01171875,-0.01190948486328125,-0.0027332305908203125,-0.0025959014892578125,-0.0032291412353515625,0.0038890838623046875,-0.0052642822265625,-0.005970001220703125,-0.0003323554992675781,-0.00818634033203125,-0.01678466796875,-0.046539306640625],"z":[0.0,0.0005598068237304688,0.002227783203125,0.005558013916015625,0.01149749755859375,0.0218658447265625,0.037384033203125,0.0565185546875,0.0760498046875,0.09564208984375,0.11907958984375,0.1478271484375,0.17724609375,0.2113037109375,0.250244140625,0.287109375,0.31884765625,0.3466796875,0.37451171875,0.3984375,0.42138671875,0.4453125,0.473876953125,0.5009765625,0.53857421875,0.57568359375,0.62109375,0.67041015625,0.72119140625,0.77490234375,0.8359375,0.89013671875,0.9375],"t":[0.0,0.009765625,0.0390625,0.087890625,0.15625,0.244140625,0.3515625,0.478515625,0.625,0.791015625,0.9765625,1.181640625,1.40625,1.650390625,1.9140625,2.197265625,2.5,2.822265625,3.1640625,3.525390625,3.90625,4.306640625,4.7265625,5.166015625,5.625,6.103515625,6.6015625,7.119140625,7.65625,8.212890625,8.7890625,9.384765625,10.0],"xStd":[0.0008466537692584097,0.0013742547016590834,0.005146022420376539,0.011964860372245312,0.021665561944246292,0.03537333756685257,0.05177035927772522,0.07459340989589691,0.10175815969705582,0.13922281563282013,0.18158076703548431,0.23590268194675446,0.3046850860118866,0.3883684575557709,0.4904645085334778,0.6165335178375244,0.7613173127174377,0.9327854514122009,1.1321805715560913,1.3685075044631958,1.6386889219284058,1.9350947141647339,2.2979817390441895,2.6931800842285156,3.153262138366699,3.6542775630950928,4.2390360832214355,4.888639450073242,5.621297359466553,6.400942802429199,7.31010627746582,8.291528701782227,9.395533561706543],"yStd":[0.0008769482956267893,0.00091903394786641,0.0009937125723809004,0.0019762334413826466,0.0034819727297872305,0.0054993522353470325,0.007908289320766926,0.011108996346592903,0.014774629846215248,0.01911979168653488,0.02479122206568718,0.031957149505615234,0.04039762169122696,0.050669968128204346,0.06318307667970657,0.07547269016504288,0.09121542423963547,0.10874495655298233,0.12688766419887543,0.14719226956367493,0.16744348406791687,0.1895531266927719,0.2102263867855072,0.23179218173027039,0.249893918633461,0.2691463530063629,0.28959906101226807,0.31589552760124207,0.3483012616634369,0.3925628662109375,0.4553808867931366,0.5378813743591309,0.6478590965270996],"zStd":[0.0008977445540949702,0.00091903394786641,0.0009706933051347733,0.0018205916276201606,0.003388052573427558,0.005414092447608709,0.007664977107197046,0.00999701488763094,0.011964860372245312,0.014098051004111767,0.016807375475764275,0.019496895372867584,0.022661011666059494,0.026184748858213425,0.029729217290878296,0.03381946682929993,0.0379500538110733,0.043425053358078,0.048444148153066635,0.05393790081143379,0.06052568182349205,0.06686510145664215,0.07503176480531693,0.08192483335733414,0.09050558507442474,0.09940093755722046,0.10959785431623459,0.1199004128575325,0.1306600719690323,0.14350201189517975,0.15637962520122528,0.17191721498966217,0.18973833322525024]},"laneLines":[{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-3.9296875,-3.93359375,-3.935546875,-3.9140625,-3.91015625,-3.916015625,-3.91796875,-3.9296875,-3.947265625,-4.01171875,-4.09375,-4.16796875,-4.2109375,-4.24609375,-4.26953125,-4.28515625,-4.296875,-4.296875,-4.27734375,-4.2734375,-4.296875,-4.30078125,-4.3203125,-4.34765625,-4.375,-4.3984375,-4.4296875,-4.453125,-4.484375,-4.49609375,-4.5234375,-4.58203125,-4.59375],"z":[1.2119140625,1.2119140625,1.21484375,1.220703125,1.224609375,1.234375,1.2470703125,1.263671875,1.291015625,1.328125,1.3740234375,1.421875,1.4677734375,1.505859375,1.54296875,1.572265625,1.6005859375,1.6240234375,1.6494140625,1.673828125,1.7001953125,1.7314453125,1.7666015625,1.8017578125,1.8427734375,1.880859375,1.923828125,1.970703125,2.015625,2.056640625,2.111328125,2.162109375,2.212890625],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-0.8603515625,-0.8564453125,-0.857421875,-0.85888671875,-0.853515625,-0.8603515625,-0.86376953125,-0.8720703125,-0.8876953125,-0.908203125,-0.921875,-0.94287109375,-0.9599609375,-0.9716796875,-0.9765625,-0.98583984375,-1.001953125,-1.0048828125,-1.017578125,-1.025390625,-1.046875,-1.06640625,-1.095703125,-1.1171875,-1.1240234375,-1.1689453125,-1.1904296875,-1.216796875,-1.23828125,-1.2578125,-1.26171875,-1.294921875,-1.349609375],"z":[1.2275390625,1.228515625,1.23046875,1.23828125,1.2431640625,1.2529296875,1.267578125,1.2919921875,1.318359375,1.349609375,1.390625,1.423828125,1.466796875,1.501953125,1.53515625,1.5673828125,1.5927734375,1.619140625,1.6435546875,1.669921875,1.697265625,1.7275390625,1.763671875,1.794921875,1.8388671875,1.8779296875,1.9130859375,1.9560546875,2.001953125,2.044921875,2.091796875,2.140625,2.19140625],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[2.1328125,2.13671875,2.1328125,2.12890625,2.119140625,2.103515625,2.091796875,2.08203125,2.078125,2.078125,2.0859375,2.083984375,2.08984375,2.091796875,2.1015625,2.099609375,2.09765625,2.08203125,2.072265625,2.05859375,2.046875,2.02734375,2.005859375,1.990234375,1.9853515625,1.947265625,1.931640625,1.9267578125,1.9267578125,1.87890625,1.87109375,1.8564453125,1.869140625],"z":[1.23046875,1.2314453125,1.23046875,1.232421875,1.23828125,1.240234375,1.248046875,1.263671875,1.287109375,1.3232421875,1.3623046875,1.4033203125,1.4482421875,1.48828125,1.5234375,1.556640625,1.5859375,1.6142578125,1.6416015625,1.66796875,1.693359375,1.724609375,1.76171875,1.7890625,1.826171875,1.8681640625,1.90234375,1.9462890625,1.994140625,2.037109375,2.080078125,2.12890625,2.18359375],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[4.359375,4.34765625,4.34765625,4.3359375,4.32421875,4.30078125,4.26171875,4.21875,4.15625,4.109375,4.109375,4.1484375,4.203125,4.2421875,4.2890625,4.3046875,4.3046875,4.30078125,4.29296875,4.26953125,4.26171875,4.26171875,4.234375,4.22265625,4.20703125,4.1796875,4.171875,4.12890625,4.125,4.12109375,4.0703125,4.02734375,3.994140625],"z":[1.224609375,1.224609375,1.2275390625,1.23046875,1.236328125,1.2412109375,1.2470703125,1.25390625,1.26171875,1.279296875,1.3115234375,1.35546875,1.408203125,1.458984375,1.5078125,1.546875,1.5830078125,1.61328125,1.64453125,1.669921875,1.6962890625,1.7265625,1.7578125,1.7919921875,1.8232421875,1.8662109375,1.9052734375,1.9453125,1.9912109375,2.03125,2.0703125,2.12890625,2.171875],"t":[]}],"laneLineProbs":[0.8844391107559204,0.9834704995155334,0.06536578387022018,0.00016219282406382263],"laneLineStds":[0.2201003134250641,0.04806715250015259,0.29444700479507446,0.7789909243583679],"roadEdges":[{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-6.25390625,-6.25390625,-6.2265625,-6.21875,-6.2265625,-6.2109375,-6.1875,-6.1875,-6.21484375,-6.2265625,-6.25390625,-6.3359375,-6.40625,-6.49609375,-6.6015625,-6.68359375,-6.73828125,-6.765625,-6.80078125,-6.80078125,-6.796875,-6.8203125,-6.8359375,-6.8828125,-6.8984375,-6.92578125,-6.9453125,-6.97265625,-7.0,-6.99609375,-7.0546875,-7.07421875,-7.109375],"z":[1.19140625,1.19140625,1.1953125,1.197265625,1.20703125,1.2177734375,1.2294921875,1.248046875,1.265625,1.2939453125,1.3291015625,1.3720703125,1.4208984375,1.466796875,1.5126953125,1.5478515625,1.58203125,1.609375,1.6376953125,1.65625,1.681640625,1.708984375,1.7431640625,1.78125,1.8173828125,1.8544921875,1.9072265625,1.9501953125,1.9921875,2.03515625,2.083984375,2.14453125,2.185546875],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[3.03125,3.03515625,3.02734375,3.01953125,3.009765625,3.005859375,2.984375,2.94921875,2.9375,2.916015625,2.904296875,2.91796875,2.9453125,2.994140625,3.05859375,3.103515625,3.154296875,3.189453125,3.203125,3.220703125,3.240234375,3.21875,3.244140625,3.23046875,3.23828125,3.228515625,3.251953125,3.279296875,3.263671875,3.265625,3.244140625,3.291015625,3.271484375],"z":[1.2109375,1.20703125,1.208984375,1.216796875,1.22265625,1.2275390625,1.23828125,1.2509765625,1.2685546875,1.291015625,1.3251953125,1.3642578125,1.4091796875,1.4560546875,1.501953125,1.5478515625,1.5830078125,1.611328125,1.640625,1.66796875,1.6953125,1.7255859375,1.7529296875,1.7861328125,1.8193359375,1.8623046875,1.9013671875,1.9375,1.9873046875,2.03515625,2.078125,2.126953125,2.17578125],"t":[]}],"roadEdgeStds":[0.448978066444397,0.28316691517829895],"meta":{"engagedProb":0.19436782598495483,"desirePrediction":[0.9950177073478699,0.001044340431690216,0.00129716657102108,0.0019133473979309201,0.0005756142199970782,5.6439654144924134e-05,9.52599584707059e-05,1.5009071319127543e-07,0.9913216233253479,0.001533203525468707,0.0021120768506079912,0.004321022424846888,0.0005262511549517512,5.8014938986161724e-05,0.000127710125525482,1.5855667356845515e-07,0.9862257242202759,0.002622682834044099,0.003518881043419242,0.006593423429876566,0.0007828715606592596,7.353300316026434e-05,0.00018270823056809604,1.873227972737368e-07,0.9793033599853516,0.004438680596649647,0.005250513553619385,0.009305368177592754,0.0013353390386328101,0.0001031716019497253,0.00026345791411586106,1.856450211334959e-07],"deprecated":{"brakeDisengageProb":0.0,"gasDisengageProb":0.0,"steerOverrideProb":0.0},"desireState":[0.9998989105224609,1.663490547798574e-05,6.579233013326302e-05,7.014106984115642e-08,5.474088311530068e-07,2.472557980581769e-06,1.5505436749663204e-05,1.3581237602513786e-16],"disengagePredictions":{"t":[2.0,4.0,6.0,8.0,10.0],"brakeDisengageProbs":[0.0026939143426716328,0.004468384198844433,0.007093862164765596,0.011158031411468983,0.01652955636382103],"gasDisengageProbs":[0.001597845577634871,0.0031974869780242443,0.004962942097336054,0.0065388246439397335,0.008251599036157131],"steerOverrideProbs":[0.21551580727100372,0.2833845913410187,0.34224167466163635,0.3911152184009552,0.4398074746131897],"brake3MetersPerSecondSquaredProbs":[0.00017952796770259738,0.0006118340534158051,0.0014779430348426104,0.0027791906613856554,0.004520818125456572],"brake4MetersPerSecondSquaredProbs":[6.922183820279315e-05,0.00016346470511052758,0.00031258779927156866,0.0005233756965026259,0.0007613258203491569],"brake5MetersPerSecondSquaredProbs":[4.2315594328101724e-05,7.254361844388768e-05,0.00010229986219201237,0.00014768038818147033,0.00019411319226492196],"gasPressProbs":[0.9906600117683411,0.9936856627464294,0.9929060935974121,0.9930693507194519,0.9937100410461426,0.9941347241401672],"brakePressProbs":[0.001151399570517242,0.0031480707693845034,0.008445627056062222,0.01808999478816986,0.03289904072880745,0.05320620536804199]},"hardBrakePredicted":false,"laneChangeState":"off","laneChangeDirection":"none"}},"recordedFeedback":{"version":1,"modelMonoTime":46926621993657,"carControlMonoTime":46926634336380,"lateralActive":true,"requestedCorrection":0.0,"appliedCorrection":0.0,"reason":"unqualified_model"},"recordedCarControl":{"latActive":true,"longActive":false},"expectedRaw":0.0013339997200774304,"firstBookmarkRefusal":{"routeSeconds":2063.366822377,"modelMonoTime":46884921373498,"model":{"position":{"x":[0.0,0.1561279296875,0.626953125,1.4150390625,2.505859375,3.921875,5.66015625,7.67578125,10.0390625,12.703125,15.71875,19.03125,22.65625,26.609375,30.875,35.4375,40.3125,45.53125,51.0,56.75,62.8125,69.125,75.75,82.6875,89.9375,97.1875,104.8125,112.5625,120.5,128.75,136.75,145.0,153.5],"y":[0.0,0.0001232624053955078,0.0004863739013671875,0.0010805130004882812,0.0019330978393554688,0.0030460357666015625,0.00439453125,0.00579071044921875,0.007236480712890625,0.00943756103515625,0.01287078857421875,0.01470184326171875,0.0114288330078125,0.006687164306640625,0.0010290145874023438,-0.00492095947265625,-0.01531219482421875,-0.0300750732421875,-0.04876708984375,-0.0723876953125,-0.0987548828125,-0.1273193359375,-0.16259765625,-0.1949462890625,-0.23095703125,-0.269287109375,-0.29931640625,-0.341064453125,-0.366943359375,-0.393310546875,-0.420166015625,-0.444091796875,-0.458740234375],"z":[0.0,0.00026154518127441406,0.0010538101196289062,0.002429962158203125,0.004283905029296875,0.006656646728515625,0.00916290283203125,0.01177215576171875,0.01557159423828125,0.0225982666015625,0.033660888671875,0.04376220703125,0.04498291015625,0.04547119140625,0.0511474609375,0.057830810546875,0.06365966796875,0.068603515625,0.0758056640625,0.08441162109375,0.09600830078125,0.1109619140625,0.131103515625,0.1553955078125,0.186279296875,0.217041015625,0.255126953125,0.295166015625,0.335205078125,0.377197265625,0.424560546875,0.47021484375,0.51220703125],"t":[0.0,0.009765625,0.0390625,0.087890625,0.15625,0.244140625,0.3515625,0.478515625,0.625,0.791015625,0.9765625,1.181640625,1.40625,1.650390625,1.9140625,2.197265625,2.5,2.822265625,3.1640625,3.525390625,3.90625,4.306640625,4.7265625,5.166015625,5.625,6.103515625,6.6015625,7.119140625,7.65625,8.212890625,8.7890625,9.384765625,10.0],"xStd":[0.000853294157423079,0.0010999395744875073,0.0038996245712041855,0.008856864646077156,0.0160377100110054,0.026338625699281693,0.03802424669265747,0.05554148927330971,0.07591621577739716,0.10275676101446152,0.1319422870874405,0.1667906939983368,0.21084319055080414,0.26394057273864746,0.33137911558151245,0.4072054326534271,0.49940598011016846,0.6083102226257324,0.7393366098403931,0.9016619324684143,1.089006781578064,1.3135128021240234,1.586721420288086,1.9256689548492432,2.3318912982940674,2.826565980911255,3.4395875930786133,4.17331600189209,5.068510055541992,6.131727695465088,7.447009086608887,9.00036907196045,10.920316696166992],"yStd":[0.0008769482956267893,0.0009118819725699723,0.0009335066424682736,0.0015271148877218366,0.002669721143320203,0.004233002196997404,0.00608722772449255,0.008484356105327606,0.011108996346592903,0.014043087139725685,0.01796138286590576,0.022749705240130424,0.028702206909656525,0.036212194710969925,0.04622572660446167,0.057304564863443375,0.07258138060569763,0.09193083643913269,0.1159847155213356,0.14618949592113495,0.18282629549503326,0.22886811196804047,0.28289052844047546,0.34660470485687256,0.41564252972602844,0.49479424953460693,0.5773443579673767,0.6663062572479248,0.7587195634841919,0.8572267889976501,0.9682576060295105,1.096342921257019,1.2463444471359253],"zStd":[0.0008907582378014922,0.0009118819725699723,0.0009371602791361511,0.001316455309279263,0.002346835331991315,0.0037210476584732533,0.005330154672265053,0.0072570545598864555,0.009138011373579502,0.011239945888519287,0.01387948077172041,0.016418032348155975,0.019420884549617767,0.022528620436787605,0.025478459894657135,0.028534522280097008,0.03177044913172722,0.036000631749629974,0.039616260677576065,0.04359501227736473,0.04816112294793129,0.05248301848769188,0.05798005312681198,0.06293675303459167,0.06912250071763992,0.07576808333396912,0.0835406556725502,0.09175146371126175,0.10057264566421509,0.11067339032888412,0.12178856879472733,0.13454462587833405,0.15024201571941376]},"laneLines":[{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-4.52734375,-4.53125,-4.5390625,-4.52734375,-4.53125,-4.546875,-4.5546875,-4.55078125,-4.515625,-4.5,-4.484375,-4.4765625,-4.4765625,-4.5,-4.54296875,-4.5859375,-4.63671875,-4.6640625,-4.67578125,-4.69140625,-4.7265625,-4.7421875,-4.78125,-4.8125,-4.859375,-4.890625,-4.9140625,-4.93359375,-4.9453125,-4.9296875,-4.9296875,-4.91796875,-4.859375],"z":[1.2646484375,1.263671875,1.2666015625,1.271484375,1.271484375,1.27734375,1.279296875,1.279296875,1.2802734375,1.279296875,1.279296875,1.28125,1.291015625,1.3037109375,1.322265625,1.337890625,1.3564453125,1.3720703125,1.388671875,1.404296875,1.421875,1.44921875,1.4833984375,1.5205078125,1.5673828125,1.611328125,1.6630859375,1.72265625,1.7841796875,1.845703125,1.9189453125,1.99609375,2.076171875],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-1.3369140625,-1.3310546875,-1.333984375,-1.333984375,-1.326171875,-1.328125,-1.326171875,-1.3173828125,-1.3154296875,-1.3212890625,-1.31640625,-1.330078125,-1.34375,-1.359375,-1.369140625,-1.3916015625,-1.4208984375,-1.4404296875,-1.4677734375,-1.49609375,-1.5341796875,-1.568359375,-1.6201171875,-1.65625,-1.689453125,-1.748046875,-1.7822265625,-1.822265625,-1.8447265625,-1.869140625,-1.8662109375,-1.86328125,-1.8740234375],"z":[1.263671875,1.2646484375,1.265625,1.26953125,1.2705078125,1.2724609375,1.2724609375,1.27734375,1.27734375,1.28125,1.291015625,1.294921875,1.310546875,1.3232421875,1.3369140625,1.3525390625,1.36328125,1.375,1.3896484375,1.404296875,1.423828125,1.447265625,1.48046875,1.515625,1.5615234375,1.6103515625,1.6591796875,1.71484375,1.7802734375,1.8466796875,1.919921875,1.994140625,2.072265625],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[2.037109375,2.041015625,2.0390625,2.041015625,2.04296875,2.041015625,2.04296875,2.044921875,2.05078125,2.048828125,2.046875,2.037109375,2.029296875,2.01953125,2.013671875,2.001953125,1.984375,1.9609375,1.9384765625,1.9130859375,1.888671875,1.8642578125,1.8369140625,1.8076171875,1.7880859375,1.7451171875,1.7265625,1.7177734375,1.7216796875,1.7109375,1.720703125,1.7490234375,1.810546875],"z":[1.2646484375,1.265625,1.2646484375,1.2666015625,1.2734375,1.2734375,1.27734375,1.28515625,1.2919921875,1.3037109375,1.3095703125,1.31640625,1.328125,1.337890625,1.349609375,1.36328125,1.376953125,1.3896484375,1.4013671875,1.4169921875,1.43359375,1.45703125,1.4912109375,1.521484375,1.564453125,1.6171875,1.66796875,1.7255859375,1.7890625,1.857421875,1.927734375,2.005859375,2.08984375],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[4.36328125,4.359375,4.359375,4.359375,4.359375,4.36328125,4.36328125,4.36328125,4.35546875,4.35546875,4.3515625,4.34375,4.3203125,4.27734375,4.26953125,4.25390625,4.234375,4.2109375,4.18359375,4.13671875,4.10546875,4.08203125,4.0390625,4.01171875,3.974609375,3.9375,3.921875,3.890625,3.896484375,3.896484375,3.89453125,3.91015625,3.92578125],"z":[1.2646484375,1.263671875,1.2666015625,1.267578125,1.2734375,1.27734375,1.2841796875,1.2890625,1.296875,1.3056640625,1.3173828125,1.326171875,1.3359375,1.341796875,1.35546875,1.3671875,1.3828125,1.3955078125,1.4091796875,1.421875,1.439453125,1.4638671875,1.490234375,1.525390625,1.5654296875,1.619140625,1.6708984375,1.728515625,1.7939453125,1.859375,1.9375,2.017578125,2.099609375],"t":[]}],"laneLineProbs":[0.32809871435165405,0.341802179813385,0.08254930377006531,0.0003514382697176188],"laneLineStds":[0.2352125644683838,0.9191089868545532,0.302609384059906,0.9315337538719177],"roadEdges":[{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[-7.20703125,-7.20703125,-7.1875,-7.18359375,-7.203125,-7.1875,-7.1640625,-7.1640625,-7.171875,-7.15625,-7.12890625,-7.14453125,-7.1484375,-7.15625,-7.2109375,-7.28515625,-7.35546875,-7.40625,-7.4765625,-7.5078125,-7.5234375,-7.5703125,-7.609375,-7.6640625,-7.69140625,-7.72265625,-7.7421875,-7.75390625,-7.7578125,-7.734375,-7.7421875,-7.70703125,-7.66015625],"z":[1.2451171875,1.244140625,1.24609375,1.24609375,1.25,1.2548828125,1.2578125,1.263671875,1.263671875,1.2685546875,1.2734375,1.2802734375,1.2890625,1.2958984375,1.30859375,1.318359375,1.3359375,1.3505859375,1.369140625,1.3828125,1.404296875,1.4287109375,1.4599609375,1.498046875,1.5400390625,1.587890625,1.6484375,1.70703125,1.767578125,1.833984375,1.91015625,1.9873046875,2.064453125],"t":[]},{"x":[0.0,0.1875,0.75,1.6875,3.0,4.6875,6.75,9.1875,12.0,15.1875,18.75,22.6875,27.0,31.6875,36.75,42.1875,48.0,54.1875,60.75,67.6875,75.0,82.6875,90.75,99.1875,108.0,117.1875,126.75,136.6875,147.0,157.6875,168.75,180.1875,192.0],"y":[3.525390625,3.53125,3.53125,3.537109375,3.544921875,3.55859375,3.546875,3.509765625,3.47265625,3.37890625,3.26953125,3.1640625,3.060546875,2.97265625,2.919921875,2.87109375,2.857421875,2.857421875,2.828125,2.83203125,2.82421875,2.798828125,2.796875,2.765625,2.76171875,2.7421875,2.734375,2.751953125,2.740234375,2.767578125,2.78515625,2.865234375,2.9140625],"z":[1.26171875,1.2587890625,1.259765625,1.265625,1.2705078125,1.2734375,1.279296875,1.2861328125,1.2939453125,1.30078125,1.3115234375,1.3212890625,1.3330078125,1.341796875,1.3515625,1.3642578125,1.3779296875,1.384765625,1.3984375,1.4140625,1.4306640625,1.4541015625,1.478515625,1.513671875,1.5556640625,1.607421875,1.6611328125,1.71875,1.78515625,1.85546875,1.927734375,2.0078125,2.087890625],"t":[]}],"roadEdgeStds":[0.5285090804100037,0.7218573689460754],"meta":{"engagedProb":0.5112133622169495,"desirePrediction":[0.9931489825248718,0.0004553816979750991,0.0003609408449847251,0.004891035612672567,0.0007731188670732081,7.699898560531437e-05,0.0002934428339358419,1.2516996150679915e-07,0.9798332452774048,0.0008660023449920118,0.0007597795338369906,0.017246125265955925,0.0008215175475925207,7.746491610305384e-05,0.00039571113302372396,1.5428946653628373e-07,0.97214275598526,0.0014193595852702856,0.0015497542917728424,0.022896120324730873,0.001370327197946608,9.003050217870623e-05,0.0005314127774909139,1.908816926743384e-07,0.96703040599823,0.001967894844710827,0.0025022709742188454,0.025445392355322838,0.0022386445198208094,0.00010655954974936321,0.000708560342900455,2.065134339090946e-07],"deprecated":{"brakeDisengageProb":0.0,"gasDisengageProb":0.0,"steerOverrideProb":0.0},"desireState":[0.9999758005142212,3.4735385270323604e-06,1.4174206626194064e-05,2.053681242841776e-07,6.276556518969301e-07,1.0348292107664747e-06,4.674157025874592e-06,3.876217650387996e-17],"disengagePredictions":{"t":[2.0,4.0,6.0,8.0,10.0],"brakeDisengageProbs":[0.004198795184493065,0.007636216003447771,0.01182339433580637,0.017576884478330612,0.024750972166657448],"gasDisengageProbs":[0.0018458482809364796,0.0034970103297382593,0.005119676236063242,0.006744989659637213,0.00854433048516512],"steerOverrideProbs":[0.12347127497196198,0.1772405207157135,0.2276410013437271,0.2750323712825775,0.319121390581131],"brake3MetersPerSecondSquaredProbs":[0.0002131875662598759,0.0006361912237480283,0.0014437558129429817,0.0026316740550100803,0.004198795184493065],"brake4MetersPerSecondSquaredProbs":[0.00010472556459717453,0.00024536947603337467,0.00045474793296307325,0.0007152324542403221,0.0010484692174941301],"brake5MetersPerSecondSquaredProbs":[4.3319039832567796e-05,8.888084994396195e-05,0.00012830943160224706,0.0001823546044761315,0.0002492325147613883],"gasPressProbs":[0.9726754426956177,0.9852713346481323,0.9842154383659363,0.9834068417549133,0.9837850332260132,0.9852146506309509],"brakePressProbs":[0.0027149852830916643,0.006050397641956806,0.014447852037847042,0.029367854818701744,0.050517670810222626,0.07696083933115005]},"hardBrakePredicted":false,"laneChangeState":"off","laneChangeDirection":"none"}}}} diff --git a/openpilot/starpilot/lateral/tests/test_lane_centering.py b/openpilot/starpilot/lateral/tests/test_lane_centering.py index b915e5ec2d..109220fc36 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_centering.py +++ b/openpilot/starpilot/lateral/tests/test_lane_centering.py @@ -1,6 +1,8 @@ """Native model-message fixtures for the standalone curvature contribution.""" from dataclasses import replace +import json import math +from pathlib import Path from types import SimpleNamespace from typing import Any, cast import unittest @@ -320,3 +322,119 @@ class TestLaneCentering(unittest.TestCase): result = controller.update(observation(elapsed_seconds=0.2)) self.assertEqual(result.reason, "qualified") self.assertGreater(result.correction, 0.0) + + +def single(*, missing_right=True, line=-0.9, edge=3.0, path=0.0): + m = model(left=line if missing_right else -3.0, right=2.0 if missing_right else -line, path=path) + m.laneLineProbs = [0., .95 if missing_right else .05, .05 if missing_right else .95, 0.] + m.init('roadEdges', 2) + for item, y in zip(m.roadEdges, (-3.0 if missing_right else -edge, edge if missing_right else 3.0), strict=True): + item.x = XS + item.y = [y] * len(XS) + m.roadEdgeStds = [.1, .1] + return m + + +class TestSingleBoundary(unittest.TestCase): + def test_recorded_bd_missing_right_line_recovers_bounded_clearance(self): + fixture = json.loads((Path(__file__).parent / 'fixtures/route_bd_single_boundary.json').read_text()) + m = messaging.new_message('modelV2').modelV2 + m.from_dict(fixture['model']) + self.assertTrue(fixture['recordedCarControl']['latActive']) + self.assertEqual(fixture['recordedFeedback']['requestedCorrection'], 0.) + self.assertEqual(fixture['recordedFeedback']['reason'], 'unqualified_model') + self.assertLess(m.laneLineProbs[2], .6) + raw = raw_correction(m, fixture['speedMps'], 0., 0.) + assert raw is not None + self.assertGreater(raw, 0.) + self.assertAlmostEqual(raw, fixture['expectedRaw'], places=12) + result = settled(LaneCenteringController(), observation(m, speed_mps=fixture['speedMps'])) + self.assertLessEqual(abs(result.correction), MAX_CORRECTION) + # At the exact earlier bookmark BOTH line probabilities are low and the + # left-line std is .919 m. Preserve that refusal rather than invent a lane. + refused = messaging.new_message('modelV2').modelV2 + refused.from_dict(fixture['firstBookmarkRefusal']['model']) + self.assertIsNone(raw_correction(refused, 15.75, 0., 0.)) + + def test_missing_either_line_recovers_clearance_without_centering_on_edge(self): + for missing_right, sign in ((True, 1), (False, -1)): + for edge in (2.5, 3.0): + with self.subTest(missing_right=missing_right, edge=edge): + m = single(missing_right=missing_right, edge=edge) + raw = raw_correction(m, 20., 0., 0.) + assert raw is not None + self.assertAlmostEqual(raw, sign * 2. * (.2 - .08) / 400., places=8) + result = settled(LaneCenteringController(), observation(m)) + self.assertEqual(result.reason, 'qualified') + self.assertGreater(sign * result.correction, 0.) + self.assertLessEqual(abs(result.correction), MAX_CORRECTION) + + def test_safe_model_path_is_retained_and_missing_line_geometry_is_unused(self): + m = single(line=-1.5) + self.assertEqual(raw_correction(m, 20., 0., 0.), 0.) + m.laneLines[2].x = [] + m.laneLines[2].y = [] + self.assertEqual(raw_correction(m, 20., 0., 0.), 0.) + + def test_refuses_both_missing_and_unqualified_path_edge_or_corridor(self): + for defect in ('both', 'path_std', 'edge_std', 'nan_edge', 'no_edge', 'crossed', 'wide', 'narrow', + 'path_outside', 'near_crossing', 'missing_std', 'short_edge', 'bad_x', 'nan_prob'): + with self.subTest(defect=defect): + m = single() + if defect == 'both': + m.laneLineProbs = [0., .05, .05, 0.] + elif defect == 'path_std': + m.position.yStd = [.36] * len(XS) + elif defect == 'edge_std': + m.roadEdgeStds = [.1, .31] + elif defect == 'nan_edge': + m.roadEdges[1].y = [float('nan')] * len(XS) + elif defect == 'no_edge': + m.roadEdges = [] + elif defect == 'crossed': + m.roadEdges[1].y = [-2.] * len(XS) + elif defect == 'wide': + m.roadEdges[1].y = [4.] * len(XS) + elif defect == 'narrow': + m.roadEdges[1].y = [1.] * len(XS) + elif defect == 'path_outside': + m.position.y = [-1.] * len(XS) + elif defect == 'near_crossing': + m.roadEdges[1].y = [-2. if x <= 10 else 3. for x in XS] + elif defect == 'missing_std': + m.position.yStd = [] + elif defect == 'short_edge': + m.roadEdges[1].x = XS[:10] + m.roadEdges[1].y = [3.] * 10 + elif defect == 'bad_x': + m.roadEdges[1].x = list(reversed(XS)) + else: + m.laneLineProbs = [0., .95, float('nan'), 0.] + self.assertIsNone(raw_correction(m, 20., 0., 0.)) + + def test_current_edge_is_rechecked_for_held_reader(self): + m = single() + source = observation(m.as_reader()) + controller = LaneCenteringController() + first = controller.update(source) + self.assertGreater(first.correction, 0.) + m.roadEdgeStds = [.1, .31] + refused = controller.update(source) + self.assertEqual(refused.reason, 'unqualified_model') + self.assertAlmostEqual(refused.correction, first.correction * math.exp(-.01 / .2)) + m.roadEdgeStds = [.1, .1] + self.assertEqual(controller.update(source).reason, 'qualified') + + def test_single_boundary_still_obeys_override_freshness_signal_and_axis(self): + source = observation(single()) + for updates in ({'driver_override':True}, {'model_valid':False}, {'lateral_active':False}, + {'time_discontinuity':True}): + controller = LaneCenteringController() + self.assertGreater(settled(controller, source).correction, 0.) + self.assertEqual(controller.update(replace(source, **updates)).correction, 0.) + controller = LaneCenteringController() + prior = settled(controller, source).correction + faded = controller.update(replace(source, turn_signal_active=True)) + self.assertEqual(faded.reason, 'signal_release') + self.assertLess(faded.correction, prior) + diff --git a/prebuilt.json b/prebuilt.json index 83064fd9a4..8e5ddb1f89 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "6eb344118272c52e2d864a01ad40934ceb8fc964e9037f936a1fd4fc4ef94b40", "schema_version": 1} +{"protected_inventory_sha256": "8c00f349b0bfdafea755cdc8cb73e63050357ecdf2609f2c0f4f6d917ee983a0", "schema_version": 1}