mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-11-05 05:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support experimental signal routes with learned navigation inputs

Browse Source 
Install coherent transient Directions routes without replacing manual
destinations, and feed fresh generation-matched maps and maneuver vectors to
compatible navigation-trained models. Preserve existing action conversion,
the 1.6 m/s hold and Tesla/cooperative steering.

Keep signal intent pending during discovery. Preserve normal lane-change
eligibility, commit the first actual LC request or eligible mapped turn, and
prevent later same-signal role changes. Guard cancellation, retained exits,
manual handoff, stale input and delayed parameter writes.

Add explicit verified local model registration and an offroad target build
recipe. No navigation model or automatic model swap is bundled.

Validation: 223 software test methods, Ruff and syntax checks; separately,
800 Mac forward calls compare an available RL navigation model with and
without real short-route inputs. Automatic intent still misses recorded
requests; target compilation, device parity and physical clearance remain
unverified. Keep this opt-in and experimental.
This commit is contained in:
Isaac Barham
2026-11-03 13:31:34 -05:00
parent 3b1816ae6c
commit 2bb640ff21
31 changed files with 2247 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/common/libparams_c.so
BIN
View File
Binary file not shown.
openpilot/common/params_keys.h
+1
View File
@@ -262,6 +262,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"MapboxTurnIntent", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
{"MapboxTurnContext", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
{"MapboxTurnCue", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
{"NavModelState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}},
{"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}},
{"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
openpilot/selfdrive/controls/lib/desire_helper.py
+10 -1
View File
@@ -49,7 +49,11 @@ class DesireHelper:
model_position=model_position, map_turn_supported=map_turn_supported)
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
# Only an already committed turn owns the signal. Pending map discovery
# leaves the existing lane-change eligibility and timing unchanged.
navigation_signal = self.lane_turn_controller.navigation_owns_signal
if (navigation_signal or not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or
self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
@@ -108,4 +112,9 @@ class DesireHelper:
elif self.lane_change_direction == LaneChangeDirection.right:
self.desire = log.Desire.laneChangeRight
# Commit the first actual request before the next model run. LC wins a
# same-frame tie; an early fast signal can otherwise remain pending until
# a validated turn approach is reached at turn speed.
self.lane_turn_controller.finalize_navigation_intent(
self.desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight), v_ego, lateral_active, map_turn_supported)
self.alc.update_state()
openpilot/selfdrive/navigation/SIGNAL_NAVIGATION.md
+78
View File
@@ -0,0 +1,78 @@
# Signal navigation experiment
The physical turn signal selects a short real route through the next supported
street. The route and maneuver instructions condition a navigation-trained
driving network continuously. This replaces the earlier map-timed turn-cue
experiment when a compatible model is selected; it does not add steering angles.
The automatic candidate begins with a pending signal and may prefetch a route
at any speed. Pending discovery does not send navigation inputs or suppress the
existing lane-change checks. The first emitted host lane-change request commits
lane-change mode and cancels the candidate route. Otherwise, a fresh validated
street approach below the configured turn speed (19 mph by default) may commit
turn mode. An early fast signal can therefore prepare a route and later become
a turn, provided no lane-change request has already started. LC wins a same-frame
tie. After either commitment, the role is fixed until cancel/opposite.
Only a committed turn suppresses competing lane-change pulses. Map conditioning
requires a validated connected street, matching position and heading, and a
coherent fresh route. Pending intent cannot feed navigation.
This arbitration still cannot reliably identify every early fast turn signal or
slow lane change. A nearby legal street does not establish the driver's intended street.
Predicted missing lane lines must not be treated as proof that no adjacent lane
exists. These ambiguity cases remain validation limitations, not solved intent.
A causal host audit of the saved recordings gives this candidate navigation
eligibility on 23 of 39 street-turn requests; nine encounter a lane-change
request first and seven remain pending. All four recorded lane-change contexts
retain lane-change mode. These are selection proxies, not neural turn successes;
map-target identity is a separate check and can still select the wrong street.
An earlier speed-at-signal classifier was rejected because 18 street-turn
requests began above 20 mph. The automatic selector is not dependable enough to
claim road-test readiness from these results.
## Model requirement
CTV3 and the small fallback have no navigation input sockets. A route on the
screen cannot make those models follow navigation. The runtime checks actual
`nav_map` and `navigation` tensors, and publishes `NavModelState` separately from
route availability. The UI distinguishes **Route ready** from **Navigation active**.
The basis is commaai/openpilot `nav-model-integration`, pinned at
`afbf2e547e9319e2eb9f515e1e4330af63705cec`. The available BIG ONNX is the earlier
RL navigation checkpoint `80eedcdd/12864`, SHA256
`1d18c42ea4cd95c8bffab4635844b78b8e2d4e25b9de44a9c5ac2e0961371a2e`.
The pinned commit's newer N16 compiled PKL is a different artifact; running the
ONNX is not evidence of running those N16 weights. Official LFS downloads for
the latest three compiled navigation artifacts returned object-not-found.
The ONNX was downloaded through a third-party mirror and independently matched
against comma's exact LFS pointer. A Chestnut-compatible plain artifact still
requires the target compilation and verification step. A Mac CPU ONNX smoke is
not a verified device build. No model is automatically swapped when signaling.
## Route ownership and input coherence
Manual destinations take priority. Automatic routes do not overwrite
`NavDestination`. Each physical signal has an identifier; a held indicator does
not repeatedly select turns. Cancellation before entry retires the route;
already-entered turns may retain guidance through the receiving road, after
which it is cleared. A missed turn is retired rather than rerouted back to the
short endpoint.
Map frames carry the exact rendered `navRoute.logMonoTime` as their stream EOF
generation; SOF remains the GPS publication time. The driving process requires
a matching route generation, fresh map/instructions and the current signal
session (or explicitly retained exit guidance). Stale/opposite/canceled route
inputs are zeroed. A route preview alone never establishes active model input.
## Scope and validation
The feature is opt-in under **Signal Navigation (Experimental)**. It needs
online Directions requests, a Mapbox public token, and automatically fetched
road-map tiles. Existing steering, cooperative steering, action conversion and
the restored 1.6 m/s low-speed handling are preserved.
Recorded-camera model replay can test whether real route inputs change the
model's plan. It cannot establish physical curb clearance or a successful driven
turn, because the camera views remain the recorded ones. Model runtime checks,
software fixture checks and recorded-sequence results must be reported separately.
openpilot/selfdrive/navigation/mapbox_next_turn.py
+39 -2
View File
@@ -328,6 +328,31 @@ def _local_exit_scope(route, matched):
'targetScopeTruncated': True}
def coherent_route_steps(route):
"""Require a complete actual Directions leg, including its arrival step.
A clipped discovery prefix is not a new route: its old distances/banners no
longer describe its geometry. It must be rerouted to the real exit endpoint.
"""
if route.get('targetScopeTruncated'):
raise NextTurnError('incomplete_navigation_route')
steps = json.loads(json.dumps(route['steps']))
if not steps or steps[-1]['maneuver']['type'] != 'arrive':
raise NextTurnError('incomplete_navigation_route')
for step in steps:
points = [_point(point) for point in step['geometry']['coordinates']]
distance_m, duration = _number(step.get('distance')), _number(step.get('duration'))
if not points or distance_m < 0 or duration < 0:
raise NextTurnError('incomplete_navigation_route')
if abs(_length(points)-distance_m) > max(5., distance_m*.2):
raise NextTurnError('inconsistent_navigation_distance')
step.setdefault('bannerInstructions', [])
for before, after in zip(steps[:-1], steps[1:], strict=True):
if distance(_point(before['geometry']['coordinates'][-1]), _point(after['geometry']['coordinates'][0])) > 3.:
raise NextTurnError('disconnected_navigation_route')
return steps
class MapboxNextTurnClient:
def __init__(self, token):
if not isinstance(token, str) or not re.fullmatch(r'[ps]k\.[A-Za-z0-9_-]+\.[A-Za-z0-9_-]+', token):
@@ -367,28 +392,40 @@ class MapboxNextTurnClient:
query_endpoint = offset(candidate['location'], candidate['exit'], EXIT_QUERY_DISTANCE_M)
validated = self._request(position, heading, query_endpoint, profile='driving-traffic', endpoint_radius=EXIT_RADIUS_M)
matched, road_class, validated = _validate_turn(validated, candidate, direction)
request_count = 2
if validated.get('targetScopeTruncated'):
# Query an actual endpoint on the validated receiving road. Keep the
# complete new leg/arrival rather than turning a clipped preview into
# model navigation data with stale step distances and instructions.
query_endpoint = validated['points'][-1]
validated = self._request(position, heading, query_endpoint, profile='driving-traffic', endpoint_radius=3.)
matched, road_class, validated = _validate_turn(validated, candidate, direction)
request_count += 1
classification = None
if road_class is None:
request_count += 1
classification = self._request(position, heading, query_endpoint, profile='driving', endpoint_radius=EXIT_RADIUS_M)
_, road_class, classification = _validate_turn(classification, candidate, direction)
if road_class is None:
raise NextTurnError('missing_road_class')
complete_steps = coherent_route_steps(validated)
approach = tuple(validated['points'][:matched['index']]) + (matched['location'],)
turn_step = validated['steps'][matched['step']]
provenance = {'method': 'router intersection discovery plus separately validated first-turn route',
'requestedPosition': position, 'requestedBearingDeg': heading, 'requestedDirection': direction,
'probeResponseSha256': probe['responseSha256'], 'validationResponseSha256': validated['responseSha256'],
'classificationResponseSha256': classification['responseSha256'] if classification else None,
'requestCount': 3 if classification else 2,
'requestCount': request_count,
'targetScopeTruncated': validated.get('targetScopeTruncated', False),
'targetExitDistanceM': _length(validated['points'][matched['index']:]),
'fullValidationResponsePreservedByHash': True,
'requestStartedAt': started, 'requestCompletedAt': time.monotonic(),
'probeEndpointIsTarget': False, 'steeringCommandIssued': False}
provenance['coherentNavigationRoute'] = True
return NextTurnTarget(junction=matched['location'], distance_m=matched['distance'],
incoming_bearing_deg=matched['incoming'], exit_bearing_deg=matched['outgoing'], direction=direction,
name=str(turn_step.get('name') or 'Unnamed road'), road_class=road_class,
geometry=tuple(validated['points']), steps=_freeze(validated['steps']), approach_geometry=approach,
geometry=tuple(validated['points']), steps=_freeze(complete_steps), approach_geometry=approach,
approach_distance_m=_length(approach), turn_step_index=matched['step'], junction_geometry_index=matched['index'],
origin=position, discovered_at=started, probe_uuid=probe['uuid'], validation_uuid=validated['uuid'],
classification_uuid=classification['uuid'] if classification else None, provenance=_freeze(provenance))
openpilot/selfdrive/navigation/mapsd.py
+6 -1
View File
@@ -24,10 +24,12 @@ class MapsD:
self.route = np.empty((0, 2))
self.prefetched = False
self.retry_at = 0.0
self.route_generation = 0
def update(self, sm) -> tuple[np.ndarray, int]:
"""The map frame for the latest messages: black with a zero timestamp without a usable map, never stale."""
if sm.updated['navRoute']:
self.route_generation = int(sm.logMonoTime['navRoute'])
route = np.array([(c.longitude, c.latitude) for c in sm['navRoute'].coordinates], dtype=np.float64).reshape((-1, 2))
if not np.array_equal(route, self.route):
self.route = route
@@ -74,7 +76,10 @@ def main():
while True:
sm.update(0)
image, timestamp = mapsd.update(sm)
server.send(VisionStreamType.VISION_STREAM_MAP, image.tobytes(), rk.frame, timestamp, time.monotonic_ns())
# EOF is the exact route generation rendered, not completion time. An
# old-route render finishing after a new route cannot impersonate it.
generation = mapsd.route_generation if timestamp else 0
server.send(VisionStreamType.VISION_STREAM_MAP, image.tobytes(), rk.frame, timestamp, generation)
rk.keep_time()
finally:
mapsd.close()
openpilot/selfdrive/navigation/navd.py
+184 -39
View File
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
import copy
import os
import re
import time
@@ -15,7 +16,7 @@ from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.navigation.helpers import latest_gps_source
from openpilot.selfdrive.navigation.mapbox_next_turn import MapboxNextTurnClient, NextTurnError
from openpilot.selfdrive.navigation.route import along_path, local_meters
from openpilot.selfdrive.navigation.route import RouteTracker, along_path, cumulative_distance, local_meters, nearest_segment
REROUTE_DISTANCE = 25
MANEUVER_TRANSITION_THRESHOLD = 10
@@ -31,6 +32,8 @@ TURN_MAX_HEADING_ERROR = 35.
TURN_MAX_HEADING_UNCERTAINTY = 15.
TURN_RETRY_SECONDS = 5.
TURN_PASSED_DISTANCE = 10.
TURN_ENTRY_HEADING_DEG = 25.
TURN_EXIT_COMPLETION_M = 12.
def chosen_heading(now, llk, gps):
@@ -127,6 +130,27 @@ def approach_context(target, position, bearing_deg, horizontal_accuracy):
return invalid
def turn_route_progress(target, position, heading, entered=False):
"""Observed route progress only; no future pose or steering command."""
points = np.asarray(target.geometry, dtype=float)
local, point = local_meters(points, points[0]), local_meters(position, points[0])
index, fraction, cross = nearest_segment(local[:-1], local[1:], point)
distance = cumulative_distance(points)
progress = float(distance[index]+fraction*(distance[index+1]-distance[index]))
junction_distance = float(distance[target.junction_geometry_index])
sign = 1. if target.direction == 'right' else -1.
change = ((heading-target.incoming_bearing_deg+180.) % 360.-180.)*sign
exit_error = abs((heading-target.exit_bearing_deg+180.) % 360.-180.)
near = np.linalg.norm(point-local[target.junction_geometry_index]) <= 25.
entered = bool(entered or near and change >= TURN_ENTRY_HEADING_DEG and exit_error <= 65. and cross <= TURN_MAX_CROSS_TRACK)
exit_distance = min(TURN_EXIT_COMPLETION_M, .6*max(0., distance[-1]-junction_distance))
completed = bool(entered and progress >= junction_distance+exit_distance and exit_error <= TURN_MAX_HEADING_ERROR and cross <= TURN_MAX_CROSS_TRACK)
approach = approach_context(target, position, heading, 1.)
missed = bool(not entered and approach['distanceM'] is not None and approach['distanceM'] < -TURN_PASSED_DISTANCE)
return {'entered': entered, 'completed': completed, 'missed': missed,
'routeProgressM': progress, 'crossTrackM': cross, 'exitHeadingErrorDeg': exit_error}
class SignalTurnEngine:
"""Session-tagged, token-only discovery with one off-thread request at a time."""
def __init__(self, params, client_factory=None, executor=None, clock=None, wall_clock=None):
@@ -148,6 +172,9 @@ class SignalTurnEngine:
self.blocked = False
self.passed = False
self.signal_blocked = False
self.entered = False
self.retained = False
self.retired_key = None
def _invalidate(self):
self.generation += 1
@@ -160,6 +187,8 @@ class SignalTurnEngine:
self.retry_at = 0.
self.passed = False
self.signal_blocked = False
self.entered = False
self.retained = False
def _clear(self):
if self.key is not None:
@@ -173,7 +202,12 @@ class SignalTurnEngine:
def _publish(self, now, status, **values):
self.context = {'session': self.key[0], 'direction': self.key[1], 'status': status, 'updatedAt': now,
'distanceM': None, 'locationValid': False, 'headingValid': False, **values}
self.params.put('MapboxTurnContext', self.context, block=False)
# route_pending is an internal proposal. Exposing it between every ready
# update would intermittently clear valid model inputs. The RouteEngine
# publishes ready only after the route and instruction are coherent.
if status != 'route_pending':
# Preserve ordering with synchronous retirement/removal of this key.
self.params.put('MapboxTurnContext', self.context, block=True)
return self.context
def _consume(self):
@@ -204,23 +238,50 @@ class SignalTurnEngine:
self._consume()
return None
intent = self.params.get('MapboxTurnIntent')
if (not isinstance(intent, dict) or not isinstance(intent.get('session'), str) or
not 1 <= len(intent['session']) <= 128 or intent.get('direction') not in ('left', 'right')):
valid_intent = (isinstance(intent, dict) and isinstance(intent.get('session'), str) and
1 <= len(intent['session']) <= 128 and intent.get('direction') in ('left', 'right') and
intent.get('mode') in ('pending', 'turn'))
model = self.params.get('NavModelState')
supported = (isinstance(model, dict) and model.get('supported') is True and
isinstance(model.get('updatedAt'), (int, float)) and 0. <= now-model['updatedAt'] <= 1.)
try:
control_age = now-sm.logMonoTime['carControl']*1e-9
lateral = bool(sm.valid['carControl'] and sm['carControl'].latActive and 0. <= control_age <= TURN_GPS_MAX_AGE)
car = sm['carState']
canceled_after_entry = self.target is not None and self.key is not None and not car.leftBlinker and not car.rightBlinker
except (AttributeError, KeyError, TypeError):
lateral, canceled_after_entry = False, False
retained = bool(canceled_after_entry and supported and lateral and
(not valid_intent or (intent['session'], intent['direction']) == self.key))
if not valid_intent and not retained:
self._clear()
self._consume()
return None
if retained:
intent = {'session': self.key[0], 'direction': self.key[1], 'mode': 'turn', 'modelSupportsNavigation': True, 'lateralActive': True}
key = intent['session'], intent['direction']
if key != self.key:
self._invalidate()
self.key = key
self.retired_key = None
if destination_active != self.blocked:
self._invalidate()
self.blocked = destination_active
self._consume()
if destination_active:
self.retired_key = key
return self._publish(now, 'blocked', reason='explicit_destination')
if self.retired_key == key:
return self._publish(now, 'blocked', reason='manual_destination_preempted')
if not supported or intent.get('modelSupportsNavigation') is not True:
self.target = None
return self._publish(now, 'unavailable', reason='navigation_model_required')
if not lateral or intent.get('lateralActive') is not True:
self.target = None
return self._publish(now, 'unavailable', reason='lateral_inactive')
self.retained = retained
if self.passed:
return self._publish(now, 'passed', reason='junction_passed')
return self._publish(now, 'passed', reason='route_completed_or_missed')
if source is None:
return self._publish(now, 'unavailable', reason='gps_unavailable')
try:
@@ -266,7 +327,7 @@ class SignalTurnEngine:
return self._publish(now, 'unavailable', reason='heading_unavailable', **quality)
bearing = heading['headingDeg']
signal = 'left' if car.leftBlinker and not car.rightBlinker else 'right' if car.rightBlinker and not car.leftBlinker else None
if signal != key[1]:
if signal != key[1] and not retained:
if not self.signal_blocked:
self._invalidate()
self.signal_blocked = True
@@ -277,10 +338,19 @@ class SignalTurnEngine:
if not 0. <= age <= TURN_TARGET_TTL:
return self._publish(now, 'unavailable', reason='target_expired', **quality)
match = approach_context(self.target, position, bearing, accuracy)
if match['locationValid'] and match['distanceM'] < -TURN_PASSED_DISTANCE:
progress = turn_route_progress(self.target, position, bearing, self.entered)
if retained and not progress['entered']:
self._clear()
return None
self.entered = progress['entered']
if progress['completed'] or progress['missed']:
self.passed = True
status = 'passed' if self.passed else 'ready' if match['locationValid'] and match['headingValid'] else 'unavailable'
return self._publish(now, status, **quality, **match, junction=list(self.target.junction),
if self.entered:
match.update(locationValid=progress['crossTrackM'] <= TURN_MAX_CROSS_TRACK,
headingValid=progress['exitHeadingErrorDeg'] <= 65.)
status = 'passed' if self.passed else 'route_pending' if match['locationValid'] and match['headingValid'] else 'unavailable'
return self._publish(now, status, **quality, **match, **{k:v for k,v in progress.items() if k != 'crossTrackM'},
retainedAfterSignalCancel=retained, routeOwner='signal', junction=list(self.target.junction),
incomingBearingDeg=self.target.incoming_bearing_deg, exitBearingDeg=self.target.exit_bearing_deg,
requestId=self.target.validation_uuid, probeRequestId=self.target.probe_uuid,
targetAgeSeconds=age, name=self.target.name)
@@ -352,6 +422,11 @@ class RouteEngine:
self.update_counter = 0
self.signal_turn = SignalTurnEngine(self.params)
self.signal_preview_key = None
self.route_owner = None
self.route_key = None
self.route_generation = 0
self.route_tracker = None
self.maneuver_distances = None
self.last_position = None # (longitude, latitude)
self.last_bearing = None
@@ -373,33 +448,88 @@ class RouteEngine:
def update(self):
self.sm.update(0)
try:
self._update_route()
except Exception as error:
# API URLs can contain credentials: report only a fixed event and type.
cloudlog.event('navd.route_processing_failed', error_type=type(error).__name__, error=True)
self.clear_route()
self.send_route()
self.params.put('MapboxTurnContext', {'status': 'unavailable', 'updatedAt': time.monotonic(),
'reason': 'route_processing_failed', 'routeGeneration': 0}, block=True)
self.send_instruction()
def _update_route(self):
source = latest_gps_source(self.sm)
if source is not None:
gps = self.sm[source]
self.last_position, self.last_bearing = (gps.longitude, gps.latitude), gps.bearingDeg
route = self.route
destination_active = self.params.get('NavDestination') is not None
destination = self.params.get('NavDestination')
destination_active = destination is not None
context = self.signal_turn.update(self.sm, source, destination_active=destination_active)
preview_key = self.signal_turn.target.validation_uuid if context is not None and context['status'] == 'ready' else None
# A canceled explicit destination must not leave its route over a signal preview.
if not destination_active and self.nav_destination is not None:
self.clear_route()
legacy_tick = self.update_counter % self.update_frequency == 0
self.update_counter += 1
try:
previous = self.route
if destination_active:
requested_destination = destination['longitude'], destination['latitude']
if self.route_owner == 'signal' or (self.route is not None and self.nav_destination != requested_destination):
self.clear_route()
self.send_route()
if legacy_tick:
self.recompute_route(gps_ok=source is not None)
if not destination_active and context is not None:
self.send_signal_instruction(context)
elif legacy_tick or self.signal_preview_key is not None:
if self.route is not previous:
self.send_route()
if legacy_tick or self.route is not previous:
self.send_instruction()
except Exception:
cloudlog.exception("navd.failed_to_compute")
# mapsd dedupes repeats, so a periodic resend is how a restarted mapsd gets the route
if self.route is not route or preview_key != self.signal_preview_key or self.sm.frame % (ROUTE_RESEND_PERIOD*self.update_frequency) == 0:
if self.route is not None and self.nav_destination == requested_destination:
self.params.put('MapboxTurnContext', {'routeOwner': 'destination', 'status': 'ready',
'updatedAt': time.monotonic(), 'routeGeneration': self.route_generation,
'routePublishedAt': self.route_generation*1e-9,
'destination': {'longitude': self.nav_destination[0], 'latitude': self.nav_destination[1]},
'automaticStatus': 'blocked' if context is not None else None}, block=True)
else:
self.params.put('MapboxTurnContext', {'routeOwner': 'destination', 'status': 'searching',
'updatedAt': time.monotonic(), 'reason': 'destination_route_pending'}, block=True)
elif context is not None and context['status'] == 'route_pending':
target = self.signal_turn.target
key = context['session'], target.validation_uuid
if self.route_owner != 'signal' or self.route_key != key:
self.install_route([copy.deepcopy(dict(step)) for step in target.to_dict()['steps']],
tuple(target.geometry[-1]), owner='signal', key=key)
self.send_route()
self.send_instruction()
self.signal_turn._publish(context['updatedAt'], 'ready', **{k:v for k,v in context.items()
if k not in ('session', 'direction', 'status', 'updatedAt')}, routeGeneration=self.route_generation,
routePublishedAt=self.route_generation*1e-9)
else:
target = self.signal_turn.target
same_signal_route = (self.route_owner == 'signal' and context is not None and target is not None and
self.route_key == (context['session'], target.validation_uuid))
transient_unavailable = (same_signal_route and context['status'] == 'unavailable' and
context.get('reason') not in ('target_expired', 'navigation_model_required', 'lateral_inactive', 'route_processing_failed'))
# Keep the generation through a brief localization outage. Context and
# instructions remain invalid, so no stale conditioning is authorized.
if self.route_owner is not None and not transient_unavailable:
self.clear_route()
self.send_route()
if context is not None:
self.send_signal_instruction(context)
else:
self.params.remove('MapboxTurnContext')
self.send_instruction()
resend_period = self.update_frequency if self.route_owner == 'signal' else ROUTE_RESEND_PERIOD*self.update_frequency
if self.sm.frame % resend_period == 0:
self.send_route()
self.signal_preview_key = preview_key
def install_route(self, steps, destination, owner='destination', key=None):
self.route = steps
self.route_geometry = [np.asarray(step['geometry']['coordinates'], dtype=float).reshape((-1, 2)) for step in steps]
self.route_tracker = RouteTracker(np.concatenate(self.route_geometry))
starts = np.cumsum([0]+[len(path) for path in self.route_geometry[:-1]])
self.maneuver_distances = self.route_tracker.distance[starts]
self.step_idx, self.nav_destination = 0, destination
self.route_owner, self.route_key = owner, key
self.route_generation = 0
def recompute_route(self, gps_ok: bool):
if self.last_position is None:
@@ -463,9 +593,7 @@ class RouteEngine:
r = resp.json()
if len(r['routes']):
self.route = r['routes'][0]['legs'][0]['steps']
self.route_geometry = [np.array(step['geometry']['coordinates'], dtype=np.float64).reshape((-1, 2)) for step in self.route]
self.step_idx = 0
self.install_route(r['routes'][0]['legs'][0]['steps'], destination)
else:
cloudlog.warning("Got empty route response")
self.clear_route()
@@ -487,17 +615,26 @@ class RouteEngine:
assert self.route is not None and self.route_geometry is not None and self.last_position is not None
step = self.route[self.step_idx]
along_geometry, _ = along_path(self.route_geometry[self.step_idx], self.last_position)
geometry = self.route_geometry[self.step_idx]
along_geometry = (along_path(geometry, self.last_position)[0] if len(geometry) >= 2 else
float(np.linalg.norm(local_meters(self.last_position, geometry[0]))))
distance_to_maneuver_along_geometry = step['distance'] - along_geometry
# Banner instructions are for the following maneuver step, don't use empty last step
banner_step = step
if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
if not len(banner_step.get('bannerInstructions', [])) and self.step_idx == len(self.route) - 1:
banner_step = self.route[max(self.step_idx - 1, 0)]
# Current instruction
msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
set_banner(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
set_banner(msg.navInstruction, banner_step.get('bannerInstructions', []), distance_to_maneuver_along_geometry)
# Exact afbf2e5 upstream allManeuvers semantics: each step starts at its
# maneuver, relative to monotonic measured progress along the full route.
progress = self.route_tracker.project(*self.last_position)
msg.navInstruction.allManeuvers = [
{'distance': float(distance-progress), 'type': item['maneuver']['type'], 'modifier': item['maneuver'].get('modifier', '')}
for item, distance in zip(self.route, self.maneuver_distances, strict=True) if distance >= progress]
# Total remaining distance and time: the rest of this step, then every step after it
remaining = 1.0 - along_geometry / max(step['distance'], 1)
@@ -507,6 +644,12 @@ class RouteEngine:
self.pm.send('navInstruction', msg)
if self.route_owner == 'signal':
# The signal engine owns bounded completion/miss; never chase its outlet
# with destination rerouting or remove a user-owned NavDestination.
self.step_idx = min(int(np.searchsorted(self.maneuver_distances, progress, side='right'))-1, len(self.route)-1)
return
# Transition to next route segment
if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
if self.step_idx + 1 < len(self.route):
@@ -521,17 +664,16 @@ class RouteEngine:
self.clear_route()
def send_route(self):
geometry = self.route_geometry
if self.params.get('NavDestination') is None:
target, context = self.signal_turn.target, self.signal_turn.context
geometry = [np.asarray(target.geometry)] if target is not None and context is not None and context['status'] == 'ready' else None
coords = np.concatenate(geometry) if geometry is not None else np.empty((0, 2))
coords = np.concatenate(self.route_geometry) if self.route_geometry is not None else np.empty((0, 2))
msg = messaging.new_message('navRoute', valid=True)
if not self.route_generation:
self.route_generation = int(msg.logMonoTime)
msg.logMonoTime = self.route_generation
msg.navRoute.coordinates = [{'longitude': lon, 'latitude': lat} for lon, lat in coords.tolist()]
self.pm.send('navRoute', msg)
def send_signal_instruction(self, context):
msg = messaging.new_message('navInstruction', valid=True)
msg = messaging.new_message('navInstruction', valid=False)
if context['status'] == 'ready':
instruction = msg.navInstruction
instruction.maneuverPrimaryText = 'Signal turn'
@@ -544,8 +686,8 @@ class RouteEngine:
instruction.timeRemaining = max(0., context['distanceM'])/max(context['speedMps'], 1.)
instruction.showFull = True
else:
texts = {'searching': (f"Finding next {context['direction']}", 'Checking mapped streets'),
'unavailable': ('Map turn unavailable', 'Turn cue withheld'),
texts = {'searching': (f"Finding next {context['direction']}", 'Creating a short navigation route'),
'unavailable': ('Navigation unavailable', 'Route conditioning withheld'),
'blocked': ('Signal turn blocked', 'Destination navigation active'),
'passed': ('Turn passed', 'Signal again for another turn')}
primary, secondary = texts.get(context['status'], texts['unavailable'])
@@ -563,6 +705,9 @@ class RouteEngine:
self.route_geometry = None
self.step_idx = None
self.nav_destination = None
self.route_owner, self.route_key = None, None
self.route_generation = 0
self.route_tracker, self.maneuver_distances = None, None
def should_recompute(self):
if self.step_idx is None:
@@ -583,7 +728,7 @@ class RouteEngine:
def main():
pm = messaging.PubMaster(['navInstruction', 'navRoute'])
sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'carState', 'liveLocationKalman'])
sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'carState', 'carControl', 'liveLocationKalman'])
rk = Ratekeeper(10.)
route_engine = RouteEngine(sm, pm, update_frequency=10)
openpilot/selfdrive/navigation/tests/test_mapbox_next_turn.py
+65 -11
View File
@@ -44,6 +44,10 @@ def body(turn=False, road_class='street'):
'geometry': {'type': 'LineString', 'coordinates': points[1:]}, 'intersections': [at], 'bannerInstructions': []})
steps.append({'mode': 'driving', 'name': 'End', 'maneuver': {'type': 'arrive', 'location': list(endpoint)},
'geometry': {'type': 'LineString', 'coordinates': [points[-1]]}, 'intersections': [arrival]})
for step in steps:
step['distance'] = next_turn._length(step['geometry']['coordinates'])
step['duration'] = step['distance']/4.
step.setdefault('bannerInstructions', [])
return {'code': 'Ok', 'uuid': 'validation-id' if turn else 'probe-id',
'waypoints': [{'location': list(ORIGIN)}, {'location': list(endpoint)}],
'routes': [{'geometry': {'type': 'LineString', 'coordinates': points}, 'legs': [{'steps': steps}]}]}
@@ -231,13 +235,13 @@ class NextTurnClientTests(unittest.TestCase):
steps[-1]['maneuver']['location'] = list(endpoint)
steps[-1]['geometry']['coordinates'] = [list(endpoint)]
steps[-1]['intersections'][0].update(location=list(endpoint), geometry_index=3)
target, _ = self.discover(validated=value)
self.assertTrue(target.provenance['targetScopeTruncated'])
self.assertTrue(target.provenance['fullValidationResponsePreservedByHash'])
self.assertAlmostEqual(next_turn._length(target.geometry[target.junction_geometry_index:]), 20., places=5)
self.assertEqual(len(target.steps), 2)
self.assertEqual(target.road_class, 'street')
self.assertNotEqual(target.geometry[-1], tuple(endpoint))
parsed = next_turn._parse_route(value, ORIGIN, 0., endpoint, 15.)
probe = next_turn._parse_route(body(), ORIGIN, 0., PROBE, 100.)
candidate = next_turn._choose_branch(probe, 'right')
_, _, scoped = next_turn._validate_turn(parsed, candidate, 'right')
self.assertTrue(scoped['targetScopeTruncated'])
with self.assertRaisesRegex(next_turn.NextTurnError, 'incomplete_navigation_route'):
next_turn.coherent_route_steps(scoped)
def test_additional_turn_within_first_20m_of_exit_is_rejected(self):
value = body(True)
@@ -268,10 +272,13 @@ class NextTurnClientTests(unittest.TestCase):
steps[-1]['maneuver']['location'] = list(endpoint)
steps[-1]['geometry']['coordinates'] = [list(endpoint)]
steps[-1]['intersections'][0].update(location=list(endpoint), geometry_index=4)
target, _ = self.discover(validated=value)
self.assertTrue(target.provenance['targetScopeTruncated'])
self.assertAlmostEqual(next_turn._length(target.geometry[1:]), 20., places=5)
self.assertLess(next_turn.distance(target.junction, target.geometry[-1]), 20.)
parsed = next_turn._parse_route(value, ORIGIN, 0., endpoint, 15.)
probe = next_turn._parse_route(body(), ORIGIN, 0., PROBE, 100.)
_, _, scoped = next_turn._validate_turn(parsed, next_turn._choose_branch(probe, 'right'), 'right')
self.assertTrue(scoped['targetScopeTruncated'])
self.assertAlmostEqual(next_turn._length(scoped['points'][1:]), 20., places=5)
with self.assertRaisesRegex(next_turn.NextTurnError, 'incomplete_navigation_route'):
next_turn.coherent_route_steps(scoped)
def test_validation_cannot_choose_opposite_exit(self):
value = body(True)
@@ -279,6 +286,53 @@ class NextTurnClientTests(unittest.TestCase):
with self.assertRaisesRegex(next_turn.NextTurnError, 'target_mismatch'):
self.discover(validated=value)
def test_clipped_discovery_is_requeried_as_a_complete_actual_exit_route(self):
value = body(True)
later = next_turn.offset(JUNCTION, 90., 35.)
endpoint = next_turn.offset(later, 0., 5.)
value['waypoints'][-1]['location'] = list(endpoint)
points = [list(ORIGIN), list(JUNCTION), list(later), list(endpoint)]
route = value['routes'][0]
route['geometry']['coordinates'] = points
steps = route['legs'][0]['steps']
steps[1]['geometry']['coordinates'] = points[1:3]
extra = junction(location=later, index=2, out=0)
extra['in'] = 3
steps.insert(2, {'mode': 'driving', 'name': 'Later', 'maneuver': {'type': 'turn', 'location': list(later)},
'geometry': {'coordinates': points[2:]}, 'intersections': [extra]})
steps[-1]['maneuver']['location'] = list(endpoint)
steps[-1]['geometry']['coordinates'] = [list(endpoint)]
steps[-1]['intersections'][0].update(location=list(endpoint), geometry_index=3)
final = body(True)
actual_endpoint = next_turn.offset(JUNCTION, 90., 20.)
final['waypoints'][-1]['location'] = list(actual_endpoint)
final_route = final['routes'][0]
final_route['geometry']['coordinates'][-1] = list(actual_endpoint)
final_steps = final_route['legs'][0]['steps']
final_steps[1]['geometry']['coordinates'][-1] = list(actual_endpoint)
final_steps[1]['distance'], final_steps[1]['duration'] = 20., 5.
final_steps[-1]['geometry']['coordinates'] = [list(actual_endpoint)]
final_steps[-1]['maneuver']['location'] = list(actual_endpoint)
final_steps[-1]['intersections'][0]['location'] = list(actual_endpoint)
target, request = self.discover(validated=value, third=final)
self.assertEqual(request.call_count, 3)
self.assertEqual(target.provenance['requestCount'], 3)
self.assertTrue(target.provenance['coherentNavigationRoute'])
self.assertFalse(target.provenance['targetScopeTruncated'])
self.assertEqual(target.steps[-1]['maneuver']['type'], 'arrive')
self.assertAlmostEqual(target.steps[1]['distance'], 20.)
def test_navigation_route_rejects_missing_arrival_or_disconnected_steps(self):
route = body(True)['routes'][0]
steps = route['legs'][0]['steps']
with self.assertRaisesRegex(next_turn.NextTurnError, 'incomplete_navigation_route'):
next_turn.coherent_route_steps({'steps': steps[:-1]})
steps[1]['geometry']['coordinates'][0] = list(next_turn.offset(JUNCTION, 0., 10.))
steps[1]['distance'] = next_turn._length(steps[1]['geometry']['coordinates'])
with self.assertRaisesRegex(next_turn.NextTurnError, 'disconnected_navigation_route'):
next_turn.coherent_route_steps({'steps': steps})
def test_close_intersection_after_corner_uses_its_local_incoming_segment(self):
value = body(True)
close = next_turn.offset(JUNCTION, 90., 3.)
openpilot/selfdrive/navigation/tests/test_navd_next_turn.py
+32 -17
View File
@@ -27,7 +27,9 @@ def source(name):
class Params:
def __init__(self, enabled=True):
self.values = {'MapboxNextTurnEnabled': enabled, 'MapboxTurnIntent': {'session': 'one', 'direction': 'right'}}
self.values = {'MapboxNextTurnEnabled': enabled, 'MapboxTurnIntent':
{'session': 'one', 'direction': 'right', 'mode': 'turn', 'modelSupportsNavigation': True, 'lateralActive': True}}
self.values['NavModelState'] = {'updatedAt': 100., 'supported': True}
self.writes = []
def get(self, key, **kwargs):
@@ -64,7 +66,7 @@ class Executor:
def message(service, valid=True):
return SimpleNamespace(valid=valid, **{service: SimpleNamespace(coordinates=[])})
return SimpleNamespace(valid=valid, logMonoTime=100_000_000_001, **{service: SimpleNamespace(coordinates=[])})
def load_navd():
@@ -74,7 +76,9 @@ def load_navd():
'API_HOST': 'https://api.example.test', 'Api': Mock(), 'Params': Params, 'Ratekeeper': Mock(),
'cloudlog': Mock(), 'MapboxNextTurnClient': Mock(), 'NextTurnError': NextTurnError,
'latest_gps_source': lambda sm: 'gpsLocation' if sm.valid['gpsLocation'] else None,
'along_path': route['along_path'], 'local_meters': route['local_meters']}
'along_path': route['along_path'], 'local_meters': route['local_meters'],
'RouteTracker': route['RouteTracker'], 'cumulative_distance': route['cumulative_distance'],
'nearest_segment': route['nearest_segment']}
tree = ast.parse(source('navd.py'))
tree.body = [node for node in tree.body
if not (isinstance(node, ast.ImportFrom) and node.module.startswith('openpilot.'))
@@ -86,7 +90,7 @@ def load_navd():
class SM:
def __init__(self):
self.frame = 1
self.valid = dict.fromkeys(('gpsLocation', 'carState'), True)
self.valid = dict.fromkeys(('gpsLocation', 'carState', 'carControl'), True)
self.recv_time = dict.fromkeys(self.valid, 100.)
self.logMonoTime = dict.fromkeys(self.valid, 100_000_000_000)
self.gps = SimpleNamespace(hasFix=True, longitude=0., latitude=0., bearingDeg=0., bearingAccuracyDeg=3.,
@@ -94,17 +98,29 @@ class SM:
self.car = SimpleNamespace(vEgo=2., gearShifter='drive', leftBlinker=False, rightBlinker=True)
def __getitem__(self, key):
return self.car if key == 'carState' else self.gps
return SimpleNamespace(latActive=True) if key == 'carControl' else self.car if key == 'carState' else self.gps
def update(self, timeout):
pass
def target(direction='right', request='validation-one', discovered=100.):
return SimpleNamespace(direction=direction, discovered_at=discovered, approach_geometry=((0., 0.), (0., .001)),
result = SimpleNamespace(direction=direction, discovered_at=discovered, approach_geometry=((0., 0.), (0., .001)),
geometry=((0., 0.), (0., .001), (.0004, .001)), junction=(0., .001),
incoming_bearing_deg=0., exit_bearing_deg=90. if direction == 'right' else 270.,
validation_uuid=request, probe_uuid='probe-one', name='Example Street')
validation_uuid=request, probe_uuid='probe-one', name='Example Street', junction_geometry_index=1)
points = list(result.geometry)
result.steps = [
{'geometry': {'coordinates': points[:2]}, 'distance': 111., 'duration': 20.,
'maneuver': {'type': 'depart'}, 'bannerInstructions': [{'distanceAlongGeometry': 111.,
'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': direction}}]},
{'geometry': {'coordinates': points[1:]}, 'distance': 44., 'duration': 8.,
'maneuver': {'type': 'turn', 'modifier': direction}, 'bannerInstructions': []},
{'geometry': {'coordinates': [points[-1]]}, 'distance': 0., 'duration': 0.,
'maneuver': {'type': 'arrive'}, 'bannerInstructions': []}]
result.to_dict = lambda: {'steps': result.steps}
return result
class TestSignalTurnEngine(unittest.TestCase):
@@ -126,7 +142,7 @@ class TestSignalTurnEngine(unittest.TestCase):
def ready(self):
self.assertEqual(self.update()['status'], 'searching')
self.executor.jobs[-1][0].set_result(target())
self.assertEqual(self.update()['status'], 'ready')
self.assertEqual(self.update()['status'], 'route_pending')
def test_disabled_makes_no_token_client_worker_or_account_request(self):
self.params.values['MapboxNextTurnEnabled'] = False
@@ -158,7 +174,7 @@ class TestSignalTurnEngine(unittest.TestCase):
def test_opposite_signal_waits_for_single_worker_and_discards_old_result(self):
self.update()
self.params.values['MapboxTurnIntent'] = {'session': 'two', 'direction': 'left'}
self.params.values['MapboxTurnIntent'] = {'session': 'two', 'direction': 'left', 'mode': 'turn', 'modelSupportsNavigation': True, 'lateralActive': True}
self.sm.car.leftBlinker, self.sm.car.rightBlinker = True, False
self.assertEqual(self.update()['status'], 'searching')
self.assertEqual(len(self.executor.jobs), 1)
@@ -168,7 +184,7 @@ class TestSignalTurnEngine(unittest.TestCase):
self.assertEqual(len(self.executor.jobs), 2)
self.executor.jobs[1][0].set_result(target('left', 'validation-two'))
context = self.update()
self.assertEqual((context['session'], context['direction'], context['status']), ('two', 'left', 'ready'))
self.assertEqual((context['session'], context['direction'], context['status']), ('two', 'left', 'route_pending'))
def test_cancel_or_disable_retires_inflight_result(self):
for cancel in ('intent', 'disable'):
@@ -189,11 +205,8 @@ class TestSignalTurnEngine(unittest.TestCase):
self.assertEqual(self.update(destination=True)['status'], 'blocked')
self.assertEqual(self.executor.jobs, [])
self.client_factory.assert_not_called()
self.update(destination=False)
self.assertEqual(len(self.executor.jobs), 1)
self.update(destination=True)
self.executor.jobs[0][0].set_result(target())
self.assertEqual(self.update(destination=True)['reason'], 'explicit_destination')
self.assertEqual(self.update(destination=False)['reason'], 'manual_destination_preempted')
self.assertEqual(self.executor.jobs, [])
self.assertIsNone(self.engine.target)
def test_stale_fix_poor_accuracy_future_publication_and_reverse_block_discovery(self):
@@ -212,7 +225,7 @@ class TestSignalTurnEngine(unittest.TestCase):
self.ready()
self.sm.car.rightBlinker = False
context = self.update()
self.assertEqual(context['reason'], 'physical_signal_mismatch')
self.assertIsNone(context)
self.assertIsNone(self.engine.target)
self.assertEqual(len(self.executor.jobs), 1)
@@ -339,7 +352,9 @@ class TestNavigationIntegration(unittest.TestCase):
sm.frame += 1
engine.update()
instructions = [call.args[1] for call in pm.send.call_args_list if call.args[0] == 'navInstruction']
self.assertEqual(instructions[-1].navInstruction.maneuverPrimaryText, 'Signal turn')
self.assertEqual(instructions[-1].navInstruction.maneuverPrimaryText, 'Turn right')
self.assertTrue(instructions[-1].navInstruction.allManeuvers)
self.assertEqual(engine.route_owner, 'signal')
self.assertTrue(instructions[-1].valid)
self.assertNotIn('NavDestination', params.values)
params.values.pop('MapboxTurnIntent')
openpilot/selfdrive/navigation/tests/test_navigation.py
+3 -2
View File
@@ -264,9 +264,10 @@ class TestRouteEngine(unittest.TestCase):
self.engine.last_position, self.engine.last_bearing = (0, 0), 90
distance = 111.195 # Mapbox's length of 0.001 degrees at the equator
self.response = mapbox_response([
{'geometry': {'coordinates': [[0, 0], [0.0005, 0], [0.001, 0]]}, 'distance': distance, 'duration': 10,
{'maneuver': {'type': 'depart'}, 'geometry': {'coordinates': [[0, 0], [0.0005, 0], [0.001, 0]]}, 'distance': distance, 'duration': 10,
'bannerInstructions': [{'distanceAlongGeometry': 100, 'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': 'right'}}]},
{'geometry': {'coordinates': [[0.001, 0], [0.002, 0]]}, 'distance': distance, 'duration': 10, 'bannerInstructions': []},
{'maneuver': {'type': 'continue', 'modifier': 'straight'}, 'geometry': {'coordinates': [[0.001, 0], [0.002, 0]]},
'distance': distance, 'duration': 10, 'bannerInstructions': []},
])
def last_message(self, service):
openpilot/selfdrive/navigation/tests/test_signal_mini_route.py
+229
View File
@@ -0,0 +1,229 @@
"""Real route ownership/generation lifecycle; no HTTP or vehicle execution."""
from pathlib import Path
import ast
import importlib.util
from types import SimpleNamespace
import unittest
from unittest.mock import Mock, patch
import numpy as np
from test_navd_next_turn import Executor, Params, SM, load_navd, target
class SignalMiniRouteTests(unittest.TestCase):
def setUp(self):
self.navd, self.sm, self.params, self.pm, self.executor = load_navd(), SM(), Params(), Mock(), Executor()
self.now, self.wall = 100., 1_000_000.
self.token = patch.dict(self.navd.RouteEngine.__init__.__globals__, get_mapbox_token=Mock(return_value='pk.fixture.signature'))
self.token.start()
self.addCleanup(self.token.stop)
self.engine = self.navd.RouteEngine(self.sm, self.pm, self.params, update_frequency=10)
self.engine.signal_turn = self.navd.SignalTurnEngine(self.params, Mock(return_value=SimpleNamespace(discover=Mock())), self.executor,
clock=lambda: self.now, wall_clock=lambda: self.wall)
def fresh(self, longitude=0., latitude=0., heading=0.):
self.sm.gps.longitude, self.sm.gps.latitude, self.sm.gps.bearingDeg = longitude, latitude, heading
self.sm.gps.unixTimestampMillis = (self.wall-.001)*1000.
self.sm.logMonoTime.update({name: int(self.now*1e9)-1000 for name in self.sm.logMonoTime})
self.sm.recv_time.update(dict.fromkeys(self.sm.recv_time, self.now))
self.params.values['NavModelState'] = {'updatedAt': self.now, 'supported': True}
def ready(self):
self.engine.update()
self.executor.jobs[-1][0].set_result(target())
self.engine.update()
self.assertEqual(self.engine.route_owner, 'signal')
self.assertEqual(self.params.values['MapboxTurnContext']['status'], 'ready')
def enter_and_cancel(self):
self.params.values.pop('MapboxTurnIntent', None)
self.sm.car.leftBlinker = self.sm.car.rightBlinker = False
self.fresh(longitude=.000025, latitude=.001, heading=90.)
self.engine.update()
def test_unsupported_model_never_discovers_or_installs(self):
self.params.values['NavModelState']['supported'] = False
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['reason'], 'navigation_model_required')
self.assertEqual(self.executor.jobs, [])
self.assertIsNone(self.engine.route)
def test_delayed_context_write_cannot_resurrect_retired_route(self):
pending, persist = [], self.params.put
def deferred(key, value, block=False):
if block:
persist(key, value, block=True)
else:
pending.append((key, value))
self.params.put = deferred
engine = self.engine.signal_turn
engine.key = ('one', 'right')
engine._publish(self.now, 'ready', routeOwner='signal', routeGeneration=123, retainedAfterSignalCancel=True)
engine._clear()
for key, value in pending:
self.params.values[key] = value
self.assertNotIn('MapboxTurnContext', self.params.values)
def test_coherent_route_arrival_all_maneuvers_and_publish_order(self):
self.ready()
self.assertEqual(self.engine.route[-1]['maneuver']['type'], 'arrive')
self.assertNotIn('NavDestination', self.params.values)
calls = self.pm.send.call_args_list
route_index = max(i for i, call in enumerate(calls) if call.args[0] == 'navRoute')
instruction_index = max(i for i, call in enumerate(calls) if call.args[0] == 'navInstruction')
self.assertLess(route_index, instruction_index)
instruction = calls[instruction_index].args[1].navInstruction
self.assertTrue(any(item['type'] == 'turn' and item['modifier'] == 'right' for item in instruction.allManeuvers))
self.assertEqual(self.params.values['MapboxTurnContext']['routeGeneration'], self.engine.route_generation)
self.assertFalse(any(key == 'MapboxTurnContext' and value.get('status') == 'route_pending'
for key, value, _block in self.params.writes))
def test_pending_route_can_prepare_without_authorizing_navigation_inputs(self):
self.params.values['MapboxTurnIntent']['mode'] = 'pending'
self.ready()
path = Path(__file__).resolve().parents[3]/'sunnypilot/modeld_v2/navigation_input.py'
spec = importlib.util.spec_from_file_location('mini_route_model_guard', path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
guard = module.SignalRouteGuard()
arguments = {'now': self.now, 'enabled': True, 'lateral_active': True, 'left_blinker': False,
'right_blinker': True, 'intent': self.params.values['MapboxTurnIntent'],
'context': self.params.values['MapboxTurnContext']}
self.assertFalse(guard.check(**arguments)['ready'])
self.params.values['MapboxTurnIntent']['mode'] = 'turn'
self.assertTrue(guard.check(**arguments)['ready'])
def test_cancel_before_entry_clears_route_and_context(self):
self.ready()
self.params.values.pop('MapboxTurnIntent')
self.sm.car.rightBlinker = False
self.engine.update()
self.assertIsNone(self.engine.route)
self.assertNotIn('MapboxTurnContext', self.params.values)
def test_same_tick_first_entry_and_auto_cancel_retains_exact_generation(self):
self.ready()
generation = self.engine.route_generation
self.assertFalse(self.engine.signal_turn.entered)
self.enter_and_cancel()
context = self.params.values['MapboxTurnContext']
self.assertEqual(context['status'], 'ready')
self.assertTrue(context['retainedAfterSignalCancel'])
self.assertEqual(context['routeGeneration'], generation)
self.assertEqual(len(self.executor.jobs), 1)
def test_receiving_road_completion_retires_without_retarget(self):
self.ready()
self.enter_and_cancel()
self.fresh(longitude=.00015, latitude=.001, heading=90.)
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['status'], 'passed')
self.assertIsNone(self.engine.route)
self.engine.update()
self.assertEqual(len(self.executor.jobs), 1)
def test_missed_junction_retires_instead_of_rerouting_to_endpoint(self):
self.ready()
self.fresh(latitude=.00115, heading=0.)
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['status'], 'passed')
self.assertIsNone(self.engine.route)
self.engine.update()
self.assertEqual(len(self.executor.jobs), 1)
def test_transient_gps_loss_retains_geometry_generation_but_invalidates_inputs(self):
self.ready()
self.enter_and_cancel()
generation = self.engine.route_generation
self.now += .6
self.wall += .6
self.params.values['NavModelState']['updatedAt'] = self.now
self.sm.logMonoTime['carControl'] = int(self.now*1e9)-1000
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['status'], 'unavailable')
self.assertEqual(self.engine.route_generation, generation)
self.assertIsNotNone(self.engine.route)
self.fresh(longitude=.00003, latitude=.001, heading=90.)
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['status'], 'ready')
self.assertEqual(self.engine.route_generation, generation)
def test_manual_destination_switch_cannot_label_old_route_as_new_destination(self):
a, b = {'longitude': .002, 'latitude': 0.}, {'longitude': .003, 'latitude': 0.}
self.params.values['NavDestination'] = a
self.engine.install_route(target().steps, (.002, 0.))
self.engine.send_route()
self.engine.recompute_route = Mock()
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['destination'], a)
self.params.values['NavDestination'] = b
self.engine.update() # not a1Hz legacy recompute tick
context = self.params.values['MapboxTurnContext']
self.assertEqual(context['status'], 'searching')
self.assertIsNone(self.engine.route)
self.assertEqual(self.params.values['NavDestination'], b)
def test_signal_route_resends_at_one_hz_without_changing_generation(self):
self.ready()
generation = self.engine.route_generation
before = sum(call.args[0] == 'navRoute' for call in self.pm.send.call_args_list)
self.sm.frame = 10
self.engine.update()
routes = [call.args[1] for call in self.pm.send.call_args_list if call.args[0] == 'navRoute']
self.assertEqual(len(routes), before+1)
self.assertEqual(routes[-1].logMonoTime, generation)
def test_manual_destination_cancel_removes_its_ready_generation_context(self):
self.params.values['NavDestination'] = {'longitude': .002, 'latitude': 0.}
self.engine.install_route(target().steps, (.002, 0.))
self.engine.send_route()
self.engine.recompute_route = Mock()
self.engine.update()
self.params.values.pop('NavDestination')
self.params.values.pop('MapboxTurnIntent')
self.sm.car.rightBlinker = False
self.engine.update()
self.assertIsNone(self.engine.route)
self.assertNotIn('MapboxTurnContext', self.params.values)
def test_malformed_route_is_contained_without_exception_text_logging(self):
self.engine._update_route = Mock(side_effect=ValueError('secret-url-must-not-be-logged'))
self.engine.update()
self.assertEqual(self.params.values['MapboxTurnContext']['reason'], 'route_processing_failed')
text = str(self.navd.cloudlog.event.call_args_list)
self.assertNotIn('secret-url', text)
class MapGenerationTests(unittest.TestCase):
def test_map_eof_source_is_route_generation_not_render_completion(self):
path = Path(__file__).resolve().parents[1]/'mapsd.py'
tree = ast.parse(path.read_text())
server_send = [node for node in ast.walk(tree) if isinstance(node, ast.Call) and
isinstance(node.func, ast.Attribute) and node.func.attr == 'send'][0]
self.assertEqual(ast.unparse(server_send.args[-1]), 'generation')
# Also execute the actual map class: an unchanging route resend may not
# relabel a render with wall-clock completion time.
namespace = {'np': np, 'time': SimpleNamespace(monotonic=lambda: 100.), 'RETRY_SECONDS': 10,
'RouteTracker': Mock(), 'latest_gps_source': lambda sm: 'gps', 'MapRenderer': Mock(), 'PMTiles': Mock(),
'RemoteTiles': Mock(), 'SIZE': 256, 'cloudlog': Mock()}
cls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == 'MapsD')
exec(compile(ast.Module(body=[cls], type_ignores=[]), str(path), 'exec'), namespace)
maps = namespace['MapsD']()
maps.renderer = Mock()
maps.renderer.render.return_value = np.zeros((256, 256), np.uint8), True
sm = SimpleNamespace(updated={'navRoute': True}, logMonoTime={'navRoute': 123, 'gps': 99_900_000_000})
values = {'navRoute': SimpleNamespace(coordinates=[SimpleNamespace(longitude=0., latitude=0.),
SimpleNamespace(longitude=0., latitude=.001)]), 'gps': SimpleNamespace(longitude=0., latitude=0.)}
class Messages:
updated, logMonoTime = sm.updated, sm.logMonoTime
def __getitem__(self, key):
return values[key]
maps.update(Messages())
self.assertEqual(maps.route_generation, 123)
if __name__ == '__main__':
unittest.main()
openpilot/selfdrive/navigation/tests/test_signal_turn_status_ui.py
+35 -2
View File
@@ -54,7 +54,7 @@ def state(status='unavailable', destination=False):
'MapboxTurnContext': {'session': 'one', 'direction': 'right', 'status': status, 'updatedAt': 100.,
'reason': 'explicit_destination' if status == 'blocked' else 'gps_unavailable',
'locationValid': status == 'ready', 'headingValid': status == 'ready',
'distanceM': 8., 'name': 'Example Street'}}
'distanceM': 8., 'name': 'Example Street', 'routeGeneration': 10}}
params = SimpleNamespace(get=values.get, get_bool=lambda key: values.get(key) is True)
return SimpleNamespace(sm=SM(), params=params, nav_destination_set=destination, navigation_enabled=True, is_metric=True), values
@@ -78,7 +78,19 @@ class TestSignalTurnStatus(unittest.TestCase):
widget = module.NavigationPanel()
widget._render(module.rl.Rectangle(0, 0, 261, 400))
self.assertGreater(module.rl.draw_line_ex.call_count, 0)
self.assertIn('Signal turn', widget._eta_label.text)
self.assertIn('Route ready', widget._eta_label.text)
def test_route_preview_is_not_navigation_active_without_fresh_model_consumption(self):
ui_state, values = state('ready')
for value in (None, {'updatedAt': 99., 'supported': True, 'active': True, 'session': 'one'},
{'updatedAt': 100., 'supported': True, 'active': True, 'session': 'old'},
{'updatedAt': 100., 'supported': False, 'active': False, 'session': 'one'}):
values['NavModelState'] = value
widget = load_widget(ui_state).NavigationPanel()
self.assertEqual(widget.signal_status['primary'], 'Route ready')
values['NavModelState'] = {'updatedAt': 100., 'supported': True, 'active': True, 'session': 'one', 'routeGeneration': 10}
widget = load_widget(ui_state).NavigationPanel()
self.assertEqual(widget.signal_status['primary'], 'Navigation active')
def test_explicit_destination_keeps_normal_arrow_and_shows_blocked_footer(self):
ui_state, _ = state('blocked', destination=True)
@@ -88,6 +100,27 @@ class TestSignalTurnStatus(unittest.TestCase):
self.assertGreater(module.rl.draw_line_ex.call_count, 0)
self.assertIn('Destination navigation active', widget._eta_label.text)
def test_pending_candidate_does_not_claim_navigation_is_active(self):
ui_state, values = state('ready')
values['MapboxTurnIntent']['mode'] = 'pending'
self.assertEqual(load_widget(ui_state).NavigationPanel().signal_status['primary'], 'Turn candidate')
def test_actual_destination_owned_context_has_priority_without_a_signal_session(self):
ui_state, values = state('ready', destination=True)
values['MapboxTurnContext'] = {'status': 'ready', 'updatedAt': 100., 'routeOwner': 'destination', 'routeGeneration': 20}
widget = load_widget(ui_state).NavigationPanel()
self.assertEqual(widget.signal_status['secondary'], 'Destination navigation active')
def test_retained_exit_guidance_remains_visible_after_auto_cancel(self):
ui_state, values = state('ready')
ui_state.sm.car.rightBlinker = False
values['MapboxTurnIntent'] = None
values['MapboxTurnContext']['retainedAfterSignalCancel'] = True
widget = load_widget(ui_state).NavigationPanel()
self.assertTrue(widget.signal_turn_active)
ui_state.sm.car.leftBlinker = True
self.assertFalse(load_widget(ui_state).NavigationPanel().signal_turn_active)
def test_cancel_disable_hazards_stale_or_wrong_session_does_not_claim_ready(self):
for update in ('off', 'cancel', 'hazards', 'stale', 'different_session'):
ui_state, values = state('ready')
openpilot/selfdrive/ui/mici/onroad/navigation.py
+25 -7
View File
@@ -23,13 +23,20 @@ MAP_COLORS = np.repeat(np.round(np.arange(256) * 0.45).astype(np.uint8)[:, None]
MAP_COLORS[ROUTE_INTENSITY - 1:ROUTE_INTENSITY + 2] = (72, 235, 132)
def signal_turn_status(enabled, intent, context, now, left_blinker, right_blinker):
"""A display-only status; it cannot authorize a model cue or a vehicle action."""
def signal_turn_status(enabled, intent, context, now, left_blinker, right_blinker, model_state=None):
"""Distinguish a route lookup from actual navigation inputs consumed by the model."""
direction = 'left' if left_blinker and not right_blinker else 'right' if right_blinker and not left_blinker else None
if (direction is None and not left_blinker and not right_blinker and isinstance(context, dict) and
context.get('retainedAfterSignalCancel') is True and context.get('status') == 'ready'):
# Auto-cancel during an entered turn does not end its short exit guidance.
direction = context.get('direction')
intent = {'session': context.get('session'), 'direction': direction}
if (enabled is not True or not isinstance(intent, dict) or not intent.get('session') or
direction is None or intent.get('direction') != direction):
direction not in ('left', 'right') or intent.get('direction') != direction):
return None
status, reason = 'unavailable', 'navigation_unavailable'
if isinstance(context, dict) and context.get('routeOwner') == 'destination':
return {'state': 'blocked', 'primary': 'Signal turn blocked', 'secondary': 'Destination navigation active'}
if isinstance(context, dict) and context.get('session') == intent['session'] and context.get('direction') == direction:
updated = context.get('updatedAt')
if isinstance(updated, (int, float)) and not isinstance(updated, bool) and np.isfinite(updated) and 0. <= now-updated <= .35:
@@ -37,8 +44,17 @@ def signal_turn_status(enabled, intent, context, now, left_blinker, right_blinke
if status == 'ready' and context.get('locationValid') is True and context.get('headingValid') is True:
distance = context.get('distanceM')
if isinstance(distance, (int, float)) and not isinstance(distance, bool) and np.isfinite(distance):
return {'state': 'ready', 'primary': 'Signal turn', 'direction': direction, 'distanceM': distance,
'secondary': f"{direction.capitalize()} onto {context.get('name', '')}".strip()}
model_updated = model_state.get('updatedAt') if isinstance(model_state, dict) else None
active = (isinstance(model_updated, (int, float)) and not isinstance(model_updated, bool) and
np.isfinite(model_updated) and 0. <= now-model_updated <= .5 and
model_state.get('supported') is True and model_state.get('active') is True and
model_state.get('session') == intent['session'] and
isinstance(context.get('routeGeneration'), int) and context['routeGeneration'] > 0 and
model_state.get('routeGeneration') == context['routeGeneration'])
pending = intent.get('mode') == 'pending'
return {'state': 'ready', 'primary': 'Turn candidate' if pending else 'Navigation active' if active else 'Route ready',
'direction': direction, 'distanceM': distance,
'secondary': (f"{direction.capitalize()} onto {context.get('name', '')}".strip() if active or pending else 'Waiting for map and model')}
if status == 'searching':
return {'state': status, 'primary': f'Finding next {direction}', 'secondary': 'Checking mapped streets'}
if status == 'passed':
@@ -47,6 +63,7 @@ def signal_turn_status(enabled, intent, context, now, left_blinker, right_blinke
return {'state': status, 'primary': 'Signal turn blocked',
'secondary': 'Destination navigation active' if reason == 'explicit_destination' else 'Signal changed'}
explanations = {'missing_token': 'Map token missing', 'invalid_token': 'Map token unavailable',
'navigation_model_required': 'Select a navigation-capable model',
'gps_unavailable': 'GPS position unavailable', 'gps_or_car_state_invalid': 'GPS position unavailable',
'heading_unavailable': 'Vehicle heading unavailable', 'navigation_unavailable': 'Waiting for navigation',
'target_expired': 'Signal again to refresh target', 'no_matching_turn': 'No supported street turn ahead',
@@ -85,7 +102,8 @@ class NavigationPanel(Widget):
car = ui_state.sm['carState']
self._signal_status = signal_turn_status(
ui_state.params.get_bool('MapboxNextTurnEnabled'), ui_state.params.get('MapboxTurnIntent'),
ui_state.params.get('MapboxTurnContext'), now, car.leftBlinker, car.rightBlinker)
ui_state.params.get('MapboxTurnContext'), now, car.leftBlinker, car.rightBlinker,
ui_state.params.get('NavModelState'))
return self._signal_status
@property
@@ -175,7 +193,7 @@ class NavigationPanel(Widget):
elif self.routing or signal_ready:
seconds = max(0, instruction.timeRemaining)
remaining = max(0, instruction.distanceRemaining)
self._eta_label.set_text(f"Signal turn: {signal['secondary']}" if signal is not None else
self._eta_label.set_text(f"{signal['primary']}: {signal['secondary']}" if signal is not None else
f'{max(1, round(seconds / 60))} min / {format_distance(remaining)}')
self._eta_label.render(rl.Rectangle(x+9, y+rect.height-45, rect.width-18, 40))
openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py
+5 -4
View File
@@ -96,9 +96,11 @@ class ModelsLayout(Widget):
"This prevents situations (like at red lights) where the car might plan the wrong turn direction."),
param="LaneTurnDesire")
self.map_turn_toggle = toggle_item_sp(tr("Mapbox Signal Turns (Experimental)"),
tr("Your signal selects the next mapped street and times a turn cue. Requires an internet connection and a Mapbox token. " +
"Also enables turn desires. Turn off to restore the usual turn-desire timing."),
self.map_turn_toggle = toggle_item_sp(tr("Signal Navigation (Experimental)"),
tr("Signals can prepare a short street route while normal lane-change checks remain available. " +
"The first committed maneuver keeps its role until the signal is canceled. " +
"Requires a navigation-capable model, internet and a Mapbox token. " +
"Early fast turn signals and slow lane changes can be ambiguous; this is experimental."),
callback=self._enable_map_turns, enabled=lambda: ui_state.is_offroad() and ui_state.active_runner_tinygrad,
param="MapboxNextTurnEnabled")
@@ -123,7 +125,6 @@ class ModelsLayout(Widget):
def _enable_map_turns(self, enabled):
if enabled:
ui_state.params.put_bool("LaneTurnDesire", True)
ui_state.params.put_bool("NavigationEnabled", True)
def _update_lagd_description(self, lagd_toggle: bool):
openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py
+5 -4
View File
@@ -99,10 +99,12 @@ class ModelsLayoutMici(NavScroller):
description=tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when " +
"lane lines are clear. Yields around obstacles. Not available with the Default model."))
self.map_turn_toggle = BigParamControl(tr("mapbox signal turns (experimental)"), "MapboxNextTurnEnabled",
self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled",
toggle_callback=self._enable_map_turns,
description=tr("Your signal selects the next mapped street and times a turn cue. Requires an internet connection and a Mapbox token. " +
"Also enables turn desires. Turn off to restore the usual turn-desire timing."))
description=tr("Signals can prepare a short street route while normal lane-change checks remain available. " +
"The first committed maneuver keeps its role until the signal is canceled. " +
"Requires a navigation-capable model, internet and a Mapbox token. " +
"Early fast turn signals and slow lane changes can be ambiguous; this is experimental."))
self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn,
self.blendful_toggle, self.map_turn_toggle]
@@ -110,7 +112,6 @@ class ModelsLayoutMici(NavScroller):
def _enable_map_turns(self, enabled):
if enabled:
ui_state.params.put_bool("LaneTurnDesire", True)
ui_state.params.put_bool("NavigationEnabled", True)
@property
openpilot/sunnypilot/modeld_v2/modeld.py
+56 -2
View File
@@ -49,6 +49,8 @@ from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
from openpilot.sunnypilot.modeld_v2.helpers import load_model
from openpilot.sunnypilot.modeld_v2.frame_fusion import fused_with_warp
from openpilot.sunnypilot.modeld_v2.model_adapters import get_model_adapter
from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
destination_route_context, navigation_capability)
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
@@ -98,6 +100,7 @@ class ModelState(ModelStateBase):
else:
model_bundle = get_active_bundle(chestnut=chestnut)
self.generation = model_bundle.generation if model_bundle is not None else None
self.model_name = model_bundle.displayName if model_bundle is not None else ''
overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {}
self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
@@ -125,6 +128,7 @@ class ModelState(ModelStateBase):
self._combined_model_type = self.adapter._combined_model_type
self._vision_input_names = self.adapter._vision_input_names
self.numpy_inputs = self.adapter.numpy_inputs
self.navigation_supported, self.navigation_capability_reason = navigation_capability(self.numpy_inputs, bundle, native=self.adapter.is_native)
self._policy_keys = self.adapter._policy_keys
self._has_on_policy = self.adapter._has_on_policy
self._desire_key = self.adapter._desire_key
@@ -174,6 +178,9 @@ class ModelState(ModelStateBase):
for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
if key in self.numpy_inputs and key in inputs:
self.numpy_inputs[key][:] = inputs[key]
for key in ('nav_map', 'navigation'):
if key in self.numpy_inputs:
self.numpy_inputs[key][:] = inputs.get(key, 0)
if self.adapter.is_native:
for i, key in enumerate(self._vision_input_names):
@@ -270,6 +277,8 @@ def main(demo=False):
AMDDevice.wait_timeout_ms = 3000
params = Params()
navigation_state = NavigationStatePublisher(params)
navigation_state.publish(now=time.monotonic(), supported=False, active=False, reason='model_loading', force=True)
params.put_bool("ChestnutLoading", CHESTNUT)
params.remove("ChestnutActive")
@@ -331,6 +340,9 @@ def main(demo=False):
params.put_bool("ChestnutLoading", False)
assert model is not None
cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
navigation_state.publish(now=time.monotonic(), supported=model.navigation_supported, active=False,
reason='waiting_for_navigation' if model.navigation_supported else model.navigation_capability_reason,
model_name=model.model_name, force=True)
config_realtime_process(7, 54)
@@ -338,7 +350,11 @@ def main(demo=False):
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
pm = PubMaster(pub_socks)
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState",
"carControl", "controlsState", "lateralDelay"])
"carControl", "controlsState", "lateralDelay", "navInstruction"])
vipc_client_map = VisionIpcClient("mapsd", VisionStreamType.VISION_STREAM_MAP, True)
navigation_buffer = NavigationInputBuffer()
signal_route_guard = SignalRouteGuard()
publish_state = PublishState()
chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
@@ -463,6 +479,33 @@ def main(demo=False):
if 'action_t' in model.numpy_inputs:
inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
navigation_route = {}
navigation_reason = model.navigation_capability_reason
if model.navigation_supported:
destination, context = params.get('NavDestination'), params.get('MapboxTurnContext')
intent = params.get('MapboxTurnIntent')
if vipc_client_map.is_connected() or vipc_client_map.connect(False):
if (buf_map := vipc_client_map.recv(0)) is not None:
navigation_buffer.observe_map(buf_map.data, buf_map.width, buf_map.height,
timestamp_ns=vipc_client_map.timestamp_sof, received_at=time.monotonic(),
route_generation=vipc_client_map.timestamp_eof)
now = time.monotonic()
if destination is not None:
signal_route_guard.reset()
navigation_route = destination_route_context(destination, context, now)
else:
navigation_route = signal_route_guard.check(
now=now, enabled=params.get_bool('MapboxNextTurnEnabled'), lateral_active=sm['carControl'].latActive,
left_blinker=sm['carState'].leftBlinker, right_blinker=sm['carState'].rightBlinker,
intent=intent, context=context,
)
inputs.update(navigation_buffer.prepare(
now=now, enabled=params.get_bool('NavigationEnabled') and navigation_route['ready'], instruction=sm['navInstruction'],
instruction_valid=sm.valid['navInstruction'], instruction_received_at=sm.recv_time['navInstruction'], speed=v_ego,
instruction_published_ns=sm.logMonoTime['navInstruction'], expected_route_generation=navigation_route.get('routeGeneration'),
))
navigation_reason = navigation_buffer.reason if navigation_route['ready'] else navigation_route['reason']
mt1 = time.perf_counter()
try:
send_chestnut = (chestnut_state is not None and
@@ -475,6 +518,9 @@ def main(demo=False):
params.put_bool("ChestnutActive", False)
assert small_model is not None
model = small_model
signal_route_guard.reset()
navigation_state.publish(now=time.monotonic(), supported=model.navigation_supported, active=False,
reason=model.navigation_capability_reason, model_name=model.model_name, force=True)
if chestnut_state is not None:
chestnut_state.big = False
run_count = 0
@@ -482,6 +528,14 @@ def main(demo=False):
mt2 = time.perf_counter()
model_execution_time = mt2 - mt1
navigation_active = model.navigation_supported and model_output is not None and navigation_reason == 'ready'
if navigation_active and navigation_route.get('session') is not None:
signal_route_guard.consumed(navigation_route['session'], navigation_route['routeGeneration'])
navigation_state.publish(now=time.monotonic(), supported=model.navigation_supported, active=navigation_active,
reason=navigation_reason if model.navigation_supported else model.navigation_capability_reason,
model_name=model.model_name, session=navigation_route.get('session'),
route_generation=navigation_route.get('routeGeneration'))
if model_output is not None:
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
@@ -506,7 +560,7 @@ def main(demo=False):
left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge,
desire_prediction=modelv2_send.modelV2.meta.desirePrediction, model_position=modelv2_send.modelV2.position,
map_turn_supported=True)
map_turn_supported=model.navigation_supported)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
mdv2sp_send.valid = modelv2_send.valid
openpilot/sunnypilot/modeld_v2/navigation_input.py
+236
View File
@@ -0,0 +1,236 @@
"""Inputs for comma's pinned nav-model-integration driving model.
The map raster and maneuver encoding follow afbf2e547e9319e2eb9f515e1e4330af63705cec.
They condition the trained driving network; they do not issue a steering command
or fabricate a turn desire. Missing/stale context clears both conditioning inputs.
"""
import math
from collections.abc import Mapping
import numpy as np
NAV_MAP_SIZE = 256
NAVIGATION_SIZE = 47
MAP_MAX_AGE = 0.5
INSTRUCTION_MAX_AGE = 2.0
VALHALLA_SIDED_TYPES = {
'depart': (1, 2, 3), 'arrive': (4, 5, 6),
'on ramp': (17, 18, 19), 'off ramp': (20, 20, 21), 'fork': (22, 23, 24),
'roundabout': (26, 26, 26), 'rotary': (26, 26, 26), 'roundabout turn': (26, 26, 26),
'exit roundabout': (27, 27, 27), 'exit rotary': (27, 27, 27), 'notification': (28, 28, 28),
}
VALHALLA_TURN_TYPES = {'straight': 8, 'slight right': 9, 'right': 10, 'sharp right': 11, 'uturn': 13,
'sharp left': 14, 'left': 15, 'slight left': 16}
def _field(value, name, default=None):
return value.get(name, default) if isinstance(value, Mapping) else getattr(value, name, default)
def valhalla_type(maneuver_type: str, modifier: str) -> int | None:
if maneuver_type == 'merge':
return None
if maneuver_type in VALHALLA_SIDED_TYPES:
straight, right, left = VALHALLA_SIDED_TYPES[maneuver_type]
return right if 'right' in modifier else left if 'left' in modifier else straight
return VALHALLA_TURN_TYPES.get(modifier, 8)
def navigation_input(maneuvers, traveled: float = 0.) -> np.ndarray:
"""The next non-depart/arrive Valhalla command plus distance in100m units."""
result = np.zeros(NAVIGATION_SIZE, dtype=np.float32)
if not math.isfinite(traveled) or traveled < 0:
return result
for maneuver in maneuvers:
maneuver_type, modifier = _field(maneuver, 'type', ''), _field(maneuver, 'modifier', '')
distance = _field(maneuver, 'distance')
if (not isinstance(maneuver_type, str) or not isinstance(modifier, str) or
isinstance(distance, bool) or not isinstance(distance, (int, float, np.floating)) or not math.isfinite(distance)):
continue
command, remaining = valhalla_type(maneuver_type, modifier), distance - traveled
if command is not None and command > 6 and remaining >= 0:
result[command], result[-1] = 1, remaining / 100
break
return result
def supports_navigation(numpy_inputs) -> bool:
"""Fail closed on CTV3/small fallback or an incompatible nav tensor contract."""
expected = {'nav_map': (1, 1, NAV_MAP_SIZE, NAV_MAP_SIZE), 'navigation': (1, NAVIGATION_SIZE)}
return all(name in numpy_inputs and numpy_inputs[name].shape == shape and numpy_inputs[name].dtype == np.float32
for name, shape in expected.items())
def navigation_capability(numpy_inputs, bundle, *, native: bool):
if not supports_navigation(numpy_inputs):
return False, 'model_unsupported'
if bundle is None:
return False, 'model_bundle_required'
if not native or bundle.generation != 12 or not bundle.is20hz:
return False, 'model_bundle_incompatible'
return True, 'supported'
def destination_route_context(destination, context, now):
"""An explicit destination must own the exact map generation, including at boot."""
missing = {'ready': False, 'reason': 'destination_route_unavailable'}
if not isinstance(destination, dict) or not isinstance(context, dict):
return missing
snapshot = context.get('destination')
updated, generation = context.get('updatedAt'), context.get('routeGeneration')
if (context.get('routeOwner') != 'destination' or context.get('status') != 'ready' or
not isinstance(snapshot, dict) or isinstance(updated, bool) or not isinstance(updated, (int, float)) or
not math.isfinite(updated) or not 0 <= now - updated <= .35 or
isinstance(generation, bool) or not isinstance(generation, int) or generation <= 0):
return missing
for name in ('latitude', 'longitude'):
value = destination.get(name)
if isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value) or snapshot.get(name) != value:
return missing
return {'ready': True, 'reason': 'ready', 'routeGeneration': generation}
class NavigationInputBuffer:
"""Latest real map frame with bounded capture and instruction ages."""
def __init__(self):
self.map = None
self.map_received_at = 0.
self.map_captured_at = 0.
self.route_generation = None
self.reason = 'map_missing'
def observe_map(self, data, width: int, height: int, *, timestamp_ns: int, received_at: float,
route_generation: int | None = None) -> bool:
# A newly received invalid/black-with-zero-timestamp frame must invalidate
# the preceding map rather than make its old geometry appear fresh.
self.map = None
self.route_generation = None
self.reason = 'map_invalid'
if (width != NAV_MAP_SIZE or height != NAV_MAP_SIZE or timestamp_ns <= 0 or
not math.isfinite(received_at) or received_at < 0):
return False
captured_at = timestamp_ns / 1e9
if not 0 <= received_at - captured_at <= MAP_MAX_AGE:
return False
try:
pixels = np.frombuffer(data, dtype=np.uint8, count=NAV_MAP_SIZE * NAV_MAP_SIZE)
except (TypeError, ValueError):
return False
# The verified nav ONNX expects float32 raw raster values0..255, not a
# normalized camera image. Assignment in comma's runner performs this cast.
self.map = pixels.astype(np.float32).reshape(1, 1, NAV_MAP_SIZE, NAV_MAP_SIZE)
self.map_received_at, self.map_captured_at = received_at, captured_at
self.route_generation = route_generation
self.reason = 'ready'
return True
def prepare(self, *, now: float, enabled: bool, instruction, instruction_valid: bool,
instruction_received_at: float, speed: float, instruction_published_ns: int | None = None,
expected_route_generation: int | None = None):
zero = {'nav_map': np.zeros((1, 1, NAV_MAP_SIZE, NAV_MAP_SIZE), dtype=np.float32),
'navigation': np.zeros((1, NAVIGATION_SIZE), dtype=np.float32)}
if not enabled:
self.reason = 'disabled'
return zero
if self.map is None:
self.reason = 'map_missing'
return zero
if not all(math.isfinite(value) for value in (now, instruction_received_at, speed)):
self.reason = 'invalid_time_or_speed'
return zero
if not (0 <= now - self.map_received_at < MAP_MAX_AGE and 0 <= now - self.map_captured_at < MAP_MAX_AGE):
self.reason = 'map_stale'
return zero
if (not isinstance(self.route_generation, int) or self.route_generation <= 0 or
not isinstance(instruction_published_ns, int) or instruction_published_ns < self.route_generation or
(expected_route_generation is not None and self.route_generation != expected_route_generation)):
self.reason = 'route_generation_mismatch'
return zero
elapsed = now - instruction_received_at
if not instruction_valid or not 0 <= elapsed < INSTRUCTION_MAX_AGE:
self.reason = 'instruction_stale'
return zero
vector = navigation_input(_field(instruction, 'allManeuvers', []), max(speed, 0.) * elapsed)
self.reason = 'ready'
return {'nav_map': self.map, 'navigation': vector.reshape(1, NAVIGATION_SIZE)}
class SignalRouteGuard:
"""Bind automatic inputs to the current signal and exact rendered route.
The sole remembered identifier is a route this model actually consumed. It
permits the router's explicitly bounded outgoing continuation after normal
signal auto-cancel; a first-time retained route or another session is rejected.
No timing, steering, maneuver selection or route-completion policy lives here.
"""
def __init__(self):
self.last_consumed = None
def reset(self):
self.last_consumed = None
def consumed(self, session, route_generation):
self.last_consumed = (session, route_generation)
def check(self, *, now, enabled, lateral_active, left_blinker, right_blinker, intent, context):
def unavailable(reason, *, retire=False):
if retire:
self.reset()
return {'ready': False, 'reason': reason}
if not enabled or not lateral_active:
return unavailable('signal_navigation_disabled', retire=True)
if isinstance(intent, dict) and self.last_consumed is not None and intent.get('session') != self.last_consumed[0]:
self.reset()
if not isinstance(context, dict):
return unavailable('route_context_missing')
updated = context.get('updatedAt')
generation, session = context.get('routeGeneration'), context.get('session')
direction = context.get('direction')
if context.get('status') in ('passed', 'completed', 'arrived', 'cancelled', 'idle', 'disabled'):
return unavailable('route_context_unavailable', retire=True)
if self.last_consumed is not None and ((session is not None and session != self.last_consumed[0]) or
(generation is not None and generation != self.last_consumed[1])):
self.reset()
if (context.get('status') != 'ready' or context.get('routeOwner') != 'signal' or
context.get('locationValid') is not True or context.get('headingValid') is not True or
isinstance(updated, bool) or not isinstance(updated, (int, float)) or not math.isfinite(updated) or
not 0 <= now - updated <= .35 or isinstance(generation, bool) or not isinstance(generation, int) or generation <= 0 or
not isinstance(session, str) or not session or direction not in ('left', 'right')):
return unavailable('route_context_unavailable')
physical_direction = 'left' if left_blinker and not right_blinker else 'right' if right_blinker and not left_blinker else None
matches = (isinstance(intent, dict) and intent.get('session') == session and intent.get('direction') == direction and
intent.get('mode') == 'turn' and intent.get('modelSupportsNavigation') is True and intent.get('lateralActive') is True)
held_signal = matches and physical_direction == direction
retained = (not left_blinker and not right_blinker and intent is None and
context.get('retainedAfterSignalCancel') is True and self.last_consumed == (session, generation))
if not (held_signal or retained):
# The camera loop can see auto-cancel before navd's next10Hz decision.
# With neither indicator, hold only the identity (never the inputs) until
# the router explicitly retains or cancels that same owned route.
return unavailable('signal_session_mismatch', retire=bool(left_blinker or right_blinker))
return {'ready': True, 'reason': 'ready', 'session': session, 'routeGeneration': generation}
class NavigationStatePublisher:
"""Runtime capability and consumed-input status, without a control claim."""
def __init__(self, params):
self.params = params
self.last_published = float('-inf')
def publish(self, *, now: float, supported: bool, active: bool, reason: str, model_name: str = '', force: bool = False,
session=None, route_generation=None):
if not force and now - self.last_published < .1:
return False
value = {'updatedAt': now, 'supported': bool(supported), 'active': bool(supported and active), 'reason': reason}
if model_name:
value['modelName'] = model_name
if session is not None:
value.update(session=session, routeGeneration=route_generation)
self.params.put('NavModelState', value)
self.last_published = now
return True
openpilot/sunnypilot/modeld_v2/tests/test_navigation_input.py
+307
View File
@@ -0,0 +1,307 @@
"""Navigation conditioning contracts without camera/GPU/process dependencies."""
import ast
from collections.abc import Callable
import importlib.util
import math
from pathlib import Path
from types import SimpleNamespace
import unittest
import numpy as np
SOURCE = Path(__file__).resolve().parents[1]
spec = importlib.util.spec_from_file_location('nav_inputs_under_test', SOURCE / 'navigation_input.py')
nav = importlib.util.module_from_spec(spec)
spec.loader.exec_module(nav)
def maneuver(kind='turn', modifier='right', distance=50.):
return SimpleNamespace(type=kind, modifier=modifier, distance=distance)
class NavigationEncodingTests(unittest.TestCase):
def test_right_left_and_distance_use_training_encoding(self):
for modifier, index in (('right', 10), ('left', 15)):
result = nav.navigation_input([maneuver(modifier=modifier, distance=50.)], traveled=8.)
self.assertEqual(result.dtype, np.float32)
self.assertEqual(result.shape, (47,))
self.assertEqual(result[index], 1.)
self.assertAlmostEqual(float(result[-1]), .42)
self.assertEqual(np.count_nonzero(result), 2)
def test_depart_arrive_merge_and_passed_steps_do_not_mask_next_turn(self):
steps = [maneuver('depart'), maneuver('merge'), maneuver(distance=2.), maneuver(modifier='left', distance=90.), maneuver('arrive')]
result = nav.navigation_input(steps, traveled=5.)
self.assertEqual(result[15], 1.)
self.assertAlmostEqual(float(result[-1]), .85)
def test_first_eligible_step_preserves_order_and_roundabout_type(self):
result = nav.navigation_input([maneuver('roundabout', '', 100.), maneuver(modifier='left', distance=120.)])
self.assertEqual(result[26], 1.)
self.assertEqual(result[15], 0.)
def test_nonfinite_distance_cannot_enter_model(self):
result = nav.navigation_input([maneuver(distance=float('inf')), maneuver(distance=float('nan'))])
self.assertTrue(np.array_equal(result, np.zeros(47, np.float32)))
self.assertFalse(nav.navigation_input([maneuver()], traveled=-1).any())
def test_small_and_ctv3_are_not_navigation_models(self):
good = {'nav_map': np.zeros((1, 1, 256, 256), np.float32), 'navigation': np.zeros((1, 47), np.float32)}
self.assertTrue(nav.supports_navigation(good))
self.assertFalse(nav.supports_navigation({}))
self.assertFalse(nav.supports_navigation({'navigation': good['navigation']}))
self.assertFalse(nav.supports_navigation({**good, 'nav_map': good['nav_map'].astype(np.uint8)}))
self.assertFalse(nav.supports_navigation({**good, 'navigation': np.zeros((1, 46), np.float32)}))
def test_navigation_needs_a_compatible_real_bundle_not_an_override(self):
inputs = {'nav_map': np.zeros((1, 1, 256, 256), np.float32), 'navigation': np.zeros((1, 47), np.float32)}
self.assertEqual(nav.navigation_capability(inputs, None, native=True), (False, 'model_bundle_required'))
bundle = SimpleNamespace(generation=12, is20hz=True)
self.assertEqual(nav.navigation_capability(inputs, bundle, native=True), (True, 'supported'))
self.assertFalse(nav.navigation_capability(inputs, SimpleNamespace(generation=9, is20hz=True), native=True)[0])
self.assertFalse(nav.navigation_capability(inputs, bundle, native=False)[0])
class NavigationFreshnessTests(unittest.TestCase):
def setUp(self):
self.buffer = nav.NavigationInputBuffer()
self.pixels = np.zeros((256, 256), np.uint8)
self.pixels[5, :3] = [0, 128, 255]
self.instruction = SimpleNamespace(allManeuvers=[maneuver(distance=50.)])
def observe(self, *, t=10., timestamp_ns=10_000_000_000):
return self.buffer.observe_map(self.pixels, 256, 256, timestamp_ns=timestamp_ns, received_at=t, route_generation=9_000_000_000)
def prepare(self, **changes):
arguments = {'now': 10.1, 'enabled': True, 'instruction': self.instruction, 'instruction_valid': True,
'instruction_received_at': 10., 'speed': 10., 'instruction_published_ns': 10_000_000_000,
'expected_route_generation': 9_000_000_000}
return self.buffer.prepare(**(arguments | changes))
def assert_empty(self, result):
self.assertFalse(result['nav_map'].any())
self.assertFalse(result['navigation'].any())
def test_real_raster_scale_and_elapsed_distance_are_preserved(self):
self.assertTrue(self.observe())
result = self.prepare()
self.assertEqual(result['nav_map'].dtype, np.float32)
np.testing.assert_array_equal(result['nav_map'][0, 0, 5, :3], [0., 128., 255.])
self.assertAlmostEqual(float(result['navigation'][0, -1]), .49)
self.assertEqual(self.buffer.reason, 'ready')
def test_missing_stale_disabled_or_invalid_context_clears_both_inputs(self):
self.assert_empty(self.prepare())
self.observe()
for changes in ({'enabled': False}, {'now': 10.51}, {'instruction_valid': False},
{'instruction_received_at': 8.}, {'instruction_received_at': 10.2}, {'speed': float('nan')}):
self.assert_empty(self.prepare(**changes))
def test_new_invalid_map_cannot_refresh_or_keep_previous_geometry(self):
self.observe()
self.assertTrue(self.prepare()['nav_map'].any())
self.assertFalse(self.observe(timestamp_ns=0))
self.assert_empty(self.prepare())
def test_old_capture_is_rejected_even_if_just_received(self):
self.assertFalse(self.observe(t=11., timestamp_ns=10_000_000_000))
self.assert_empty(self.prepare(now=11.))
self.assertFalse(self.observe(t=10., timestamp_ns=11_000_000_000))
def test_wrong_sized_or_truncated_raster_is_rejected(self):
self.assertFalse(self.buffer.observe_map(b'', 256, 256, timestamp_ns=10_000_000_000, received_at=10.))
self.assertFalse(self.buffer.observe_map(self.pixels, 128, 256, timestamp_ns=10_000_000_000, received_at=10.))
def test_old_route_map_or_pre_route_instruction_cannot_condition_model(self):
self.observe()
self.assert_empty(self.prepare(expected_route_generation=9_500_000_000))
self.assertEqual(self.buffer.reason, 'route_generation_mismatch')
self.assert_empty(self.prepare(instruction_published_ns=8_999_999_999))
self.assertTrue(self.prepare()['nav_map'].any())
class SignalRouteGuardTests(unittest.TestCase):
def setUp(self):
self.guard = nav.SignalRouteGuard()
self.intent = {'session': 'signal1', 'direction': 'right', 'mode': 'turn', 'modelSupportsNavigation': True, 'lateralActive': True}
self.context = {'status': 'ready', 'session': 'signal1', 'direction': 'right', 'routeGeneration': 9_000_000_000,
'updatedAt': 10., 'routeOwner': 'signal', 'locationValid': True, 'headingValid': True}
def check(self, **changes):
arguments = {'now': 10.1, 'enabled': True, 'lateral_active': True, 'left_blinker': False, 'right_blinker': True,
'intent': self.intent, 'context': self.context}
return self.guard.check(**(arguments | changes))
def test_current_signal_session_and_generation_are_returned(self):
result = self.check()
self.assertTrue(result['ready'])
self.assertEqual((result['session'], result['routeGeneration']), ('signal1', 9_000_000_000))
def test_cancel_opposite_hazards_inactive_and_unsupported_are_blocked(self):
for changes in ({'right_blinker': False}, {'left_blinker': True, 'right_blinker': False}, {'left_blinker': True},
{'lateral_active': False}, {'intent': self.intent | {'modelSupportsNavigation': False}},
{'intent': self.intent | {'mode': 'lane_change'}}, {'intent': self.intent | {'mode': 'pending'}},
{'intent': self.intent | {'session': 'new-session'}}):
self.assertFalse(self.check(**changes)['ready'])
def test_missing_stale_and_unowned_contexts_fail_closed(self):
for context in (None, self.context | {'updatedAt': 9.}, self.context | {'routeOwner': 'destination'},
self.context | {'status': 'passed'}, self.context | {'routeGeneration': None},
self.context | {'headingValid': False}):
self.assertFalse(self.check(context=context)['ready'])
def test_retention_needs_previously_consumed_same_route_and_explicit_router_flag(self):
retained = self.context | {'retainedAfterSignalCancel': True}
self.assertFalse(self.check(right_blinker=False, intent=None, context=retained)['ready'])
self.guard.consumed('signal1', 9_000_000_000)
self.assertTrue(self.check(right_blinker=False, intent=None, context=retained)['ready'])
self.assertFalse(self.check(right_blinker=False, intent=None)['ready'])
def test_lateral_loss_or_new_session_revokes_retention(self):
retained = self.context | {'retainedAfterSignalCancel': True}
self.guard.consumed('signal1', 9_000_000_000)
self.check(lateral_active=False)
self.assertFalse(self.check(right_blinker=False, intent=None, context=retained)['ready'])
self.guard.consumed('signal1', 9_000_000_000)
self.check(intent=self.intent | {'session': 'new-session'})
self.assertFalse(self.check(right_blinker=False, intent=None, context=retained)['ready'])
def test_temporary_stale_context_zeros_inputs_but_can_resume_same_consumed_exit(self):
retained = self.context | {'retainedAfterSignalCancel': True}
self.guard.consumed('signal1', 9_000_000_000)
self.assertFalse(self.check(now=11., right_blinker=False, intent=None, context=retained)['ready'])
self.assertEqual(self.guard.last_consumed, ('signal1', 9_000_000_000))
self.assertTrue(self.check(now=11.1, right_blinker=False, intent=None, context=retained | {'updatedAt': 11.})['ready'])
self.assertFalse(self.check(context=self.context | {'status': 'completed'})['ready'])
self.assertIsNone(self.guard.last_consumed)
def test_auto_cancel_waits_zeroed_for_router_confirmation_then_resumes(self):
self.guard.consumed('signal1', 9_000_000_000)
waiting = self.check(right_blinker=False, intent=None)
self.assertFalse(waiting['ready'])
self.assertEqual(self.guard.last_consumed, ('signal1', 9_000_000_000))
retained = self.context | {'retainedAfterSignalCancel': True, 'updatedAt': 10.2}
self.assertTrue(self.check(now=10.21, right_blinker=False, intent=None, context=retained)['ready'])
self.check(right_blinker=False, intent=None, context=self.context | {'status': 'idle'})
self.assertIsNone(self.guard.last_consumed)
class DestinationOwnershipTests(unittest.TestCase):
def setUp(self):
self.destination = {'latitude': 40., 'longitude': -80., 'place_name': 'example'}
self.context = {'routeOwner': 'destination', 'status': 'ready', 'routeGeneration': 9_000_000_000,
'updatedAt': 10., 'destination': {'latitude': 40., 'longitude': -80.}}
def test_matching_existing_destination_is_usable_at_boot(self):
result = nav.destination_route_context(self.destination, self.context, 10.1)
self.assertTrue(result['ready'])
self.assertEqual(result['routeGeneration'], 9_000_000_000)
def test_signal_to_manual_or_changed_destination_never_reuses_old_route(self):
self.assertFalse(nav.destination_route_context(self.destination, self.context | {'routeOwner': 'signal'}, 10.1)['ready'])
changed = self.destination | {'longitude': -79.}
self.assertFalse(nav.destination_route_context(changed, self.context, 10.1)['ready'])
self.assertFalse(nav.destination_route_context(self.destination, self.context | {'destination': None}, 10.1)['ready'])
self.assertFalse(nav.destination_route_context(self.destination, self.context, 11.)['ready'])
class SourceModelInputTests(unittest.TestCase):
@staticmethod
def source_run():
tree = ast.parse((SOURCE / 'modeld.py').read_text())
model_class = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == 'ModelState')
run = next(node for node in model_class.body if isinstance(node, ast.FunctionDef) and node.name == 'run')
namespace = {'np': np, 'Callable': Callable, 'VisionBuf': object}
exec(compile(ast.fix_missing_locations(ast.Module(body=[run], type_ignores=[])), 'actual_modeld_run', 'exec'), namespace)
return namespace['run']
@staticmethod
def model(*, navigation=True):
arrays = {'desire': np.zeros(8, np.float32), 'traffic_convention': np.zeros(2, np.float32),
'action_t': np.zeros(2, np.float32)}
if navigation:
arrays.update(nav_map=np.full((1, 1, 256, 256), 255., np.float32), navigation=np.ones((1, 47), np.float32))
class Output:
def numpy(self):
return np.zeros(1, np.float32)
model = SimpleNamespace(numpy_inputs=arrays, prev_desire=np.zeros(8, np.float32), desire_key='desire', chestnut=False,
_vision_input_names=[], _combined_model_type='supercombo', vision_output_slices={'plan': slice(0, 1)},
parser=SimpleNamespace(parse_outputs=lambda _raw: {}))
model.adapter = SimpleNamespace(copy_frames=lambda _frames: None, is_native=True, run=lambda: Output())
return model
def test_actual_run_copies_nav_inputs_and_preserves_native_desire_edges(self):
run, model = self.source_run(), self.model()
request = np.zeros(8, np.float32)
request[2] = 1
inputs = {'desire': request, 'traffic_convention': np.array([1., 0.]), 'action_t': np.array([.295, .525]),
'nav_map': np.full((1, 1, 256, 256), 128., np.float32),
'navigation': nav.navigation_input([maneuver()]).reshape(1, 47)}
run(model, {}, {}, inputs)
np.testing.assert_array_equal(model.numpy_inputs['nav_map'], inputs['nav_map'])
np.testing.assert_array_equal(model.numpy_inputs['navigation'], inputs['navigation'])
self.assertEqual(model.numpy_inputs['desire'][2], 1.)
run(model, {}, {}, inputs)
self.assertEqual(model.numpy_inputs['desire'][2], 0.)
np.testing.assert_array_equal(model.numpy_inputs['action_t'], np.array([.295, .525], np.float32))
def test_actual_run_zeros_absent_nav_and_accepts_nonnav_fallback(self):
run = self.source_run()
inputs = {'desire': np.zeros(8, np.float32)}
model = self.model()
run(model, {}, {}, inputs)
self.assertFalse(model.numpy_inputs['nav_map'].any())
self.assertFalse(model.numpy_inputs['navigation'].any())
fallback = self.model(navigation=False)
run(fallback, {}, {}, inputs)
self.assertNotIn('nav_map', fallback.numpy_inputs)
def test_existing_float_host_layout_also_fits_verified_nav_graph(self):
tree = ast.parse((SOURCE / 'frame_fusion.py').read_text())
layout = next(node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name == 'pack_layout')
namespace = {'math': math}
exec(compile(ast.fix_missing_locations(ast.Module(body=[layout], type_ignores=[])), 'source_layout', 'exec'), namespace)
old = {'desire': (8,), 'traffic_convention': (1, 2), 'action_t': (1, 2)}
before, old_size = namespace['pack_layout'](old, 4096)
expanded, new_size = namespace['pack_layout'](old | {'nav_map': (1, 1, 256, 256), 'navigation': (1, 47)}, 4096)
for name in ('tfm', *old):
self.assertEqual(before[name], expanded[name])
self.assertEqual(expanded['nav_map'], before['frames'])
self.assertGreaterEqual(expanded['navigation'] - expanded['nav_map'], 256 * 256 * 4)
self.assertGreater(new_size, old_size)
class NavigationStatusTests(unittest.TestCase):
def test_status_is_throttled_but_load_and_fallback_can_publish_immediately(self):
writes = []
publisher = nav.NavigationStatePublisher(SimpleNamespace(put=lambda key, value: writes.append((key, value))))
self.assertTrue(publisher.publish(now=10., supported=True, active=True, reason='ready', model_name='Nav N16'))
self.assertFalse(publisher.publish(now=10.05, supported=True, active=True, reason='ready'))
self.assertTrue(publisher.publish(now=10.11, supported=True, active=False, reason='map_stale'))
self.assertTrue(publisher.publish(now=10.12, supported=False, active=False, reason='model_unsupported', force=True))
self.assertEqual(len(writes), 3)
self.assertEqual(writes[0][0], 'NavModelState')
self.assertEqual(writes[0][1]['modelName'], 'Nav N16')
self.assertFalse(writes[-1][1]['supported'])
self.assertFalse(writes[-1][1]['active'])
def test_status_identifiers_belong_to_the_consumed_automatic_route(self):
writes = []
publisher = nav.NavigationStatePublisher(SimpleNamespace(put=lambda key, value: writes.append(value)))
publisher.publish(now=10., supported=True, active=True, reason='ready', session='signal1', route_generation=9_000_000_000)
self.assertEqual(writes[-1]['session'], 'signal1')
self.assertEqual(writes[-1]['routeGeneration'], 9_000_000_000)
publisher.publish(now=10.2, supported=False, active=False, reason='model_unsupported')
self.assertNotIn('session', writes[-1])
def test_unsupported_model_can_never_report_active_nav(self):
writes = []
publisher = nav.NavigationStatePublisher(SimpleNamespace(put=lambda key, value: writes.append(value)))
publisher.publish(now=10., supported=False, active=True, reason='model_unsupported')
self.assertFalse(writes[-1]['active'])
if __name__ == '__main__':
unittest.main()
openpilot/sunnypilot/models/NAVIGATION_MODEL.md
+70
View File
@@ -0,0 +1,70 @@
# Experimental learned navigation model
The normal model selector can expose **RL Navigation80eedcdd**, compiled locally
from the exact official ONNX SHA256
`1d18c42ea4cd95c8bffab4635844b78b8e2d4e25b9de44a9c5ac2e0961371a2e`.
The ONNX pointer is present at comma commit
`afbf2e547e9319e2eb9f515e1e4330af63705cec`, but it was unchanged when that commit
updated the separate N16 PKL. This ONNX must not be described as the N16 weights.
No device model is included or selected automatically. No manifest/artifact means
no selectable entry, and the runtime reports navigation unsupported. The exact
ONNX runs on Mac CPU with five recurrent queues; that does not supply a runnable
Chestnut artifact or prove route following or curb clearance.
## Offroad target build
The following reuses this repository's existing Chestnut native-model compiler
recipe. The complete RL Navigation graph has **not yet been built or validated on
a target device** as part of this change. It requires an offroad comma device,
the supported connected Chestnut GPU, and the normal model-compilation
dependencies. The Mac ORT result is not a substitute for this check.
Obtain the ONNX by a trusted transfer and verify the exact hash above. A mirror
carrying those official bytes is:
`https://github.com/happymaj11r/openpilot-models/releases/download/big-nav-model-integration-1d18c42e/big_driving_supercombo.onnx`
Then, from the repository root on the offroad target:
```sh
python3 -m openpilot.sunnypilot.models.build_navigation_model --onnx /path/to/big_driving_supercombo.onnx
```
The installer verifies the source hash, invokes `compile_modeld_v2.py` with
`DEV=USB+AMD:LLVM`, `FRAME_DEV=CPU`, `FLOAT16=1`, `GMMU=0`, `TC_OPT=2`,
`TC_MIN_GLOBALS=32`, all inputs on-device and one benchmark run. It stops if the
device enters onroad. A failed build publishes no selector entry. After checking
the generated plain artifact's hash, target and navigation input/output protocol,
it writes the artifact and `rl_nav_80eedcdd_bundle.json` under
`Paths.model_root()`. It does not change the selected model or start a drive.
After a successful validated build, explicitly select the experimental model in
the existing Chestnut model selector while offroad. The local row survives remote
catalog refreshes, while the downloaded remote catalog stays unchanged. Removing
or changing its artifact makes the local row unavailable. Existing bundles remain
available through the ordinary selector.
## Protocol and unchanged controls
The bundle uses **generation12 as sunnypilot's existing native/action protocol**,
not as a training checkpoint number. This graph preserves the semantic CTV3
output slices, including plan990, action4, desire-state8 and desire-prediction32,
and adds map/navigation feature heads and two recurrent queues. The adapter's
float32 host layout already fits the verified map input; it is unchanged.
The existing action conversion,1.6m/s curvature hold, steering and cooperative
steering code are preserved. An unbundled `COMBINED_MODEL_PKL` override cannot
activate learned-navigation capability because its generation/control metadata
is absent. The small fallback lacks navigation inputs and reports unsupported.
Map and instruction conditioning is enabled only with matching real route
ownership/generation and fresh inputs. The automatic route must match the active
signal session, or be an explicitly retained outgoing continuation of a route
this model already consumed. `NavModelState.active` reports consumed conditioning
inputs, not successful autonomous turn completion.
A target build, source-versus-device numerical comparison and recorded-route
evaluation with real navigation inputs are still required before road-test
readiness can be claimed. No controller or curb-safety verdict follows from the
Mac runtime smoke or the metadata/selector tests.
openpilot/sunnypilot/models/build_navigation_model.py
+74
View File
@@ -0,0 +1,74 @@
"""Offroad target build/registration; never selects or starts the model.
The complete Nav graph has not yet been compiled on a supported target in this
work. This uses the repository's existing Chestnut compile recipe, rather than
pretending the Mac ORT smoke provides a device artifact or numerical parity.
"""
import argparse
import json
import os
from pathlib import Path
import subprocess
import sys
import tempfile
import time
from openpilot.sunnypilot.models.navigation_model import MANIFEST_NAME, SOURCE_COMMIT, SOURCE_ONNX_SHA256, file_sha256, installed_bundle
COMPILE_ENV = {'DEBUG': '1', 'DEV': 'USB+AMD:LLVM', 'FRAME_DEV': 'CPU', 'FLOAT16': '1',
'GMMU': '0', 'TC_OPT': '2', 'TC_MIN_GLOBALS': '32'}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--onnx', required=True, type=Path, help='Already downloaded exact official-SHA ONNX')
args = parser.parse_args()
from openpilot.common.basedir import BASEDIR
from openpilot.common.hardware import COMMA_HARDWARE
from openpilot.common.hardware.hw import Paths
from openpilot.common.params import Params
params = Params()
if not COMMA_HARDWARE or params.get_bool('IsOnroad'):
parser.error('This build requires an offroad comma device with a supported Chestnut GPU; Mac outputs are not device artifacts.')
if file_sha256(args.onnx) != SOURCE_ONNX_SHA256:
parser.error('The ONNX bytes do not match the official RL Navigation artifact.')
root = Path(Paths.model_root())
root.mkdir(parents=True, exist_ok=True)
compiler = Path(BASEDIR) / 'openpilot/sunnypilot/modeld_v2/compile_modeld_v2.py'
with tempfile.TemporaryDirectory(prefix='.rl-nav-build-', dir=root) as directory:
output = Path(directory) / 'rl_nav_80eedcdd_tinygrad.pkl'
command = [sys.executable, str(compiler), str(args.onnx.resolve()), str(output), '--device-input', '*', '--benchmark-runs', '1']
with subprocess.Popen(command, cwd=BASEDIR, env=os.environ | COMPILE_ENV) as process:
while process.poll() is None:
if params.get_bool('IsOnroad'):
process.terminate()
try:
process.wait(timeout=5)
except subprocess.TimeoutExpired:
process.kill()
raise RuntimeError('Build stopped because the device entered onroad; no model was registered.')
time.sleep(.2)
if process.returncode:
raise RuntimeError(f'Existing target compiler failed (exit {process.returncode}); no model was registered.')
if params.get_bool('IsOnroad'):
raise RuntimeError('Device entered onroad; no model was registered.')
digest = file_sha256(output)
name = 'rl_nav_80eedcdd_' + digest[:16] + '_tinygrad.pkl'
descriptor = {'version': 1, 'target': 'chestnut', 'sourceOnnxSha256': SOURCE_ONNX_SHA256,
'sourceRef': 'commaai/openpilot:' + SOURCE_COMMIT, 'sourceCheckpoint': '80eedcdd/12864 (not the separate N16 PKL)',
'artifact': {'fileName': name, 'sha256': digest}, 'compilerEnvironment': COMPILE_ENV,
'compilerSha256': file_sha256(compiler)}
# Validate in the temporary directory before publishing either artifact.
output.rename(Path(directory) / name)
(Path(directory) / MANIFEST_NAME).write_text(json.dumps(descriptor, indent=2) + '\n')
if installed_bundle(directory) is None:
raise RuntimeError('Target artifact failed its plain-format/navigation protocol checks; not registered.')
(Path(directory) / name).replace(root / name)
(Path(directory) / MANIFEST_NAME).replace(root / MANIFEST_NAME)
print('Verified RL Navigation build registered. Select it explicitly in the Chestnut model selector while offroad.')
print('This build check is not a route-following, curb-clearance or source-vs-device numerical parity verdict.')
if __name__ == '__main__':
main()
openpilot/sunnypilot/models/fetcher.py
+17 -3
View File
@@ -14,6 +14,17 @@ from openpilot.sunnypilot.models.helpers import is_bundle_version_compatible
from openpilot.cereal import custom
def _with_local_navigation(bundles, source):
if source == 'chestnut':
from openpilot.common.hardware.hw import Paths
from openpilot.sunnypilot.models.navigation_model import installed_bundle
local = installed_bundle(Paths.model_root())
if local is not None:
parsed = ModelParser._parse_bundle(local)
return [bundle for bundle in bundles if bundle.ref != parsed.ref] + [parsed]
return bundles
class ModelParser:
"""Handles parsing of model data into cereal objects"""
@@ -179,6 +190,9 @@ class ModelFetcher:
return not any(bundle.get("is_big") is True for bundle in bundles)
def get_bundles_for_source(self, source: str) -> list[custom.ModelManagerSP.ModelBundle]:
return _with_local_navigation(self._get_remote_bundles_for_source(source), source)
def _get_remote_bundles_for_source(self, source: str) -> list[custom.ModelManagerSP.ModelBundle]:
if source not in self.MODEL_SOURCES:
cloudlog.warning(f"Unknown model source: {source}")
return []
@@ -226,12 +240,12 @@ def get_cached_bundles(params: Params, source: str) -> list[custom.ModelManagerS
_, suffix = ModelFetcher.MODEL_SOURCES[source]
cached_data = params.get(f"ModelManager_ModelsCache{suffix}")
if not cached_data:
return []
return _with_local_navigation([], source)
try:
return ModelParser.parse_models(cached_data)
return _with_local_navigation(ModelParser.parse_models(cached_data), source)
except Exception as e:
cloudlog.warning(f"Failed to parse cached models for source {source}: {e}")
return []
return _with_local_navigation([], source)
if __name__ == "__main__":
openpilot/sunnypilot/models/navigation_model.py
+114
View File
@@ -0,0 +1,114 @@
"""Explicitly installed RL Navigation model, appended to the normal catalog.
No artifact is bundled or selected automatically. The installer verifies the
official ONNX bytes before target compilation; this reader rechecks the resulting
plain artifact hash, device and native protocol before exposing a selectable row.
"""
import base64
import builtins
import hashlib
import io
import json
from pathlib import Path
import pickle
import struct
import numpy as np
SOURCE_COMMIT = 'afbf2e547e9319e2eb9f515e1e4330af63705cec'
SOURCE_ONNX_SHA256 = '1d18c42ea4cd95c8bffab4635844b78b8e2d4e25b9de44a9c5ac2e0961371a2e'
MODEL_NAME = 'RL Navigation 80eedcdd (experimental)'
MANIFEST_NAME = 'rl_nav_80eedcdd_bundle.json'
SELECTOR_VERSION = 21
_validated = {}
class _SliceMetadata(pickle.Unpickler):
def find_class(self, module, name):
if module == 'builtins' and name == 'slice':
return builtins.slice
raise pickle.UnpicklingError('Unsupported output-slice metadata class')
def file_sha256(path):
with Path(path).open('rb') as stream:
return hashlib.file_digest(stream, 'sha256').hexdigest()
def compatible_metadata(extras):
try:
specs, outputs = extras['input_specs'], extras['metadata']['output_shapes']
expected = {
'new_img': ((2, 6, 128, 256), np.uint8), 'desire': ((8,), np.float32),
'traffic_convention': ((1, 2), np.float32), 'action_t': ((1, 2), np.float32),
'nav_map': ((1, 1, 256, 256), np.float32), 'navigation': ((1, 47), np.float32),
'state_img_q': ((2, 5, 6, 128, 256), np.uint8), 'state_desire_q': ((132, 1, 8), np.float32),
'state_feat_q': ((128, 1, 16384), np.float32), 'state_map_features_q': ((128, 1, 64), np.float32),
'state_navigation_features_q': ((128, 1, 64), np.float32),
}
if set(specs) != set(expected):
return False
for name, (shape, dtype) in expected.items():
if tuple(specs[name][0]) != shape or np.dtype(specs[name][1]) != dtype or 'AMD' not in specs[name][2]:
return False
if name.startswith('state_') and tuple(outputs.get('next_' + name, ())) != shape:
return False
expected_outputs = {'outputs': ((1, 18580), np.float32)} | {
'next_' + name: value for name, value in expected.items() if name.startswith('state_')}
if set(outputs) != set(expected_outputs) or set(extras['output_specs']) != set(expected_outputs):
return False
for name, (shape, dtype) in expected_outputs.items():
actual = extras['output_specs'][name]
if tuple(outputs[name]) != shape or tuple(actual[0]) != shape or np.dtype(actual[1]) != dtype or 'AMD' not in actual[2]:
return False
encoded = extras['metadata']['metadata']['output_slices']
slices = _SliceMetadata(io.BytesIO(base64.b64decode(encoded, validate=True))).load()
required = {'plan': slice(917, 1907), 'action': slice(2062, 2066),
'desire_state': slice(2054, 2062), 'desire_pred': slice(855, 887)}
if any(slices.get(name) != value for name, value in required.items()):
return False
except (ValueError, KeyError, TypeError, IndexError, AttributeError, EOFError, pickle.UnpicklingError):
return False
return True
def installed_bundle(model_root):
"""Return a normal selector bundle only for a present verified target build."""
root = Path(model_root)
try:
manifest_path = root / MANIFEST_NAME
if manifest_path.stat().st_size > 16384:
return None
manifest = json.loads(manifest_path.read_text())
artifact = manifest['artifact']
name, expected = artifact['fileName'], artifact['sha256']
if (manifest.get('version') != 1 or manifest.get('target') != 'chestnut' or
manifest.get('sourceOnnxSha256') != SOURCE_ONNX_SHA256 or
not isinstance(name, str) or Path(name).name != name or not name.endswith('.pkl') or
not isinstance(expected, str) or len(expected) != 64 or any(c not in '0123456789abcdef' for c in expected)):
return None
path = root / name
if path.resolve().parent != root.resolve():
return None
stat = path.stat()
key = (str(path.resolve()), stat.st_size, stat.st_mtime_ns, expected)
if key not in _validated:
if file_sha256(path) != expected:
return None
from openpilot.sunnypilot.modeld_v2.helpers import read_parts
parts, extras = read_parts(str(path))
if 'run' not in parts or not compatible_metadata(extras):
return None
_validated.clear()
_validated[key] = True
return {
'index': 2147483000, 'short_name': 'RLNAV80', 'display_name': MODEL_NAME,
'ref': 'local-rl-nav-80eedcdd-' + expected[:16], 'is_big': True,
'environment': 'development', 'runner': 'tinygrad', 'generation': 12,
'is_20hz': True, 'minimum_selector_version': SELECTOR_VERSION,
'overrides': {'folder': 'Local experimental models', 'lat': '.0', 'long': '.3'},
'models': [{'type': 'chunked', 'artifact': {'file_name': name,
'download_uri': {'url': path.resolve().as_uri(), 'sha256': expected}}}],
}
except (OSError, ValueError, KeyError, TypeError, IndexError, AttributeError, EOFError, pickle.UnpicklingError, struct.error):
return None
openpilot/sunnypilot/models/tests/test_navigation_model.py
+204
View File
@@ -0,0 +1,204 @@
"""Local selector entries require an installed verified target artifact."""
import ast
import base64
import hashlib
import importlib.util
import json
from pathlib import Path
import pickle
import sys
from tempfile import TemporaryDirectory
from types import SimpleNamespace
import unittest
from unittest.mock import patch
SOURCE = Path(__file__).resolve().parents[1]
spec = importlib.util.spec_from_file_location('local_nav_model_under_test', SOURCE / 'navigation_model.py')
nav = importlib.util.module_from_spec(spec)
spec.loader.exec_module(nav)
def metadata(*, device='USB+AMD:LLVM'):
inputs = {'new_img': ((2, 6, 128, 256), '|u1', device),
'desire': ((8,), '<f4', device), 'traffic_convention': ((1, 2), '<f4', device), 'action_t': ((1, 2), '<f4', device),
'nav_map': ((1, 1, 256, 256), '<f4', device), 'navigation': ((1, 47), '<f4', device),
'state_img_q': ((2, 5, 6, 128, 256), '|u1', device), 'state_desire_q': ((132, 1, 8), '<f4', device),
'state_feat_q': ((128, 1, 16384), '<f4', device),
'state_map_features_q': ((128, 1, 64), '<f4', device),
'state_navigation_features_q': ((128, 1, 64), '<f4', device)}
slices = {'plan': slice(917, 1907), 'action': slice(2062, 2066),
'desire_state': slice(2054, 2062), 'desire_pred': slice(855, 887)}
outputs = {'outputs': ((1, 18580), '<f4', device)} | {'next_' + name: value for name, value in inputs.items() if name.startswith('state_')}
return {'input_specs': inputs, 'output_specs': outputs,
'metadata': {'metadata': {'output_slices': base64.b64encode(pickle.dumps(slices)).decode()},
'output_shapes': {name: value[0] for name, value in outputs.items()}}}
class LocalNavigationModelTests(unittest.TestCase):
def setUp(self):
self.temporary = TemporaryDirectory()
self.root = Path(self.temporary.name)
self.artifact = self.root / 'local_nav_test.pkl'
self.artifact.write_bytes(b'test-plain-artifact-no-kernel-execution')
self.descriptor = {'version': 1, 'target': 'chestnut', 'sourceOnnxSha256': nav.SOURCE_ONNX_SHA256,
'artifact': {'fileName': self.artifact.name, 'sha256': hashlib.sha256(self.artifact.read_bytes()).hexdigest()}}
self.extras = metadata()
nav._validated.clear()
self.reader = SimpleNamespace(read_parts=lambda _path: ({'run': ({}, b'')}, self.extras))
self.patch = patch.dict(sys.modules, {'openpilot.sunnypilot.modeld_v2.helpers': self.reader})
self.patch.start()
def tearDown(self):
self.patch.stop()
self.temporary.cleanup()
def write(self, changes=None):
(self.root / nav.MANIFEST_NAME).write_text(json.dumps(self.descriptor | (changes or {})))
def test_no_installed_manifest_means_no_selectable_entry(self):
self.assertIsNone(nav.installed_bundle(self.root))
self.write()
self.artifact.unlink()
self.assertIsNone(nav.installed_bundle(self.root))
def test_verified_native_target_is_normal_explicit_bundle_not_n16_claim(self):
self.write()
bundle = nav.installed_bundle(self.root)
self.assertEqual(bundle['generation'], 12)
self.assertTrue(bundle['is_20hz'])
self.assertEqual(bundle['minimum_selector_version'], 21)
self.assertEqual(bundle['runner'], 'tinygrad')
self.assertIn('80eedcdd', bundle['display_name'])
self.assertNotIn('N16', bundle['display_name'])
self.assertEqual(bundle['overrides']['lat'], '.0')
self.assertEqual(bundle['models'][0]['artifact']['download_uri']['sha256'], self.descriptor['artifact']['sha256'])
self.assertNotIn('selected', bundle)
def test_model_hash_source_or_path_tamper_never_registers(self):
for change in ({'sourceOnnxSha256': '0' * 64}, {'target': 'metal'},
{'artifact': {'fileName': '../escape.pkl', 'sha256': 'a' * 64}},
{'artifact': {'fileName': self.artifact.name, 'sha256': 'a' * 64}}):
self.write(change)
self.assertIsNone(nav.installed_bundle(self.root))
def test_mac_cpu_small_or_missing_nav_metadata_never_registers(self):
self.write()
for extras in (metadata(device='METAL'), metadata(device='CPU'), {'input_specs': {}, 'metadata': {}},
metadata() | {'metadata': {'output_shapes': {'outputs': (1, 2576)}}}):
self.extras = extras
nav._validated.clear()
self.assertIsNone(nav.installed_bundle(self.root))
def test_changed_installed_bytes_are_reverified(self):
self.write()
self.assertIsNotNone(nav.installed_bundle(self.root))
self.artifact.write_bytes(b'changed artifact bytes')
self.assertIsNone(nav.installed_bundle(self.root))
def test_incompatible_action_layout_cannot_claim_generation12(self):
self.write()
slices = {'plan': slice(917, 1907), 'action': slice(2062, 2070),
'desire_state': slice(2054, 2062), 'desire_pred': slice(855, 887)}
self.extras['metadata']['metadata']['output_slices'] = base64.b64encode(pickle.dumps(slices)).decode()
self.assertIsNone(nav.installed_bundle(self.root))
def test_every_state_pair_needs_matching_output_shape_and_dtype(self):
for name in ('state_img_q', 'state_desire_q', 'state_feat_q', 'state_map_features_q', 'state_navigation_features_q'):
for change in ('missing', 'shape', 'dtype'):
extras = metadata()
output = 'next_' + name
if change == 'missing':
extras['metadata']['output_shapes'].pop(output)
elif change == 'shape':
extras['metadata']['output_shapes'][output] = (1,)
else:
shape, _, device = extras['output_specs'][output]
extras['output_specs'][output] = (shape, '<f8', device)
self.assertFalse(nav.compatible_metadata(extras), (name, change))
def test_host_input_and_malformed_metadata_contracts_fail_closed(self):
for name in ('desire', 'traffic_convention', 'action_t'):
extras = metadata()
extras['input_specs'][name] = ((99,), '<f4', 'USB+AMD:LLVM')
self.assertFalse(nav.compatible_metadata(extras))
for extras in (None, [], {}, {'input_specs': []}, metadata() | {'input_specs': {'new_img': ()}}):
self.assertFalse(nav.compatible_metadata(extras))
def test_invalid_plain_metadata_fails_closed_without_code_execution(self):
self.write()
def invalid(_path):
raise pickle.UnpicklingError('Rejected arbitrary metadata class')
self.reader.read_parts = invalid
self.assertIsNone(nav.installed_bundle(self.root))
class CatalogHookTests(unittest.TestCase):
def test_cached_offline_model_is_selectable_without_changing_remote_cache(self):
tree = ast.parse((SOURCE / 'fetcher.py').read_text())
methods = [node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name in ('_with_local_navigation', 'get_cached_bundles')]
local, remote = SimpleNamespace(ref='local-verified'), SimpleNamespace(ref='remote')
payload = {'bundles': [{'ref': 'remote'}]}
namespace = {'Params': object, 'custom': SimpleNamespace(ModelManagerSP=SimpleNamespace(ModelBundle=object)),
'ModelFetcher': SimpleNamespace(MODEL_SOURCES={'chestnut': ('url', '_Chestnut'), 'qcom': ('url', '')}),
'ModelParser': SimpleNamespace(parse_models=lambda value: [remote], _parse_bundle=lambda value: local),
'cloudlog': SimpleNamespace(warning=lambda *_args: None)}
exec(compile(ast.fix_missing_locations(ast.Module(body=methods, type_ignores=[])), 'actual_catalog_hook', 'exec'), namespace)
modules = {'openpilot.common.hardware.hw': SimpleNamespace(Paths=SimpleNamespace(model_root=lambda: '/test/models')),
'openpilot.sunnypilot.models.navigation_model': SimpleNamespace(installed_bundle=lambda path: {'ref': local.ref})}
with patch.dict(sys.modules, modules):
cached = namespace['get_cached_bundles'](SimpleNamespace(get=lambda _key: payload), 'chestnut')
self.assertEqual(cached, [remote, local])
self.assertEqual(payload, {'bundles': [{'ref': 'remote'}]})
offline = namespace['get_cached_bundles'](SimpleNamespace(get=lambda _key: None), 'chestnut')
self.assertEqual(offline, [local])
self.assertEqual(namespace['_with_local_navigation']([remote], 'qcom'), [remote])
class OffroadBuilderTests(unittest.TestCase):
def run_builder(self, *, hardware, onroad, expected_hash=False, compile_fails=False):
with TemporaryDirectory() as directory:
root = Path(directory)
modules = {
'openpilot.sunnypilot.models.navigation_model': nav,
'openpilot.common.basedir': SimpleNamespace(BASEDIR=str(SOURCE.parents[2])),
'openpilot.common.hardware': SimpleNamespace(COMMA_HARDWARE=hardware),
'openpilot.common.hardware.hw': SimpleNamespace(Paths=SimpleNamespace(model_root=lambda: str(root))),
'openpilot.common.params': SimpleNamespace(Params=lambda: SimpleNamespace(get_bool=lambda _key: onroad)),
}
with patch.dict(sys.modules, modules), patch.object(sys, 'argv', ['build_navigation_model', '--onnx', str(root / 'source.onnx')]):
builder_spec = importlib.util.spec_from_file_location('nav_builder_under_test', SOURCE / 'build_navigation_model.py')
builder = importlib.util.module_from_spec(builder_spec)
builder_spec.loader.exec_module(builder)
with patch.object(builder, 'file_sha256', return_value=nav.SOURCE_ONNX_SHA256 if expected_hash else 'wrong'):
if compile_fails:
class Failed:
returncode = 1
def __enter__(self):
return self
def __exit__(self, *_args):
pass
def poll(self):
return 1
with patch.object(builder.subprocess, 'Popen', return_value=Failed()):
with self.assertRaises(RuntimeError):
builder.main()
else:
with patch.object(builder.subprocess, 'Popen', side_effect=AssertionError('Compiler must not start')):
with self.assertRaises(SystemExit) as error:
builder.main()
self.assertEqual(error.exception.code, 2)
self.assertFalse((root / nav.MANIFEST_NAME).exists())
def test_mac_or_onroad_build_never_starts_compiler(self):
self.run_builder(hardware=False, onroad=False)
self.run_builder(hardware=True, onroad=True)
def test_wrong_onnx_bytes_never_start_compiler(self):
self.run_builder(hardware=True, onroad=False)
def test_failed_compile_exposes_no_manifest_or_selected_model(self):
self.run_builder(hardware=True, onroad=False, expected_hash=True, compile_fails=True)
if __name__ == '__main__':
unittest.main()
openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py
+28 -18
View File
@@ -8,8 +8,8 @@ from openpilot.cereal import custom
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.sunnypilot.selfdrive.controls.lib.map_turn_desire import MapTurnGate
from openpilot.sunnypilot.selfdrive.controls.lib.nav_turn_intent import NavTurnIntent, turn_route_ready
import time
TurnDirection = custom.ModelDataV2SP.TurnDirection
@@ -17,7 +17,6 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
# "Wait for the turn": the blinker only arms the turn, and the one-shot turn desire is sent when one of these says
# the turn is here. The model treats the desire as "turn now", so sending it early makes it take the first opening.
TURN_STOPPED_SPEED = 0.5 # m/s, stopped at the turn (red light, stop sign)
MODEL_TURN_PROB = 0.35 # model's own prediction that a turn starts within the next horizons
MODEL_TURN_HORIZONS = 2 # desirePrediction horizons checked, soonest first
DESIRE_PRED_WIDTH = 8
@@ -40,7 +39,9 @@ class LaneTurnController:
self.armed_direction = TurnDirection.none
self.cue_sent = False
self.params = Params()
self.map_gate = MapTurnGate(self.params)
self.nav_intent = NavTurnIntent(self.params)
self.navigation_enabled = False
self.navigation_owns_signal = False
self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
self.param_read_counter = 0
self.enabled = self.params.get_bool("LaneTurnDesire")
@@ -50,8 +51,7 @@ class LaneTurnController:
self.enabled = self.params.get_bool("LaneTurnDesire")
value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
self.map_gate.configure(self.params.get_bool("MapboxNextTurnEnabled"),
self.params.get("MapboxTurnLeadDistance", return_default=True))
self.navigation_enabled = self.params.get_bool("MapboxNextTurnEnabled")
def update_params(self) -> None:
if self.param_read_counter % 50 == 0:
@@ -62,8 +62,17 @@ class LaneTurnController:
steering_pressed: bool = False, steering_torque: float = 0., turn_probs: tuple[float, float] = (0., 0.),
lateral_active: bool = True, model_position=None, map_turn_supported: bool = False) -> None:
signal = 'left' if left_blinker and not right_blinker else 'right' if right_blinker and not left_blinker else None
mapped = self.map_gate.enabled and map_turn_supported
self.map_gate.update_signal(signal, self.enabled and mapped)
self.nav_intent.update(signal, self.navigation_enabled, map_turn_supported, lateral_active)
self.navigation_owns_signal = (self.navigation_enabled and map_turn_supported and
self.nav_intent.mode == 'turn' and signal is not None)
self.turn_blindspot = blindspot_left if signal == 'left' else blindspot_right
# Navigation-conditioned models consume the short route continuously. Sending
# the legacy turn-now desire as well would introduce a second timing source.
if self.navigation_enabled and map_turn_supported:
self.turn_direction = TurnDirection.none
self.armed_direction = TurnDirection.none
self.cue_sent = False
return
if not left_blinker and not right_blinker:
self.armed_direction = TurnDirection.none
self.cue_sent = False
@@ -81,9 +90,6 @@ class LaneTurnController:
else:
requested = TurnDirection.none
if requested == TurnDirection.none or not lateral_active:
self.map_gate.reset_motion()
if not self.wait_for_turn or requested == TurnDirection.none:
self.turn_direction = requested
return
@@ -93,15 +99,9 @@ class LaneTurnController:
# Eligibility can clear the request, but only a canceled or switched signal permits another cue.
if self.turn_direction == TurnDirection.none and not self.cue_sent:
ready = self.map_gate.ready(v_ego, model_position, lateral_active) if mapped else self.turn_is_here(
requested, v_ego, steering_pressed, steering_torque, turn_probs)
ready = self.turn_is_here(requested, v_ego, steering_pressed, steering_torque, turn_probs)
if ready:
self.turn_direction = requested
if self.enabled and mapped:
self.map_gate.record_cue()
cloudlog.event('modeld.mapbox_turn_cue', session=self.map_gate.session, direction=self.map_gate.direction,
distance_m=self.map_gate.last_decision.distance_m, lead_m=self.map_gate.lead_m,
cue_mode=self.map_gate.last_mode, forecast_travel_m=self.map_gate.motion.last_motion_m)
if self.enabled and self.turn_direction != TurnDirection.none:
self.cue_sent = True
@@ -112,6 +112,16 @@ class LaneTurnController:
model_sees_turn = (turn_probs[0] if left else turn_probs[1]) >= MODEL_TURN_PROB
return nudged or model_sees_turn
def finalize_navigation_intent(self, lane_change_requested, speed, lateral_active, model_supported):
if not self.navigation_enabled or not model_supported or not lateral_active:
return
if lane_change_requested:
self.nav_intent.commit('lane_change')
elif (self.nav_intent.mode == 'pending' and 0. <= speed < self.lane_turn_value and not self.turn_blindspot and
turn_route_ready(self.params.get('MapboxTurnContext'), self.nav_intent.session, self.nav_intent.direction, time.monotonic())):
self.nav_intent.commit('turn')
self.navigation_owns_signal = self.nav_intent.mode == 'turn'
def get_turn_direction(self):
if not self.enabled:
return TurnDirection.none
openpilot/sunnypilot/selfdrive/controls/lib/map_turn_desire.py
+1 -1
View File
@@ -1,4 +1,4 @@
"""Opt-in map timing for the existing turn-desire input, without actuator changes."""
"""Legacy cue-experiment helpers; live signal navigation uses nav_turn_intent."""
from dataclasses import dataclass
import math
import time
openpilot/sunnypilot/selfdrive/controls/lib/nav_turn_intent.py
+60
View File
@@ -0,0 +1,60 @@
"""Own a signal's pending route, then its first committed maneuver."""
import math
import time
def turn_route_ready(context, session, direction, now):
if not isinstance(context, dict):
return False
updated, generation, distance = context.get('updatedAt'), context.get('routeGeneration'), context.get('distanceM')
return (context.get('session') == session and context.get('direction') == direction and
context.get('status') == 'ready' and context.get('routeOwner') == 'signal' and
context.get('locationValid') is True and context.get('headingValid') is True and
isinstance(generation, int) and not isinstance(generation, bool) and generation > 0 and
isinstance(updated, (int, float)) and not isinstance(updated, bool) and math.isfinite(updated) and
math.isfinite(now) and 0. <= now-updated <= .35 and
isinstance(distance, (int, float)) and not isinstance(distance, bool) and math.isfinite(distance) and distance > 0.)
class NavTurnIntent:
def __init__(self, params, session_clock=time.monotonic_ns):
self.params = params
self.session_clock = session_clock
self.direction = None
self.session = None
self.physical_direction = None
self.mode = None
self._supported, self._active = False, False
self._published = None
self.params.remove('MapboxTurnIntent')
def update(self, direction, enabled, model_supported, lateral_active):
physical = direction if enabled and direction in ('left', 'right') else None
if physical != self.physical_direction:
self.physical_direction = physical
self.mode = 'pending' if physical else None
self._supported, self._active = bool(model_supported), bool(lateral_active)
self._publish()
def commit(self, mode):
if self.mode != 'pending' or self.physical_direction is None or mode not in ('turn', 'lane_change'):
return False
self.mode = mode
self._publish()
return True
def _publish(self):
direction = self.physical_direction if self._active and self.mode in ('pending', 'turn') else None
if direction != self.direction:
self.direction = direction
self.session = str(self.session_clock()) if direction else None
value = {'session': self.session, 'direction': direction, 'mode': self.mode,
'modelSupportsNavigation': self._supported, 'lateralActive': True} if direction else None
if value != self._published:
if value is None:
self.params.remove('MapboxTurnIntent')
else:
# This event is rare. Persist it before a synchronous cancel/remove can
# race a queued write and resurrect a retired signal session.
self.params.put('MapboxTurnIntent', value, block=True)
self._published = value
openpilot/sunnypilot/selfdrive/controls/lib/tests/test_map_turn_desire.py
-97
View File
@@ -1,10 +1,7 @@
"""Map cue/session behavior; no vehicle control or network dependency."""
import ast
import importlib.util
from enum import IntEnum
from pathlib import Path
import sys
from types import SimpleNamespace
import unittest
ROOT = Path(__file__).resolve().parents[1]
@@ -16,11 +13,6 @@ SPEC = importlib.util.spec_from_file_location('map_turn_desire_test_subject', RO
MAP = importlib.util.module_from_spec(SPEC)
sys.modules[SPEC.name] = MAP
SPEC.loader.exec_module(MAP)
STATIONARY_POSITION = SimpleNamespace(t=[0., 1., 2., 3.], x=[0., 0., 0., 0.], y=[0., 0., 0., 0.])
class Direction(IntEnum):
none, turnLeft, turnRight = 0, 1, 2
class FakeParams:
@@ -96,94 +88,5 @@ class TestMapTurnGate(unittest.TestCase):
self.assertEqual(gate.session, '3')
class TestMapTurnControllerIntegration(unittest.TestCase):
def setUp(self):
self.params = FakeParams()
tree = ast.parse((ROOT/'lane_turn_desire.py').read_text())
tree.body = [node for node in tree.body if not isinstance(node, (ast.Import, ast.ImportFrom))]
ns = {'custom': SimpleNamespace(ModelDataV2SP=SimpleNamespace(TurnDirection=Direction)),
'CV': SimpleNamespace(MPH_TO_MS=.44704), 'Params': lambda: self.params, 'MapTurnGate': MAP.MapTurnGate,
'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
exec(compile(tree, 'lane_turn_desire.py', 'exec'), ns)
self.controller = ns['LaneTurnController'](None)
self.controller.map_gate.clock = lambda: 10.1
self.sessions = iter(range(1, 10))
self.controller.map_gate.session_clock = lambda: next(self.sessions)
def step(self, speed=3., right=True, left=False, blindspot=False, probabilities=(0., 0.), lateral_active=True,
model_position=STATIONARY_POSITION, map_turn_supported=True):
self.controller.update_params()
self.controller.update_lane_turn(False, blindspot, left, right, speed, turn_probs=probabilities, lateral_active=lateral_active,
model_position=model_position, map_turn_supported=map_turn_supported)
return self.controller.get_turn_direction()
def enable(self):
self.params.put('MapboxNextTurnEnabled', True)
self.controller.read_params()
def test_disabled_map_preserves_existing_model_gate(self):
self.assertEqual(self.step(), Direction.none)
self.assertEqual(self.step(probabilities=(0., .8)), Direction.turnRight)
self.assertNotIn('MapboxTurnIntent', self.params.values)
def test_map_times_cue_even_without_readiness_and_suppresses_early_readiness(self):
self.enable()
self.assertEqual(self.step(probabilities=(0., .9)), Direction.none)
self.params.put('MapboxTurnContext', context(distanceM=30.))
self.assertEqual(self.step(probabilities=(0., .9)), Direction.none)
self.params.put('MapboxTurnContext', context())
self.assertEqual(self.step(), Direction.turnRight)
self.assertEqual(self.params.get('MapboxTurnCue')['session'], '1')
def test_speed_and_blindspot_and_single_cue_guard_are_retained(self):
self.enable()
self.step()
self.params.put('MapboxTurnContext', context())
self.assertEqual(self.step(speed=0.), Direction.none)
self.assertEqual(self.step(blindspot=True), Direction.none)
self.assertEqual(self.step(), Direction.turnRight)
self.assertEqual(self.step(speed=10.), Direction.none)
self.assertEqual(self.step(), Direction.none, 'Eligibility recovery must not create a second cue')
self.step(right=False)
self.step()
self.assertEqual(self.controller.map_gate.session, '2')
self.assertEqual(self.controller.get_turn_direction(), Direction.none)
def test_preloads_target_while_inactive_without_consuming_cue(self):
self.enable()
self.step(lateral_active=False)
self.params.put('MapboxTurnContext', context())
self.assertEqual(self.step(lateral_active=False), Direction.none)
self.assertFalse(self.controller.cue_sent)
self.assertEqual(self.step(lateral_active=True), Direction.turnRight)
def test_stopped_preparation_requires_sustained_model_movement(self):
self.enable()
self.step(speed=0.)
self.params.put('MapboxTurnContext', context())
stationary = SimpleNamespace(t=[0., 1., 2., 3.], x=[0.]*4, y=[0.]*4)
launch = SimpleNamespace(t=[0., 1., 2., 3.], x=[0., 0., 0., 2.], y=[0.]*4)
self.assertEqual(self.step(speed=0., model_position=stationary), Direction.none)
for i in range(5):
now = 10.1+i*.05
self.controller.map_gate.clock = lambda now=now: now
self.params.put('MapboxTurnContext', context(updatedAt=now))
direction = self.step(speed=0., model_position=launch)
self.assertEqual(direction, Direction.turnRight if i == 4 else Direction.none)
self.assertEqual(self.params.get('MapboxTurnCue')['cueMode'], 'forecast_movement')
def test_unsupported_model_retains_original_gate_and_no_map_intent(self):
self.enable()
self.assertEqual(self.step(probabilities=(0., .9), model_position=None, map_turn_supported=False), Direction.turnRight)
self.assertNotIn('MapboxTurnIntent', self.params.values)
def test_supported_model_with_missing_trajectory_refuses_a_map_cue(self):
self.enable()
self.step()
self.params.put('MapboxTurnContext', context())
self.assertEqual(self.step(probabilities=(0., .9), model_position=None), Direction.none)
self.assertFalse(self.controller.cue_sent)
if __name__ == '__main__':
unittest.main()
openpilot/sunnypilot/selfdrive/controls/lib/tests/test_nav_turn_intent.py
+322
View File
@@ -0,0 +1,322 @@
"""Exercise source maneuver commitment without native Params or vehicle IO."""
import ast
from enum import IntEnum
import importlib.util
from pathlib import Path
from types import SimpleNamespace
import unittest
SOURCE = Path(__file__).resolve().parents[1]
spec = importlib.util.spec_from_file_location('nav_turn_intent', SOURCE/'nav_turn_intent.py')
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
NOW = 100.
class Params:
def __init__(self):
self.values = {}
self.writes = 0
def put(self, key, value, **kwargs):
self.values[key] = value
self.writes += 1
def remove(self, key):
self.values.pop(key, None)
def get(self, key, **kwargs):
return self.values.get(key, 19. if key == 'LaneTurnValue' else None)
def get_bool(self, key):
return self.values.get(key) is True
class Direction(IntEnum):
none, turnLeft, turnRight = 0, 1, 2
class ChangeState(IntEnum):
off, preLaneChange, laneChangeStarting = 0, 1, 2
class ChangeDirection(IntEnum):
none, left, right = 0, 1, 2
DESIRE = SimpleNamespace(none=0, turnLeft=1, turnRight=2, laneChangeLeft=3, laneChangeRight=4)
def source_namespace(path, namespace):
tree = ast.parse(path.read_text())
tree.body = [node for node in tree.body if not isinstance(node, (ast.Import, ast.ImportFrom))]
exec(compile(tree, str(path), 'exec'), namespace)
return namespace
def controller(dh=None):
namespace = {'custom': SimpleNamespace(ModelDataV2SP=SimpleNamespace(TurnDirection=Direction)),
'CV': SimpleNamespace(MPH_TO_MS=.44704), 'Params': Params, 'NavTurnIntent': module.NavTurnIntent,
'turn_route_ready': module.turn_route_ready, 'time': SimpleNamespace(monotonic=lambda: NOW)}
obj = source_namespace(SOURCE/'lane_turn_desire.py', namespace)['LaneTurnController'](dh)
obj.navigation_enabled, obj.enabled = True, True
return obj
def ready_context(obj, **overrides):
value = {'session': obj.nav_intent.session, 'direction': obj.nav_intent.direction, 'status': 'ready',
'routeOwner': 'signal', 'locationValid': True, 'headingValid': True, 'updatedAt': NOW,
'routeGeneration': 123, 'distanceM': 25.}
value.update(overrides)
return value
class ALC:
"""Only external ALC eligibility is doubled; the actual DesireHelper FSM executes."""
def __init__(self, _dh):
self.lane_change_set_timer = 1
self.auto_lane_change_allowed = True
self.lane_change_blocked = False
def update_params(self):
pass
def update_lane_change(self, blindspot, edge, brake, torque):
self.lane_change_blocked = blindspot or edge
def update_state(self):
pass
def helper(enabled=True, supported=True):
path = SOURCE.parents[3]/'selfdrive/controls/lib/desire_helper.py'
namespace = {'log': SimpleNamespace(LaneChangeState=ChangeState, LaneChangeDirection=ChangeDirection, Desire=DESIRE),
'custom': SimpleNamespace(ModelDataV2SP=SimpleNamespace(TurnDirection=Direction)),
'CV': SimpleNamespace(MPH_TO_MS=.44704), 'DT_MDL': .05, 'AutoLaneChangeController': ALC,
'AutoLaneChangeMode': SimpleNamespace(OFF=0), 'LaneTurnController': controller,
'model_turn_probs': lambda _prediction: (0., 0.)}
dh = source_namespace(path, namespace)['DesireHelper']()
dh.lane_turn_controller.params.values.update(MapboxNextTurnEnabled=enabled, LaneTurnDesire=True, LaneTurnValue=19.)
car = SimpleNamespace(vEgo=12., leftBlinker=False, rightBlinker=True, leftBlindspot=False,
rightBlindspot=False, steeringPressed=False, steeringTorque=0., brakePressed=False)
def step(speed=None, active=True, **kwargs):
if speed is not None:
car.vEgo = speed
dh.update(car, active, .5, map_turn_supported=supported, **kwargs)
return dh.desire
return dh, car, step
class NavigationSignalTests(unittest.TestCase):
def test_delayed_parameter_writes_cannot_resurrect_canceled_signal(self):
class DeferredParams(Params):
def __init__(self):
super().__init__()
self.pending = []
def put(self, key, value, block=False):
if block:
super().put(key, value)
else:
self.pending.append((key, value))
params = DeferredParams()
intent = module.NavTurnIntent(params)
intent.update('right', True, True, True)
intent.commit('turn')
intent.update(None, True, True, True)
for key, value in params.pending:
params.values[key] = value
self.assertNotIn('MapboxTurnIntent', params.values)
def test_held_signal_does_not_repeat_and_new_signal_gets_new_session(self):
params, counter = Params(), iter(range(100))
intent = module.NavTurnIntent(params, session_clock=lambda: next(counter))
for _ in range(100):
intent.update('right', True, True, True)
self.assertEqual(params.writes, 1)
old = intent.session
intent.update(None, True, True, True)
self.assertNotIn('MapboxTurnIntent', params.values)
intent.update('right', True, True, True)
self.assertNotEqual(intent.session, old)
def test_disable_disengage_or_hazards_retires_published_intent(self):
for args in [('right', False, True, True), ('right', True, True, False), (None, True, True, True)]:
intent = module.NavTurnIntent(Params())
intent.update('right', True, True, True)
intent.update(*args)
self.assertNotIn('MapboxTurnIntent', intent.params.values)
def test_fallback_updates_capability_without_rearming_signal(self):
intent = module.NavTurnIntent(Params())
intent.update('left', True, True, True)
session = intent.session
intent.update('left', True, False, True)
self.assertEqual(intent.session, session)
self.assertIs(intent.params.values['MapboxTurnIntent']['modelSupportsNavigation'], False)
def test_pending_requires_explicit_commit_and_keeps_session(self):
intent = module.NavTurnIntent(Params())
intent.update('right', True, True, True)
session = intent.session
self.assertEqual(intent.mode, 'pending')
self.assertTrue(intent.commit('turn'))
self.assertEqual(intent.session, session)
self.assertEqual(intent.params.values['MapboxTurnIntent']['mode'], 'turn')
self.assertFalse(intent.commit('lane_change'))
def test_lane_change_commit_cancels_route_and_cannot_flip(self):
intent = module.NavTurnIntent(Params())
intent.update('right', True, True, True)
self.assertTrue(intent.commit('lane_change'))
self.assertNotIn('MapboxTurnIntent', intent.params.values)
intent.update('right', True, True, True)
self.assertFalse(intent.commit('turn'))
intent.update(None, True, True, True)
intent.update('right', True, True, True)
self.assertEqual(intent.mode, 'pending')
def test_direct_opposite_signal_starts_new_pending_session(self):
intent = module.NavTurnIntent(Params())
intent.update('left', True, True, True)
intent.commit('turn')
old = intent.session
intent.update('right', True, True, True)
self.assertEqual(intent.mode, 'pending')
self.assertNotEqual(intent.session, old)
def test_real_navigation_never_also_sends_a_turn_now_desire(self):
obj = controller()
for speed in (0., 2., 7., 12.):
obj.update_lane_turn(False, False, False, True, speed, steering_pressed=True,
steering_torque=-5., turn_probs=(0., 1.), map_turn_supported=True)
self.assertEqual(obj.get_turn_direction(), Direction.none)
self.assertEqual(obj.params.values['MapboxTurnIntent']['mode'], 'pending')
def test_navigation_does_not_depend_on_legacy_turn_desire_toggle(self):
obj = controller()
obj.enabled = False
obj.update_lane_turn(False, False, True, False, 2., map_turn_supported=True)
obj.params.values['MapboxTurnContext'] = ready_context(obj)
obj.finalize_navigation_intent(False, 2., True, True)
self.assertEqual(obj.nav_intent.mode, 'turn')
self.assertEqual(obj.get_turn_direction(), Direction.none)
def test_nonnav_model_keeps_legacy_behavior_and_reports_unsupported(self):
obj = controller()
obj.update_lane_turn(False, False, True, False, 2., turn_probs=(1., 0.), map_turn_supported=False)
self.assertEqual(obj.get_turn_direction(), Direction.turnLeft)
self.assertIs(obj.params.values['MapboxTurnIntent']['modelSupportsNavigation'], False)
obj.finalize_navigation_intent(False, 2., True, False)
self.assertEqual(obj.nav_intent.mode, 'pending')
def test_ready_context_rejects_stale_wrong_session_passed_or_invalid(self):
obj = controller()
obj.update_lane_turn(False, False, False, True, 2., map_turn_supported=True)
for overrides in ({'updatedAt': NOW-.351}, {'updatedAt': NOW+.01}, {'session': 'wrong'}, {'direction': 'left'},
{'distanceM': 0.}, {'distanceM': -1.}, {'distanceM': float('nan')}, {'status': 'passed'},
{'routeOwner': 'destination'}, {'headingValid': False}, {'locationValid': False}, {'routeGeneration': True}):
with self.subTest(overrides=overrides):
obj.params.values['MapboxTurnContext'] = ready_context(obj, **overrides)
obj.finalize_navigation_intent(False, 2., True, True)
self.assertEqual(obj.nav_intent.mode, 'pending')
def test_speed_bsm_and_lateral_permission_block_turn_commit(self):
for speed, bsm, active in ((9., False, True), (2., True, True), (2., False, False)):
obj = controller()
obj.update_lane_turn(False, bsm, False, True, speed, map_turn_supported=True)
obj.params.values['MapboxTurnContext'] = ready_context(obj)
obj.finalize_navigation_intent(False, speed, active, True)
self.assertEqual(obj.nav_intent.mode, 'pending')
def test_early_fast_signal_can_slow_to_turn_before_lc_request(self):
dh, _car, step = helper()
self.assertEqual(step(), DESIRE.none)
obj = dh.lane_turn_controller
session = obj.nav_intent.session
self.assertEqual(dh.lane_change_state, ChangeState.preLaneChange)
self.assertEqual(obj.nav_intent.mode, 'pending')
obj.params.values['MapboxTurnContext'] = ready_context(obj)
self.assertEqual(step(2.), DESIRE.none)
self.assertEqual(obj.nav_intent.mode, 'turn')
self.assertEqual(obj.nav_intent.session, session)
self.assertTrue(obj.navigation_owns_signal)
self.assertEqual(step(12.), DESIRE.none)
self.assertEqual(dh.lane_change_state, ChangeState.off)
def test_actual_lane_change_first_remains_lane_change_after_slowing(self):
dh, _car, step = helper()
step()
obj = dh.lane_turn_controller
obj.params.values['MapboxTurnContext'] = ready_context(obj)
self.assertEqual(step(), DESIRE.laneChangeRight)
self.assertEqual(obj.nav_intent.mode, 'lane_change')
self.assertNotIn('MapboxTurnIntent', obj.params.values)
step(2.)
self.assertEqual(obj.nav_intent.mode, 'lane_change')
self.assertFalse(obj.navigation_owns_signal)
def test_pre_lane_change_blocked_by_bsm_does_not_commit(self):
dh, car, step = helper()
step()
car.rightBlindspot = True
self.assertEqual(step(), DESIRE.none)
self.assertEqual(dh.lane_turn_controller.nav_intent.mode, 'pending')
car.rightBlindspot = False
dh.lane_turn_controller.params.values['MapboxTurnContext'] = ready_context(dh.lane_turn_controller)
step(2.)
self.assertEqual(dh.lane_turn_controller.nav_intent.mode, 'turn')
def test_waiting_alc_timer_or_detected_edge_does_not_commit_lane_change(self):
for edge in (False, True):
dh, _car, step = helper()
step()
dh.alc.auto_lane_change_allowed = edge
self.assertEqual(step(right_edge_detected=edge), DESIRE.none)
self.assertEqual(dh.lane_turn_controller.nav_intent.mode, 'pending')
def test_left_and_right_turn_commit_requires_matching_route(self):
for left in (False, True):
dh, car, step = helper()
car.leftBlinker, car.rightBlinker = left, not left
step(2.)
obj = dh.lane_turn_controller
obj.params.values['MapboxTurnContext'] = ready_context(obj)
self.assertEqual(step(), DESIRE.none)
self.assertEqual(obj.nav_intent.mode, 'turn')
self.assertEqual(obj.nav_intent.direction, 'left' if left else 'right')
def test_lane_change_request_wins_same_frame_ready_turn_tie(self):
obj = controller()
obj.update_lane_turn(False, False, False, True, 2., map_turn_supported=True)
obj.params.values['MapboxTurnContext'] = ready_context(obj)
obj.finalize_navigation_intent(True, 2., True, True)
self.assertEqual(obj.nav_intent.mode, 'lane_change')
self.assertNotIn('MapboxTurnIntent', obj.params.values)
def test_feature_enabled_mid_existing_lane_change_preserves_lane_change(self):
dh, _car, step = helper(enabled=False)
step()
self.assertEqual(step(), DESIRE.laneChangeRight)
obj = dh.lane_turn_controller
obj.navigation_enabled = True
self.assertEqual(step(), DESIRE.laneChangeRight)
self.assertEqual(obj.nav_intent.mode, 'lane_change')
def test_feature_off_and_unsupported_preserve_same_lane_change_sequence(self):
traces = []
for enabled, supported in ((False, True), (True, False)):
dh, car, step = helper(enabled, supported)
trace = []
for speed, signal in ((12., True), (12., True), (2., True), (2., False), (12., True), (12., True)):
car.rightBlinker = signal
trace.append((step(speed), dh.lane_change_state, dh.lane_change_direction))
traces.append(trace)
self.assertEqual(*traces)
if __name__ == '__main__':
unittest.main()
openpilot/tools/params/README.md
+4 -2
View File
@@ -19,8 +19,10 @@ the inputs, command, binary hash and checks. Existing wheel downloads are
verified and reused. An unavailable pinned package is a build error.
The checked-in `build_provenance.json` describes the shipped feature build.
Its original 272 key names, types and raw defaults were compared with the
base release; exactly five Mapbox keys were added. The C API exports and dynamic
The previous build preserved all 272 original key names, types and raw defaults
and added five Mapbox keys. The navigation build adds only `NavModelState`;
all 277 previous registry entries were compared against the real ARM64 library
and remain identical. The status JSON clears on manager startup and going offroad. The C API exports and dynamic
library dependencies match the base release, with no increased symbol-version
requirements. The `params_keys_by_flag` correction returns distinct buffers
owned by the handle instead of aliasing one temporary string.
openpilot/tools/params/build_provenance.json
+31 -25
View File
@@ -1,6 +1,6 @@
{
"schemaVersion": 1,
"baseCommit": "f5d861d86d9748908ef6aa4928d7fdb5f0c5df2b",
"baseCommit": "3b1816ae6c1521b91f98907e2034f7ef1bda8c8b",
"sourceTreeIncludesFeatureChanges": true,
"builder": {
"architecture": "native aarch64 Linux",
@@ -14,7 +14,7 @@
"openpilot/common/params.h": "3c7b87d050c79275a401597dc7d15ae9fa57c2316c816ff5472cbdd84588a6e6",
"openpilot/common/params.cc": "0074bdf766e9bf410f21820b8f730ed9403117c54725b6193176f28a14233737",
"openpilot/common/params_c.cc": "7c3c86301ab3d65aa0eecc3f7b61c3baf4730baf72a9166cbb4de4237419a48d",
"openpilot/common/params_keys.h": "d456c7933aee031d61515a2a9948a908655b5931169f23975d42700b086d869e",
"openpilot/common/params_keys.h": "738e4fa928acbd2980a9aac590320f1b8bed4fe1a98ed06db45b890114f37fdc",
"openpilot/common/queue.h": "f7c75c847ae63dfd6e864406e722248540a79908270e85ca2674b43407408fcf",
"openpilot/common/timing.h": "66a580e240fa4fe4af03bc3073ddd0f4db5d1027720e8adfa92ba448f7c8bc71",
"openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
@@ -57,8 +57,8 @@
],
"rebuildTools": {
"Dockerfile": "c60c96043ea7b6d8ad94ab4e9181033606ae35e4fb43e034ab38b7505d867cff",
"README.md": "aa504aa3fa9bbfd49d89d5fefaf3fc58135c5622f1ff56ca0714ebdde1fdf1c8",
"verify.py": "5cbc0cdb9af80c07c04aa2a1ba3fa1fda3f3ef33d725464e16281a9d8b37fa68",
"README.md": "65b95ef339e8a0183c7030cf759d042c128c0ba846080fecfb5c19740db1cda7",
"verify.py": "b068578579c1694c7b1594deca244917894ba43d69a80651058280c9a60a1b9f",
"rebuild.py": "62f25ff2e6bd7daa9a635d6a6f49b7fdf03533370f2f75296d77003bed01d765"
},
"build": {
@@ -94,8 +94,8 @@
],
"compiler": "g++ (Debian 12.2.0-14+deb12u1) 12.2.0",
"machine": "aarch64",
"sha256": "8096350b61b400bd987394d58520724102e372b63429548bac1d5fdcb0dfd9fb",
"bytes": 4129336
"sha256": "4391c55759edb28c41987914e5b02c7fa3df7a4c499a884d67be1f1b16cac511",
"bytes": 4132776
},
"verification": {
"originalTestMethods": [
@@ -148,19 +148,6 @@
],
"hardwareExecuted": false
},
"originalRegistryCompatibility": {
"originalKeyCount": 272,
"rebuiltKeyCount": 277,
"exactAddedKeys": [
"MapboxNextTurnEnabled",
"MapboxTurnContext",
"MapboxTurnCue",
"MapboxTurnIntent",
"MapboxTurnLeadDistance"
],
"allOriginalNamesTypesDefaultsUnchanged": true,
"executionArchitecture": "nativeaarch64Linux"
},
"abiComparison": {
"baseline": {
"sha256": "4ae3277dccb111ae65db482f3b04e85735eb215158d41160742aadf2231605b0",
@@ -194,7 +181,7 @@
]
},
"rebuilt": {
"sha256": "8096350b61b400bd987394d58520724102e372b63429548bac1d5fdcb0dfd9fb",
"sha256": "4391c55759edb28c41987914e5b02c7fa3df7a4c499a884d67be1f1b16cac511",
"needed": [
"libstdc++.so.6",
"libgcc_s.so.1",
@@ -228,13 +215,32 @@
"noHigherSymbolVersionRequirement": true,
"hardwareExecuted": false
},
"reproduction": {
"sameBinaryFromInitialAndPinnedBuilder": true,
"sha256": "8096350b61b400bd987394d58520724102e372b63429548bac1d5fdcb0dfd9fb"
},
"limits": [
"Executed under ARM64 Linux containers, not on the vehicle device.",
"Original Params method bodies use isolated test-directory scaffolding and a logging stub.",
"No model, steering, Tesla, cooperative steering, CAN or safety binaries were rebuilt."
]
],
"previousRegistryCompatibility": {
"originalKeyCount": 272,
"rebuiltKeyCount": 277,
"exactAddedKeys": [
"MapboxNextTurnEnabled",
"MapboxTurnContext",
"MapboxTurnCue",
"MapboxTurnIntent",
"MapboxTurnLeadDistance"
],
"allOriginalNamesTypesDefaultsUnchanged": true,
"executionArchitecture": "nativeaarch64Linux"
},
"registryCompatibility": {
"originalKeyCount": 277,
"rebuiltKeyCount": 278,
"exactAddedKeys": [
"NavModelState"
],
"allOriginalNamesTypesDefaultsUnchanged": true,
"executionArchitecture": "nativeaarch64Linux",
"baselineCommit": "3b1816ae6c1521b91f98907e2034f7ef1bda8c8b"
}
}
openpilot/tools/params/verify.py
+6 -1
View File
@@ -59,7 +59,7 @@ def run():
with tempfile.TemporaryDirectory() as directory:
params = native.Params(directory)
expected = {'MapboxNextTurnEnabled': ('BOOL', False), 'MapboxTurnLeadDistance': ('FLOAT', 20.0),
**dict.fromkeys(events, ('JSON', None))}
**dict.fromkeys(events + ['NavModelState'], ('JSON', None))}
assert {key: (params.get_type(key).name, params.get_default_value(key)) for key in expected} == expected
params.put_bool('MapboxNextTurnEnabled', True, block=True)
params.put('MapboxTurnLeadDistance', 20., block=True)
@@ -71,6 +71,11 @@ def run():
assert all(params.get(key) is None for key in events)
assert params.get_bool('MapboxNextTurnEnabled') is True
assert params.get('MapboxTurnLeadDistance') == 20.
for flag in (native.ParamKeyFlag.CLEAR_ON_MANAGER_START, native.ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION):
params.put('NavModelState', {'supported': True, 'active': False}, block=True)
assert params.get('NavModelState') == {'supported': True, 'active': False}
params.clear_all(flag)
assert params.get('NavModelState') is None
params.put(events[0], {'session': 'async-test'}, block=False)
deadline = time.monotonic() + 2.
while params.get(events[0]) != {'session': 'async-test'} and time.monotonic() < deadline: