diff --git a/openpilot/common/libparams_c.so b/openpilot/common/libparams_c.so index 4c4149defb..22b862e0ce 100755 Binary files a/openpilot/common/libparams_c.so and b/openpilot/common/libparams_c.so differ diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 0cd5472690..09a9867a3c 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -262,6 +262,7 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/selfdrive/controls/lib/desire_helper.py b/openpilot/selfdrive/controls/lib/desire_helper.py index 7c89012f05..a72a81ef89 100644 --- a/openpilot/selfdrive/controls/lib/desire_helper.py +++ b/openpilot/selfdrive/controls/lib/desire_helper.py @@ -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() diff --git a/openpilot/selfdrive/navigation/SIGNAL_NAVIGATION.md b/openpilot/selfdrive/navigation/SIGNAL_NAVIGATION.md new file mode 100644 index 0000000000..5e985f5c81 --- /dev/null +++ b/openpilot/selfdrive/navigation/SIGNAL_NAVIGATION.md @@ -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. diff --git a/openpilot/selfdrive/navigation/mapbox_next_turn.py b/openpilot/selfdrive/navigation/mapbox_next_turn.py index 7d1003254b..647f23d292 100644 --- a/openpilot/selfdrive/navigation/mapbox_next_turn.py +++ b/openpilot/selfdrive/navigation/mapbox_next_turn.py @@ -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)) diff --git a/openpilot/selfdrive/navigation/mapsd.py b/openpilot/selfdrive/navigation/mapsd.py index 43bd7c4f75..fd8301d392 100755 --- a/openpilot/selfdrive/navigation/mapsd.py +++ b/openpilot/selfdrive/navigation/mapsd.py @@ -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() diff --git a/openpilot/selfdrive/navigation/navd.py b/openpilot/selfdrive/navigation/navd.py index 390afc1a64..c8af95ae99 100755 --- a/openpilot/selfdrive/navigation/navd.py +++ b/openpilot/selfdrive/navigation/navd.py @@ -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) diff --git a/openpilot/selfdrive/navigation/tests/test_mapbox_next_turn.py b/openpilot/selfdrive/navigation/tests/test_mapbox_next_turn.py index 96846f373e..9411cb1feb 100644 --- a/openpilot/selfdrive/navigation/tests/test_mapbox_next_turn.py +++ b/openpilot/selfdrive/navigation/tests/test_mapbox_next_turn.py @@ -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.) diff --git a/openpilot/selfdrive/navigation/tests/test_navd_next_turn.py b/openpilot/selfdrive/navigation/tests/test_navd_next_turn.py index 03791f5822..37de89535b 100644 --- a/openpilot/selfdrive/navigation/tests/test_navd_next_turn.py +++ b/openpilot/selfdrive/navigation/tests/test_navd_next_turn.py @@ -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') diff --git a/openpilot/selfdrive/navigation/tests/test_navigation.py b/openpilot/selfdrive/navigation/tests/test_navigation.py index 382bba5e86..84d2beee03 100644 --- a/openpilot/selfdrive/navigation/tests/test_navigation.py +++ b/openpilot/selfdrive/navigation/tests/test_navigation.py @@ -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): diff --git a/openpilot/selfdrive/navigation/tests/test_signal_mini_route.py b/openpilot/selfdrive/navigation/tests/test_signal_mini_route.py new file mode 100644 index 0000000000..480437cfba --- /dev/null +++ b/openpilot/selfdrive/navigation/tests/test_signal_mini_route.py @@ -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() diff --git a/openpilot/selfdrive/navigation/tests/test_signal_turn_status_ui.py b/openpilot/selfdrive/navigation/tests/test_signal_turn_status_ui.py index f4cdabe2f3..9d0589133f 100644 --- a/openpilot/selfdrive/navigation/tests/test_signal_turn_status_ui.py +++ b/openpilot/selfdrive/navigation/tests/test_signal_turn_status_ui.py @@ -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') diff --git a/openpilot/selfdrive/ui/mici/onroad/navigation.py b/openpilot/selfdrive/ui/mici/onroad/navigation.py index b5312a504c..8f91f381f8 100644 --- a/openpilot/selfdrive/ui/mici/onroad/navigation.py +++ b/openpilot/selfdrive/ui/mici/onroad/navigation.py @@ -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)) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py index 24bc6abecc..c0eedbdf76 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py @@ -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): diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index 7aa1489fae..2aad0b3a12 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -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 diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 3177b3cf59..01e5701139 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -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 diff --git a/openpilot/sunnypilot/modeld_v2/navigation_input.py b/openpilot/sunnypilot/modeld_v2/navigation_input.py new file mode 100644 index 0000000000..0d11ecb19e --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/navigation_input.py @@ -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 diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_navigation_input.py b/openpilot/sunnypilot/modeld_v2/tests/test_navigation_input.py new file mode 100644 index 0000000000..049d28e9af --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/tests/test_navigation_input.py @@ -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() diff --git a/openpilot/sunnypilot/models/NAVIGATION_MODEL.md b/openpilot/sunnypilot/models/NAVIGATION_MODEL.md new file mode 100644 index 0000000000..80cb1046c0 --- /dev/null +++ b/openpilot/sunnypilot/models/NAVIGATION_MODEL.md @@ -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. diff --git a/openpilot/sunnypilot/models/build_navigation_model.py b/openpilot/sunnypilot/models/build_navigation_model.py new file mode 100644 index 0000000000..8a67402c5f --- /dev/null +++ b/openpilot/sunnypilot/models/build_navigation_model.py @@ -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() diff --git a/openpilot/sunnypilot/models/fetcher.py b/openpilot/sunnypilot/models/fetcher.py index 0c40c9e598..ece86ed7ee 100644 --- a/openpilot/sunnypilot/models/fetcher.py +++ b/openpilot/sunnypilot/models/fetcher.py @@ -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__": diff --git a/openpilot/sunnypilot/models/navigation_model.py b/openpilot/sunnypilot/models/navigation_model.py new file mode 100644 index 0000000000..08ee80a3d7 --- /dev/null +++ b/openpilot/sunnypilot/models/navigation_model.py @@ -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 diff --git a/openpilot/sunnypilot/models/tests/test_navigation_model.py b/openpilot/sunnypilot/models/tests/test_navigation_model.py new file mode 100644 index 0000000000..90ef3f1131 --- /dev/null +++ b/openpilot/sunnypilot/models/tests/test_navigation_model.py @@ -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,), ' 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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/map_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/map_turn_desire.py index a057674862..300ea5b586 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/map_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/map_turn_desire.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/nav_turn_intent.py b/openpilot/sunnypilot/selfdrive/controls/lib/nav_turn_intent.py new file mode 100644 index 0000000000..5940eb0535 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/nav_turn_intent.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_map_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_map_turn_desire.py index a70caf888d..287c2109cf 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_map_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_map_turn_desire.py @@ -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() diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_nav_turn_intent.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_nav_turn_intent.py new file mode 100644 index 0000000000..ecb6892f3c --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_nav_turn_intent.py @@ -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() diff --git a/openpilot/tools/params/README.md b/openpilot/tools/params/README.md index abbf176c99..0083cc6057 100644 --- a/openpilot/tools/params/README.md +++ b/openpilot/tools/params/README.md @@ -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. diff --git a/openpilot/tools/params/build_provenance.json b/openpilot/tools/params/build_provenance.json index 7783ecf949..d53afa7d27 100644 --- a/openpilot/tools/params/build_provenance.json +++ b/openpilot/tools/params/build_provenance.json @@ -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" + } } diff --git a/openpilot/tools/params/verify.py b/openpilot/tools/params/verify.py index 5b30124c5a..cb4d9fb90a 100644 --- a/openpilot/tools/params/verify.py +++ b/openpilot/tools/params/verify.py @@ -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: