c3 ui fix. (#294)

This commit is contained in:
carrot
2026-04-18 12:45:41 +09:00
committed by GitHub
parent 9ff6013d69
commit 2547682e71
34 changed files with 1368 additions and 452 deletions

View File

@@ -107,6 +107,7 @@ class CarState(CarStateBase):
self.manual_speed_limit_assist = None
self.accelerator = None
self.blinkers = None
self.blinkers_alt = None
self.doors_seatbelts = None
self.cruise_buttons_alt2 = None
@@ -248,6 +249,7 @@ class CarState(CarStateBase):
if self.gear_msg_canfd == "ACCELERATOR":
add_and_cache(self.cp, "ACCELERATOR", "accelerator", ignore_counter = True)
add_and_cache(self.cp, "BLINKERS", "blinkers")
add_and_cache(self.cp, "BLINKERS_ALT", "blinkers_alt")
add_and_cache(self.cp, "DOORS_SEATBELTS", "doors_seatbelts")
elif self.controls_ready_count == 126:
add_and_cache(self.cp, "CRUISE_BUTTONS_ALT2", "cruise_buttons_alt2", ignore_counter = True)
@@ -534,8 +536,8 @@ class CarState(CarStateBase):
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 or cp.vl["MDPS"]["LFA2_FAULT"] != 0
#ret.steerFaultTemporary = False
if self.blinkers is not None:
blinkers_info = self.blinkers
blinkers_info = self.blinkers if self.blinkers is not None else self.blinkers_alt if self.blinkers_alt is not None else None
if blinkers_info is not None:
left_blinker_lamp = blinkers_info["LEFT_LAMP"] or blinkers_info["LEFT_LAMP_ALT"]
right_blinker_lamp = blinkers_info["RIGHT_LAMP"] or blinkers_info["RIGHT_LAMP_ALT"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, left_blinker_lamp, right_blinker_lamp)

View File

@@ -766,6 +766,16 @@ BO_ 961 BLINKER_STALKS: 8 XXX
SG_ LEFT_BLINKER : 30|1@0+ (1,0) [0|1] "" XXX
SG_ LIGHT_KNOB_POSITION : 21|2@0+ (1,0) [0|3] "" XXX
BO_ 995 BLINKERS_ALT: 16 XXX
SG_ LEFT_BLINKER : 74|1@0+ (1,0) [0|1] "" XXX
SG_ RIGHT_BLINKER : 76|1@0+ (1,0) [0|1] "" XXX
SG_ LEFT_LAMP_ALT : 89|1@0+ (1,0) [0|1] "" XXX
SG_ LEFT_LAMP : 90|1@0+ (1,0) [0|1] "" XXX
SG_ RIGHT_LAMP_ALT : 91|1@0+ (1,0) [0|1] "" XXX
SG_ RIGHT_LAMP : 92|1@0+ (1,0) [0|1] "" XXX
SG_ NEW_SIGNAL_2 : 95|1@0+ (1,0) [0|1] "" XXX
SG_ BLINKER_ACTIVE : 97|1@0+ (1,0) [0|1] "" XXX
BO_ 1041 DOORS_SEATBELTS: 8 XXX
SG_ CHECKSUM_MAYBE : 7|8@0+ (1,0) [0|65535] "" XXX
SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX
@@ -851,6 +861,12 @@ BO_ 687 STEER_TOUCH_2AF: 8 XXX
SG_ NEW_SIGNAL_3 : 55|8@0+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_4 : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 1052 LIGHT_SENSOR: 8 XXX
SG_ CHECKSUM : 7|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
SG_ IS_DARK : 19|1@0+ (1,0) [0|1] "" XXX
SG_ LIGHT_LEVEL : 24|10@1+ (1,0) [0|1023] "" XXX
BO_ 1144 DRIVE_MODE: 8 XXX
SG_ DRIVE_MODE : 0|16@1+ (1,-61611) [0|61611] "" XXX
SG_ DRIVE_MODE2 : 28|3@1+ (1,0) [1|3] "" XXX

View File

@@ -3,7 +3,7 @@ docker_out/
process_replay/diff.txt
process_replay/model_diff.txt
process_replay/fakedata/
valgrind_logs.txt
*.bz2
*.hevc

View File

@@ -1,12 +1,14 @@
#!/usr/bin/env bash
set -e
# To build sim and docs, you can run the following to mount the scons cache to the same place as in CI:
# mkdir -p .ci_cache/scons_cache
# sudo mount --bind /tmp/scons_cache/ .ci_cache/scons_cache
SCRIPT_DIR=$(dirname "$0")
OPENPILOT_DIR=$SCRIPT_DIR/../../
DOCKER_IMAGE=openpilot
DOCKER_FILE=Dockerfile.openpilot
DOCKER_REGISTRY=ghcr.io/commaai
COMMIT_SHA=$(git rev-parse HEAD)
if [ -n "$TARGET_ARCHITECTURE" ]; then
PLATFORM="linux/$TARGET_ARCHITECTURE"
TAG_SUFFIX="-$TARGET_ARCHITECTURE"
@@ -15,9 +17,11 @@ else
TAG_SUFFIX=""
fi
source $SCRIPT_DIR/docker_common.sh $1 "$TAG_SUFFIX"
LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX
REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG
REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA
DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR
DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR
if [ -n "$PUSH_IMAGE" ]; then
docker push $REMOTE_TAG

View File

@@ -44,10 +44,10 @@ class FuzzyGenerator:
except capnp.lib.capnp.KjException:
return self.generate_struct(field.schema)
def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: str = None) -> st.SearchStrategy[dict[str, Any]]:
def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: str | None = None) -> st.SearchStrategy[dict[str, Any]]:
single_fill: tuple[str, ...] = (event,) if event else (self.draw(st.sampled_from(schema.union_fields)),) if schema.union_fields else ()
fields_to_generate = schema.non_union_fields + single_fill
return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in fields_to_generate if not field.endswith('DEPRECATED')})
fields_to_generate = [f for f in schema.non_union_fields + single_fill if not f.endswith('DEPRECATED') and f != 'deprecated']
return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in fields_to_generate})
@staticmethod
@cache

View File

@@ -37,6 +37,43 @@ def release_only(f):
return wrap
def collect_logs(services, duration):
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
logs = []
start = time.monotonic()
while time.monotonic() - start < duration:
for s in socks:
logs.extend(messaging.drain_sock(s))
return logs
@contextlib.contextmanager
def log_collector(services):
"""Background thread that continuously drains messages from services.
Use when the main thread needs to do blocking work (e.g. capturing images)."""
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
raw_logs = []
lock = threading.Lock()
stop_event = threading.Event()
def _drain():
while not stop_event.is_set():
for s in socks:
msgs = messaging.drain_sock(s)
if msgs:
with lock:
raw_logs.extend(msgs)
time.sleep(0.01)
thread = threading.Thread(target=_drain, daemon=True)
thread.start()
try:
yield raw_logs, lock
finally:
stop_event.set()
thread.join(timeout=2)
@contextlib.contextmanager
def processes_context(processes, init_time=0, ignore_stopped=None):
ignore_stopped = [] if ignore_stopped is None else ignore_stopped

View File

@@ -60,7 +60,8 @@ class Maneuver:
log['distance_lead'],
log['speed'],
speed_lead,
log['acceleration']]))
log['acceleration'],
log['d_rel']]))
if d_rel < .4 and (self.only_radar or prob_lead > 0.5):
print("Crashed!!!!")

View File

@@ -107,6 +107,7 @@ class Plant:
position = log.XYZTData.new_message()
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
model.modelV2.position = position
model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
velocity = log.XYZTData.new_message()
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
velocity.x[0] = float(self.speed) # always start at current speed

View File

@@ -1,5 +1,5 @@
import itertools
from parameterized import parameterized_class
from openpilot.common.parameterized import parameterized_class
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver

View File

@@ -5,7 +5,7 @@ Process replay is a regression test designed to identify any changes in the outp
If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated.
Use `test_processes.py` to run the test locally.
Use `FILEREADER_CACHE='1' test_processes.py` to cache log files.
Log files are cached by default. Use `DISABLE_FILEREADER_CACHE='1' test_processes.py` to disable caching.
Currently the following processes are tested:
@@ -22,7 +22,7 @@ Currently the following processes are tested:
### Usage
```
Usage: test_processes.py [-h] [--whitelist-procs PROCS] [--whitelist-cars CARS] [--blacklist-procs PROCS]
[--blacklist-cars CARS] [--ignore-fields FIELDS] [--ignore-msgs MSGS] [--update-refs] [--upload-only]
[--blacklist-cars CARS] [--ignore-fields FIELDS] [--ignore-msgs MSGS] [--update-refs]
Regression test to identify changes in a process's output
optional arguments:
-h, --help show this help message and exit
@@ -33,12 +33,11 @@ optional arguments:
--ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. driverMonitoringState.events)
--ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents)
--update-refs Updates reference logs using current commit
--upload-only Skips testing processes and uploads logs from previous test run
```
## Forks
openpilot forks can use this test with their own reference logs, by default `test_proccess.py` saves logs locally.
openpilot forks can use this test with their own reference logs, by default `test_proccesses.py` saves logs locally.
To generate new logs:

View File

@@ -3,13 +3,16 @@ import sys
import math
import capnp
import numbers
import dictdiffer
from collections import Counter
from openpilot.tools.lib.logreader import LogReader
EPSILON = sys.float_info.epsilon
_DynamicStructReader = capnp.lib.capnp._DynamicStructReader
_DynamicListReader = capnp.lib.capnp._DynamicListReader
_DynamicEnum = capnp.lib.capnp._DynamicEnum
def remove_ignored_fields(msg, ignore):
msg = msg.as_builder()
@@ -39,6 +42,61 @@ def remove_ignored_fields(msg, ignore):
return msg
def _diff_capnp(r1, r2, path, tolerance):
"""Walk two capnp struct readers and yield (action, dotted_path, value) diffs.
Floats are compared with the given tolerance (combined absolute+relative).
"""
schema = r1.schema
for fname in schema.non_union_fields:
child_path = path + (fname,)
v1 = getattr(r1, fname)
v2 = getattr(r2, fname)
yield from _diff_capnp_values(v1, v2, child_path, tolerance)
if schema.union_fields:
w1, w2 = r1.which(), r2.which()
if w1 != w2:
yield 'change', '.'.join(path), (w1, w2)
else:
child_path = path + (w1,)
v1, v2 = getattr(r1, w1), getattr(r2, w2)
yield from _diff_capnp_values(v1, v2, child_path, tolerance)
def _diff_capnp_values(v1, v2, path, tolerance):
if isinstance(v1, _DynamicStructReader):
yield from _diff_capnp(v1, v2, path, tolerance)
elif isinstance(v1, _DynamicListReader):
dot = '.'.join(path)
n1, n2 = len(v1), len(v2)
n = min(n1, n2)
for i in range(n):
yield from _diff_capnp_values(v1[i], v2[i], path + (str(i),), tolerance)
if n2 > n:
yield 'add', dot, list(enumerate(v2[n:], n))
if n1 > n:
yield 'remove', dot, list(reversed([(i, v1[i]) for i in range(n, n1)]))
elif isinstance(v1, _DynamicEnum):
s1, s2 = str(v1), str(v2)
if s1 != s2:
yield 'change', '.'.join(path), (s1, s2)
elif isinstance(v1, float):
if not (v1 == v2 or (
math.isfinite(v1) and math.isfinite(v2) and
abs(v1 - v2) <= max(tolerance, tolerance * max(abs(v1), abs(v2)))
)):
yield 'change', '.'.join(path), (v1, v2)
else:
if v1 != v2:
yield 'change', '.'.join(path), (v1, v2)
def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None,):
if ignore_fields is None:
ignore_fields = []
@@ -65,26 +123,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
msg2 = remove_ignored_fields(msg2, ignore_fields)
if msg1.to_bytes() != msg2.to_bytes():
msg1_dict = msg1.as_reader().to_dict(verbose=True)
msg2_dict = msg2.as_reader().to_dict(verbose=True)
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields)
# Dictdiffer only supports relative tolerance, we also want to check for absolute
# TODO: add this to dictdiffer
def outside_tolerance(diff):
try:
if diff[0] == "change":
a, b = diff[2]
finite = math.isfinite(a) and math.isfinite(b)
if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number):
return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b)))
except TypeError:
pass
return True
dd = list(filter(outside_tolerance, dd))
dd = list(_diff_capnp(msg1.as_reader(), msg2.as_reader(), (), tolerance))
diff.extend(dd)
return diff

View File

@@ -0,0 +1,92 @@
import os
from collections import defaultdict
from opendbc.car.tests.car_diff import format_diff, format_numeric_diffs
from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs
from openpilot.selfdrive.test.process_replay.process_replay import PROC_REPLAY_DIR
class MsgWrap:
"""Adapter so to_dict() includes defaults"""
def __init__(self, msg):
self._msg = msg
def to_dict(self) -> dict:
return self._msg.to_dict(verbose=True)
def diff_process(cfg, ref_msgs, new_msgs) -> tuple | None:
ref = defaultdict(list)
new = defaultdict(list)
for m in ref_msgs:
if m.which() in cfg.subs:
ref[m.which()].append(m)
for m in new_msgs:
if m.which() in cfg.subs:
new[m.which()].append(m)
diffs = []
for sub in cfg.subs:
if len(ref[sub]) != len(new[sub]):
diffs.append((f"{sub} (message count)", 0, (len(ref[sub]), len(new[sub])), 0))
for i, (r, n) in enumerate(zip(ref[sub], new[sub], strict=False)):
for d in compare_logs([r], [n], cfg.ignore, tolerance=cfg.tolerance):
if d[0] == "change":
a, b = d[2]
if a != a and b != b:
continue
diffs.append((d[1], i, d[2], r.logMonoTime))
elif d[0] in ("add", "remove"):
for item in d[2]:
if item[1] != item[1]:
continue
diffs.append((f"{d[1]}.{item[0]}", i, (d[0], item[1]), r.logMonoTime))
return (diffs, ref, new) if diffs else None
def diff_format(diffs, ref, new, field) -> list[str]:
if any(part.isdigit() for part in field.split(".")):
return format_numeric_diffs(diffs)
msg_type = field.split(".")[0]
ref_ts = [(m.logMonoTime, MsgWrap(m)) for m in ref.get(msg_type, [])]
new_wrapped = [MsgWrap(m) for m in new.get(msg_type, [])]
return format_diff(diffs, ref_ts, new_wrapped, field)
def diff_report(replay_diffs, segments) -> None:
seg_to_plat = {seg: plat for plat, seg in segments}
with_diffs, errors, n_passed = [], [], 0
for seg, proc, data in replay_diffs:
plat = seg_to_plat.get(seg, "UNKNOWN")
if data is None:
n_passed += 1
elif isinstance(data, str):
errors.append((plat, seg, proc, data))
else:
with_diffs.append((plat, seg, proc, data))
icon = "⚠️" if with_diffs else ""
lines = [
"## Process replay diff report",
"Replays driving segments through this PR and compares the behavior to master.",
"Please review any changes carefully to ensure they are expected.\n",
f"{icon} {len(with_diffs)} changed, {n_passed} passed, {len(errors)} errors",
]
for plat, seg, proc, err in errors:
lines.append(f"\nERROR {plat} - {seg} [{proc}]: {err}")
if with_diffs:
lines.append("<details><summary><b>Show changes</b></summary>\n\n```")
for plat, seg, proc, (diffs, ref, new) in with_diffs:
lines.append(f"\n{plat} - {seg} [{proc}]")
by_field = defaultdict(list)
for d in diffs:
by_field[d[0]].append(d)
for field, fd in sorted(by_field.items()):
lines.append(f"\n {field} ({len(fd)} diffs)")
lines.extend(diff_format(fd, ref, new, field))
lines.append("```\n</details>")
with open(os.path.join(PROC_REPLAY_DIR, "diff_report.txt"), "w") as f:
f.write("\n".join(lines))

View File

@@ -1,5 +1,6 @@
from collections import defaultdict
from collections.abc import Callable
from typing import cast
import capnp
import functools
import traceback
@@ -9,10 +10,11 @@ from opendbc.car.fingerprints import MIGRATION
from opendbc.car.toyota.values import EPS_SCALE, ToyotaSafetyFlags
from opendbc.car.ford.values import CAR as FORD, FordFlags, FordSafetyFlags
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.gm.values import GMSafetyFlags
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX
from openpilot.system.manager.process_config import managed_processes
from openpilot.tools.lib.logreader import LogIterable
@@ -21,12 +23,12 @@ MigrationOps = tuple[list[tuple[int, capnp.lib.capnp._DynamicStructReader]], lis
MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps]
## rules for migration functions
## 1. must use the decorator @migration(inputs=[...], product="...") and MigrationFunc signature
## 2. it only gets the messages that are in the inputs list
## 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr
## 4. it must return a list of operations to be applied to the logreader (replace, add, delete)
## 5. all migration functions must be independent of each other
# rules for migration functions
# 1. must use the decorator @migration(inputs=[...], product="...") and MigrationFunc signature
# 2. it only gets the messages that are in the inputs list
# 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr
# 4. it must return a list of operations to be applied to the logreader (replace, add, delete)
# 5. all migration functions must be independent of each other
def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False):
migrations = [
migrate_sensorEvents,
@@ -37,6 +39,7 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
migrate_controlsState,
migrate_carState,
migrate_liveLocationKalman,
migrate_livePose,
migrate_liveTracks,
migrate_driverAssistance,
migrate_drivingModelData,
@@ -66,7 +69,7 @@ def migrate(lr: LogIterable, migration_funcs: list[MigrationFunc]):
if migration.product in grouped: # skip if product already exists
continue
sorted_indices = sorted(ii for i in migration.inputs for ii in grouped[i])
sorted_indices = sorted(ii for i in cast(list[str], migration.inputs) for ii in grouped.get(i, []))
msg_gen = [(i, lr[i]) for i in sorted_indices]
r_ops, a_ops, d_ops = migration(msg_gen)
replace_ops.extend(r_ops)
@@ -95,6 +98,17 @@ def migration(inputs: list[str], product: str|None=None):
return decorator
def migrate_onroad_event(event: capnp.lib.capnp._DynamicStructReader):
event_dict = event.to_dict()
try:
return log.OnroadEvent(**event_dict)
except capnp.lib.capnp.KjException as e:
# Ignore legacy events the current schema no longer defines.
if "enum has no such enumerant" in str(e):
return None
raise
@migration(inputs=["longitudinalPlan", "carParams"])
def migrate_longitudinalPlan(msgs):
ops = []
@@ -108,7 +122,7 @@ def migrate_longitudinalPlan(msgs):
if msg.which() != 'longitudinalPlan':
continue
new_msg = msg.as_builder()
a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels)
a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX)
new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop)
ops.append((index, new_msg.as_reader()))
return ops, [], []
@@ -173,6 +187,7 @@ def migrate_liveLocationKalman(msgs):
m = messaging.new_message('livePose')
m.valid = msg.valid
m.logMonoTime = msg.logMonoTime
m.livePose.timestamp = msg.logMonoTime
for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]:
lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field)
lp_field.x, lp_field.y, lp_field.z = llk_field.value or nans
@@ -184,6 +199,21 @@ def migrate_liveLocationKalman(msgs):
return ops, [], []
@migration(inputs=["livePose"])
def migrate_livePose(msgs):
ops = []
needs_migration = all(msg.livePose.timestamp == 0 for _, msg in msgs if msg.which() == 'livePose')
if not needs_migration:
return [], [], []
for index, msg in msgs:
if msg.which() == "livePose":
new_msg = msg.as_builder()
new_msg.livePose.timestamp = msg.logMonoTime
ops.append((index, new_msg.as_reader()))
return ops, [], []
@migration(inputs=["controlsState"], product="selfdriveState")
def migrate_controlsState(msgs):
add_ops = []
@@ -195,7 +225,7 @@ def migrate_controlsState(msgs):
for field in ("enabled", "active", "state", "engageable", "alertText1", "alertText2",
"alertStatus", "alertSize", "alertType", "experimentalMode",
"personality"):
setattr(ss, field, getattr(msg.controlsState, field+"DEPRECATED"))
setattr(ss, field, getattr(msg.controlsState.deprecated, field))
add_ops.append(m.as_reader())
return [], add_ops, []
@@ -208,10 +238,10 @@ def migrate_carState(msgs):
if msg.which() == 'controlsState':
last_cs = msg
elif msg.which() == 'carState' and last_cs is not None:
if last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruise > 0.1:
if last_cs.controlsState.deprecated.vCruise - msg.carState.vCruise > 0.1:
msg = msg.as_builder()
msg.carState.vCruise = last_cs.controlsState.vCruiseDEPRECATED
msg.carState.vCruiseCluster = last_cs.controlsState.vCruiseClusterDEPRECATED
msg.carState.vCruise = last_cs.controlsState.deprecated.vCruise
msg.carState.vCruiseCluster = last_cs.controlsState.deprecated.vCruiseCluster
ops.append((index, msg.as_reader()))
return ops, [], []
@@ -241,14 +271,16 @@ def migrate_gpsLocation(msgs):
@migration(inputs=["deviceState", "initData"])
def migrate_deviceState(msgs):
init_data = next((m.initData for _, m in msgs if m.which() == 'initData'), None)
device_state = next((m.deviceState for _, m in msgs if m.which() == 'deviceState'), None)
if init_data is None or device_state is None:
return [], [], []
ops = []
dt = None
for i, msg in msgs:
if msg.which() == 'initData':
dt = msg.initData.deviceType
if msg.which() == 'deviceState':
n = msg.as_builder()
n.deviceState.deviceType = dt
n.deviceState.deviceType = init_data.deviceType
ops.append((i, n.as_reader()))
return ops, [], []
@@ -271,10 +303,12 @@ def migrate_pandaStates(msgs):
safety_param_migration = {
"TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | ToyotaSafetyFlags.STOCK_LONGITUDINAL,
"TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | ToyotaSafetyFlags.ALT_BRAKE,
"KIA_EV6": HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CANFD_LKA_STEERING,
"KIA_EV6": HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CANFD_LKA_STEER_MSG,
"CHEVROLET_VOLT": GMSafetyFlags.EV,
"CHEVROLET_BOLT_EUV": GMSafetyFlags.EV | GMSafetyFlags.HW_CAM,
}
# TODO: get new Ford route
safety_param_migration |= {car: FordSafetyFlags.LONG_CONTROL for car in (set(FORD) - FORD.with_flags(FordFlags.CANFD))}
safety_param_migration |= dict.fromkeys((set(FORD) - FORD.with_flags(FordFlags.CANFD)), FordSafetyFlags.LONG_CONTROL)
# Migrate safety param base on carParams
CP = next((m.carParams for _, m in msgs if m.which() == 'carParams'), None)
@@ -301,6 +335,8 @@ def migrate_pandaStates(msgs):
elif msg.which() == 'pandaStates':
new_msg = msg.as_builder()
new_msg.pandaStates[-1].safetyParam = safety_param
# Clear DISABLE_DISENGAGE_ON_GAS bit to fix controls mismatch
new_msg.pandaStates[-1].alternativeExperience &= ~1
ops.append((index, new_msg.as_reader()))
return ops, [], []
@@ -431,12 +467,13 @@ def migrate_onroadEvents(msgs):
for event in msg.onroadEventsDEPRECATED:
try:
if not str(event.name).endswith('DEPRECATED'):
# dict converts name enum into string representation
onroadEvents.append(log.OnroadEvent(**event.to_dict()))
migrated_event = migrate_onroad_event(event)
if migrated_event is not None:
onroadEvents.append(migrated_event)
except RuntimeError: # Member was null
traceback.print_exc()
new_msg = messaging.new_message('onroadEvents', len(msg.onroadEventsDEPRECATED))
new_msg = messaging.new_message('onroadEvents', len(onroadEvents))
new_msg.valid = msg.valid
new_msg.logMonoTime = msg.logMonoTime
new_msg.onroadEvents = onroadEvents
@@ -451,11 +488,12 @@ def migrate_driverMonitoringState(msgs):
for index, msg in msgs:
msg = msg.as_builder()
events = []
for event in msg.driverMonitoringState.eventsDEPRECATED:
for event in msg.driverMonitoringState.deprecated.events:
try:
if not str(event.name).endswith('DEPRECATED'):
# dict converts name enum into string representation
events.append(log.OnroadEvent(**event.to_dict()))
migrated_event = migrate_onroad_event(event)
if migrated_event is not None:
events.append(migrated_event)
except RuntimeError: # Member was null
traceback.print_exc()

View File

@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import os
import pickle
import sys
from collections import defaultdict
from typing import Any
@@ -8,22 +9,22 @@ from itertools import zip_longest
import matplotlib.pyplot as plt
import numpy as np
from tabulate import tabulate
from openpilot.common.utils import tabulate
from openpilot.common.git import get_commit
from openpilot.system.hardware import PC
from openpilot.tools.lib.openpilotci import get_url
from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff
from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process
from openpilot.tools.lib.framereader import FrameReader, NumpyFrameReader
from openpilot.tools.lib.framereader import FrameReader
from openpilot.tools.lib.logreader import LogReader, save_log
from openpilot.tools.lib.github_utils import GithubUtils
TEST_ROUTE = "8494c69d3c710e81|000001d4--2648a9a404"
SEGMENT = 4
MAX_FRAMES = 100 if PC else 400
START_FRAME = 0
END_FRAME = 60
NO_MODEL = "NO_MODEL" in os.environ
SEND_EXTRA_INPUTS = bool(int(os.getenv("SEND_EXTRA_INPUTS", "0")))
DATA_TOKEN = os.getenv("CI_ARTIFACTS_TOKEN","")
@@ -33,8 +34,8 @@ GITHUB = GithubUtils(API_TOKEN, DATA_TOKEN)
EXEC_TIMINGS = [
# model, instant max, average max
("modelV2", 0.035, 0.025),
("driverStateV2", 0.02, 0.015),
("modelV2", 0.05, 0.028),
("driverStateV2", 0.05, 0.016),
]
def get_log_fn(test_route, ref="master"):
@@ -64,6 +65,7 @@ def generate_report(proposed, master, tmp, commit):
ModelV2_Plots = zl([
(lambda x: get_idx_if_non_empty(x.velocity.x, 0), "velocity.x"),
(lambda x: get_idx_if_non_empty(x.action.desiredCurvature), "desiredCurvature"),
(lambda x: get_idx_if_non_empty(x.action.desiredAcceleration), "desiredAcceleration"),
(lambda x: get_idx_if_non_empty(x.leadsV3[0].x, 0), "leadsV3.x"),
(lambda x: get_idx_if_non_empty(x.laneLines[1].y, 0), "laneLines.y"),
(lambda x: get_idx_if_non_empty(x.meta.desireState, 3), "desireState.laneChangeLeft"),
@@ -74,8 +76,8 @@ def generate_report(proposed, master, tmp, commit):
(lambda x: get_idx_if_non_empty(x.wheelOnRightProb), "wheelOnRightProb"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceProb), "leftDriverData.faceProb"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceOrientation, 0), "leftDriverData.faceOrientation0"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.leftBlinkProb), "leftDriverData.leftBlinkProb"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.notReadyProb, 0), "leftDriverData.notReadyProb0"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.eyesClosedProb), "leftDriverData.eyesClosedProb"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.phoneProb), "leftDriverData.phoneProb"),
(lambda x: get_idx_if_non_empty(x.rightDriverData.faceProb), "rightDriverData.faceProb"),
], "driverStateV2")
@@ -122,18 +124,17 @@ def comment_replay_report(proposed, master, full_logs):
diff_plots = create_table("Model Replay Differences", diff_files, link, open_table=True)
all_plots = create_table("All Model Replay Plots", files, link)
comment = f"ref for commit {commit}: {link}/{log_name}" + diff_plots + all_plots
GITHUB.comment_on_pr(comment, PR_BRANCH)
GITHUB.comment_on_pr(comment, PR_BRANCH, "commaci-public", True)
def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types):
def trim_logs(logs, start_frame, end_frame, frs_types, include_all_types):
all_msgs = []
cam_state_counts = defaultdict(int)
# keep adding messages until cam states are equal to MAX_FRAMES
for msg in sorted(logs, key=lambda m: m.logMonoTime):
all_msgs.append(msg)
if msg.which() in frs_types:
cam_state_counts[msg.which()] += 1
if all(cam_state_counts[state] == max_frames for state in frs_types):
if any(cam_state_counts[state] >= start_frame for state in frs_types):
all_msgs.append(msg)
if all(cam_state_counts[state] == end_frame for state in frs_types):
break
if len(include_all_types) != 0:
@@ -145,9 +146,9 @@ def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types):
def model_replay(lr, frs):
# modeld is using frame pairs
modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"},
{"roadEncodeIdx", "wideRoadEncodeIdx", "carParams", "carState", "carControl"})
dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"})
modeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"roadCameraState", "wideRoadCameraState"},
{"roadEncodeIdx", "wideRoadEncodeIdx", "carParams", "carState", "carControl", "can"})
dmodeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"driverCameraState"}, {"driverEncodeIdx", "carParams", "can"})
if not SEND_EXTRA_INPUTS:
modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration']
@@ -164,9 +165,6 @@ def model_replay(lr, frs):
dmonitoringmodeld = get_process_config("dmonitoringmodeld")
modeld_msgs = replay_process(modeld, modeld_logs, frs)
if isinstance(frs['roadCameraState'], NumpyFrameReader):
del frs['roadCameraState'].frames
del frs['wideRoadCameraState'].frames
dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs)
msgs = modeld_msgs + dmonitoringmodeld_msgs
@@ -192,34 +190,36 @@ def model_replay(lr, frs):
print("----------------- Model Timing -----------------")
print("------------------------------------------------")
print(tabulate(rows, header, tablefmt="simple_grid", stralign="center", numalign="center", floatfmt=".4f"))
assert timings_ok
assert timings_ok or PC
return msgs
def get_frames():
regen_cache = "--regen-cache" in sys.argv
cache = "--cache" in sys.argv or not PC or regen_cache
videos = ('fcamera.hevc', 'dcamera.hevc', 'ecamera.hevc')
cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
if cache:
frames_cache = '/tmp/model_replay_cache' if PC else '/data/model_replay_cache'
os.makedirs(frames_cache, exist_ok=True)
cache_size = 200
for v in videos:
if not all(os.path.isfile(f'{frames_cache}/{TEST_ROUTE}_{v}_{i}.npy') for i in range(MAX_FRAMES//cache_size)) or regen_cache:
f = FrameReader(get_url(TEST_ROUTE, SEGMENT, v)).get(0, MAX_FRAMES + 1, pix_fmt="nv12")
print(f'Caching {v}...')
for i in range(MAX_FRAMES//cache_size):
np.save(f'{frames_cache}/{TEST_ROUTE}_{v}_{i}', f[(i * cache_size) + 1:((i + 1) * cache_size) + 1])
del f
return {c : NumpyFrameReader(f"{frames_cache}/{TEST_ROUTE}_{v}", 1928, 1208, cache_size) for c,v in zip(cams, videos, strict=True)}
else:
return {c : FrameReader(get_url(TEST_ROUTE, SEGMENT, v), readahead=True) for c,v in zip(cams, videos, strict=True)}
cache_name = f'{frames_cache}/{TEST_ROUTE}_{SEGMENT}_{START_FRAME}_{END_FRAME}.pkl'
if os.path.isfile(cache_name) and not regen_cache:
try:
print(f"Loading frames from cache {cache_name}")
return pickle.load(open(cache_name, "rb"))
except Exception as e:
print(f"Failed to load frames from cache {cache_name}: {e}")
frs = {
'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "fcamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME),
'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "dcamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME),
'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "ecamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME),
}
for fr in frs.values():
for fidx in range(START_FRAME, END_FRAME):
fr.get(fidx)
fr.it = None
print(f"Dumping frame cache {cache_name}")
pickle.dump(frs, open(cache_name, "wb"))
return frs
if __name__ == "__main__":
update = "--update" in sys.argv or (os.getenv("GIT_BRANCH", "") == 'master')
@@ -231,7 +231,6 @@ if __name__ == "__main__":
log_msgs = []
# run replays
if not NO_MODEL:
log_msgs += model_replay(lr, frs)
# get diff
@@ -241,13 +240,10 @@ if __name__ == "__main__":
try:
all_logs = list(LogReader(GITHUB.get_file_url(MODEL_REPLAY_BUCKET, log_fn)))
cmp_log = []
# logs are ordered based on type: modelV2, drivingModelData, driverStateV2
if not NO_MODEL:
model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "drivingModelData", "cameraOdometry"))
cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*3]
cmp_log += all_logs[model_start_index+START_FRAME*3:model_start_index + END_FRAME*3]
dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2")
cmp_log += all_logs[dmon_start_index:dmon_start_index + MAX_FRAMES]
cmp_log += all_logs[dmon_start_index+START_FRAME:dmon_start_index + END_FRAME]
ignore = [
'logMonoTime',

View File

@@ -2,11 +2,12 @@
import os
import time
import copy
import json
import heapq
import signal
from collections import Counter, OrderedDict
import numpy as np
from collections import Counter
from dataclasses import dataclass, field
from itertools import islice
from typing import Any
from collections.abc import Callable, Iterable
from tqdm import tqdm
@@ -17,37 +18,25 @@ import cereal.messaging as messaging
from cereal import car
from cereal.services import SERVICE_LIST
from msgq.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name
from opendbc.car.can_definitions import CanData
from opendbc.car.car_helpers import get_car, interfaces
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.timeout import Timeout
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.card import can_comm_callbacks
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
from openpilot.system.manager.process_config import managed_processes
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams
from openpilot.selfdrive.test.process_replay.migration import migrate_all
from openpilot.selfdrive.test.process_replay.capture import ProcessOutputCapture
from openpilot.tools.lib.logreader import LogIterable
from openpilot.tools.lib.framereader import BaseFrameReader
from openpilot.tools.lib.framereader import FrameReader
# Numpy gives different results based on CPU features after version 19
NUMPY_TOLERANCE = 1e-7
NUMPY_TOLERANCE = 1e-2
PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__))
FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/")
class DummySocket:
def __init__(self):
self.data: list[bytes] = []
def receive(self, non_blocking: bool = False) -> bytes | None:
if non_blocking:
return None
return self.data.pop()
def send(self, data: bytes):
self.data.append(data)
class LauncherWithCapture:
def __init__(self, capture: ProcessOutputCapture, launcher: Callable):
@@ -65,8 +54,7 @@ class ReplayContext:
self.pubs = cfg.pubs
self.main_pub = cfg.main_pub
self.main_pub_drained = cfg.main_pub_drained
self.unlocked_pubs = cfg.unlocked_pubs
assert(len(self.pubs) != 0 or self.main_pub is not None)
assert len(self.pubs) != 0 or self.main_pub is not None
def __enter__(self):
self.open_context()
@@ -81,9 +69,8 @@ class ReplayContext:
messaging.set_fake_prefix(self.proc_name)
if self.main_pub is None:
self.events = OrderedDict()
pubs_with_events = [pub for pub in self.pubs if pub not in self.unlocked_pubs]
for pub in pubs_with_events:
self.events = {}
for pub in self.pubs:
self.events[pub] = messaging.fake_event_handle(pub, enable=True)
else:
self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)}
@@ -140,19 +127,25 @@ class ProcessConfig:
processing_time: float = 0.001
timeout: int = 30
simulation: bool = True
# Set to service process receives on first
main_pub: str | None = None
main_pub_drained: bool = True
main_pub_drained: bool = False
vision_pubs: list[str] = field(default_factory=list)
ignore_alive_pubs: list[str] = field(default_factory=list)
unlocked_pubs: list[str] = field(default_factory=list)
def __post_init__(self):
# If the process is polling a service, we can just lock that one to speed up replay
if self.main_pub is None and isinstance(self.should_recv_callback, MessageBasedRcvCallback):
self.main_pub = self.should_recv_callback.trigger_msg_type
class ProcessContainer:
def __init__(self, cfg: ProcessConfig):
self.prefix = OpenpilotPrefix(clean_dirs_on_exit=False)
self.prefix = OpenpilotPrefix(create_dirs_on_enter=False, clean_dirs_on_exit=False)
self.cfg = copy.deepcopy(cfg)
self.process = copy.deepcopy(managed_processes[cfg.proc_name])
self.msg_queue: list[capnp._DynamicStructReader] = []
self.last_input_log_mono_time: int = -1
self.cnt = 0
self.pm: messaging.PubMaster | None = None
self.sockets: list[messaging.SubSocket] | None = None
@@ -211,8 +204,10 @@ class ProcessContainer:
streams_metas = available_streams(all_msgs)
for meta in streams_metas:
if meta.camera_state in self.cfg.vision_pubs:
assert frs[meta.camera_state].pix_fmt == 'nv12'
frame_size = (frs[meta.camera_state].w, frs[meta.camera_state].h)
vipc_server.create_buffers(meta.stream, 2, *frame_size)
stride, y_height, _, yuv_size = get_nv12_info(frame_size[0], frame_size[1])
vipc_server.create_buffers_with_sizes(meta.stream, 2, frame_size[0], frame_size[1], yuv_size, stride, stride * y_height)
vipc_server.start_listener()
self.vipc_server = vipc_server
@@ -226,10 +221,11 @@ class ProcessContainer:
def start(
self, params_config: dict[str, Any], environ_config: dict[str, Any],
all_msgs: LogIterable, frs: dict[str, BaseFrameReader] | None,
all_msgs: LogIterable, frs: dict[str, FrameReader] | None,
fingerprint: str | None, capture_output: bool
):
with self.prefix as p:
self.prefix.create_dirs()
self._setup_env(params_config, environ_config)
if self.cfg.config_callback is not None:
@@ -255,11 +251,6 @@ class ProcessContainer:
if self.cfg.init_callback is not None:
self.cfg.init_callback(self.rc, self.pm, all_msgs, fingerprint)
# wait for process to startup
with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(self.cfg.proc_name)}"):
while not all(self.pm.all_readers_updated(s) for s in self.cfg.pubs if s not in self.cfg.ignore_alive_pubs):
time.sleep(0)
def stop(self):
with self.prefix:
self.process.signal(signal.SIGKILL)
@@ -268,50 +259,70 @@ class ProcessContainer:
self.prefix.clean_dirs()
self._clean_env()
def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, BaseFrameReader] | None) -> list[capnp._DynamicStructReader]:
def get_output_msgs(self, start_time: int):
assert self.rc and self.sockets
output_msgs = []
self.rc.wait_for_recv_called()
for socket in self.sockets:
ms = messaging.drain_sock(socket)
for m in ms:
m = m.as_builder()
assert start_time > 0, "start_time must be positive"
m.logMonoTime = start_time + int(self.cfg.processing_time * 1e9)
output_msgs.append(m.as_reader())
return output_msgs
def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, FrameReader] | None) -> list[capnp._DynamicStructReader]:
assert self.rc and self.pm and self.sockets and self.process.proc
output_msgs = []
with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"):
end_of_cycle = True
if self.cfg.should_recv_callback is not None:
end_of_cycle = self.cfg.should_recv_callback(msg, self.cfg, self.cnt)
self.msg_queue.append(msg)
if end_of_cycle:
self.rc.wait_for_recv_called()
with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"):
# call recv to let sub-sockets reconnect, after we know the process is ready
if self.cnt == 0:
for s in self.sockets:
messaging.recv_one_or_none(s)
# empty recv on drained pub indicates the end of messages, only do that if there're any
# certain processes use drain_sock. need to cause empty recv to break from this loop
trigger_empty_recv = False
if self.cfg.main_pub and self.cfg.main_pub_drained:
trigger_empty_recv = next((True for m in self.msg_queue if m.which() == self.cfg.main_pub), False)
trigger_empty_recv = any(m.which() == self.cfg.main_pub for m in self.msg_queue)
# get output msgs from previous inputs
output_msgs = self.get_output_msgs(self.last_input_log_mono_time)
for m in self.msg_queue:
self.pm.send(m.which(), m.as_builder())
self.last_input_log_mono_time = max(self.last_input_log_mono_time, m.logMonoTime)
# send frames if needed
if self.vipc_server is not None and m.which() in self.cfg.vision_pubs:
camera_state = getattr(m, m.which())
camera_meta = meta_from_camera_state(m.which())
assert frs is not None
img = frs[m.which()].get(camera_state.frameId, pix_fmt="nv12")[0]
self.vipc_server.send(camera_meta.stream, img.flatten().tobytes(),
img = frs[m.which()].get(camera_state.frameId)
h, w = frs[m.which()].h, frs[m.which()].w
stride, y_height, _, yuv_size = get_nv12_info(w, h)
uv_offset = stride * y_height
padded_img = np.zeros(((uv_offset //stride) + (h // 2), stride))
padded_img[:h, :w] = img[:h * w].reshape((-1, w))
padded_img[uv_offset // stride:uv_offset // stride + h // 2, :w] = img[h * w:].reshape((-1, w))
img_bytes = np.zeros((yuv_size,), dtype=np.uint8)
img_bytes[:padded_img.size] = padded_img.flatten()
self.vipc_server.send(camera_meta.stream, img_bytes.tobytes(),
camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof)
self.msg_queue = []
self.rc.unlock_sockets()
self.rc.wait_for_next_recv(trigger_empty_recv)
for socket in self.sockets:
ms = messaging.drain_sock(socket)
for m in ms:
m = m.as_builder()
m.logMonoTime = msg.logMonoTime + int(self.cfg.processing_time * 1e9)
output_msgs.append(m.as_reader())
if trigger_empty_recv:
self.rc.unlock_sockets()
self.cnt += 1
assert self.process.proc.is_alive()
@@ -321,7 +332,7 @@ class ProcessContainer:
def card_fingerprint_callback(rc, pm, msgs, fingerprint):
print("start fingerprinting")
params = Params()
canmsgs = [msg for msg in msgs if msg.which() == "can"][:300]
canmsgs = list(islice((m for m in msgs if m.which() == "can"), 300))
# card expects one arbitrary can and pandaState
rc.send_sync(pm, "can", messaging.new_message("can", 1))
@@ -342,40 +353,28 @@ def card_fingerprint_callback(rc, pm, msgs, fingerprint):
def get_car_params_callback(rc, pm, msgs, fingerprint):
params = Params()
if fingerprint:
CarInterface, _, _, _ = interfaces[fingerprint]
CarInterface = interfaces[fingerprint]
CP = CarInterface.get_non_essential_params(fingerprint)
else:
can = DummySocket()
sendcan = DummySocket()
canmsgs = [msg for msg in msgs if msg.which() == "can"]
can_msgs = ([CanData(can.address, can.dat, can.src) for can in m.can] for m in msgs if m.which() == "can")
cached_params_raw = params.get("CarParamsCache")
has_cached_cp = cached_params_raw is not None
assert len(canmsgs) != 0, "CAN messages are required for fingerprinting"
assert os.environ.get("SKIP_FW_QUERY", False) or has_cached_cp, \
assert next(can_msgs, None), "CAN messages are required for fingerprinting"
assert os.environ.get("SKIP_FW_QUERY", False) or cached_params_raw is not None, \
"CarParamsCache is required for fingerprinting. Make sure to keep carParams msgs in the logs."
for m in canmsgs[:300]:
can.send(m.as_builder().to_bytes())
can_callbacks = can_comm_callbacks(can, sendcan)
def can_recv(wait_for_one: bool = False) -> list[list[CanData]]:
return [next(can_msgs, [])]
cached_params = None
if has_cached_cp:
if cached_params_raw is not None:
with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
cached_params = _cached_params
CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params).CP
if not params.get_bool("DisengageOnAccelerator"):
CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
CP = get_car(can_recv, lambda _msgs: None, lambda obd: None, params.get_bool("AlphaLongitudinalEnabled"), False, cached_params=cached_params).CP
params.put("CarParams", CP.to_bytes())
def selfdrived_rcv_callback(msg, cfg, frame):
return (frame - 1) == 0 or msg.which() == 'carState'
def card_rcv_callback(msg, cfg, frame):
# no sendcan until card is initialized
if msg.which() != "can":
@@ -390,21 +389,6 @@ def card_rcv_callback(msg, cfg, frame):
return len(socks) > 0
def calibration_rcv_callback(msg, cfg, frame):
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
# should_recv always true to increment frame
return (frame - 1) == 0 or msg.which() == 'cameraOdometry'
def torqued_rcv_callback(msg, cfg, frame):
# should_recv always true to increment frame
return (frame - 1) == 0 or msg.which() == 'livePose'
def dmonitoringmodeld_rcv_callback(msg, cfg, frame):
return msg.which() == "driverCameraState"
class ModeldCameraSyncRcvCallback:
def __init__(self):
self.road_present = False
@@ -429,26 +413,13 @@ class ModeldCameraSyncRcvCallback:
class MessageBasedRcvCallback:
def __init__(self, trigger_msg_type):
def __init__(self, trigger_msg_type: str, first_frame: bool = False):
self.trigger_msg_type = trigger_msg_type
self.first_frame = first_frame
def __call__(self, msg, cfg, frame):
return msg.which() == self.trigger_msg_type
class FrequencyBasedRcvCallback:
def __init__(self, trigger_msg_type):
self.trigger_msg_type = trigger_msg_type
def __call__(self, msg, cfg, frame):
if msg.which() != self.trigger_msg_type:
return False
resp_sockets = [
s for s in cfg.subs
if frame % max(1, int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency)) == 0
]
return bool(len(resp_sockets))
# publish on first frame or trigger msg
return ((frame - 1) == 0 and self.first_frame) or msg.which() == self.trigger_msg_type
def selfdrived_config_callback(params, cfg, lr):
@@ -463,16 +434,16 @@ CONFIGS = [
proc_name="selfdrived",
pubs=[
"carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
"longitudinalPlan", "livePose", "liveParameters", "radarState",
"modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
"liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
"gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance", "alertDebug",
"longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2",
"driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters",
"accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState",
"carControl", "driverAssistance", "alertDebug", "audioFeedback",
],
subs=["selfdriveState", "onroadEvents"],
ignore=["logMonoTime"],
config_callback=selfdrived_config_callback,
init_callback=get_car_params_callback,
should_recv_callback=selfdrived_rcv_callback,
should_recv_callback=MessageBasedRcvCallback("carState", True),
tolerance=NUMPY_TOLERANCE,
processing_time=0.004,
),
@@ -497,6 +468,7 @@ CONFIGS = [
tolerance=NUMPY_TOLERANCE,
processing_time=0.004,
main_pub="can",
main_pub_drained=True,
),
ProcessConfig(
proc_name="radard",
@@ -504,7 +476,7 @@ CONFIGS = [
subs=["radarState"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=FrequencyBasedRcvCallback("modelV2"),
should_recv_callback=MessageBasedRcvCallback("modelV2"),
),
ProcessConfig(
proc_name="plannerd",
@@ -512,22 +484,23 @@ CONFIGS = [
subs=["longitudinalPlan", "driverAssistance"],
ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
init_callback=get_car_params_callback,
should_recv_callback=FrequencyBasedRcvCallback("modelV2"),
should_recv_callback=MessageBasedRcvCallback("modelV2"),
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(
proc_name="calibrationd",
pubs=["carState", "cameraOdometry", "carParams"],
pubs=["carState", "cameraOdometry"],
subs=["liveCalibration"],
ignore=["logMonoTime"],
should_recv_callback=calibration_rcv_callback,
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("cameraOdometry", True),
),
ProcessConfig(
proc_name="dmonitoringd",
pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"],
subs=["driverMonitoringState"],
ignore=["logMonoTime"],
should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"),
should_recv_callback=MessageBasedRcvCallback("driverStateV2"),
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(
@@ -539,7 +512,7 @@ CONFIGS = [
ignore=["logMonoTime"],
should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
tolerance=NUMPY_TOLERANCE,
unlocked_pubs=["accelerometer", "gyroscope"],
processing_time=0.01,
),
ProcessConfig(
proc_name="paramsd",
@@ -547,10 +520,19 @@ CONFIGS = [
subs=["liveParameters"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=FrequencyBasedRcvCallback("livePose"),
should_recv_callback=MessageBasedRcvCallback("livePose"),
tolerance=NUMPY_TOLERANCE,
processing_time=0.004,
),
ProcessConfig(
proc_name="lagd",
pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"],
subs=["liveDelay"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("livePose"),
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(
proc_name="ubloxd",
pubs=["ubloxRaw"],
@@ -559,23 +541,22 @@ CONFIGS = [
),
ProcessConfig(
proc_name="torqued",
pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"],
pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"],
subs=["liveTorqueParameters"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=torqued_rcv_callback,
should_recv_callback=MessageBasedRcvCallback("livePose", True),
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(
proc_name="modeld",
pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState", "carState", "carControl"],
pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"],
subs=["modelV2", "drivingModelData", "cameraOdometry"],
ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"],
should_recv_callback=ModeldCameraSyncRcvCallback(),
tolerance=NUMPY_TOLERANCE,
processing_time=0.020,
main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("roadCameraState").stream),
main_pub_drained=False,
vision_pubs=["roadCameraState", "wideRoadCameraState"],
ignore_alive_pubs=["wideRoadCameraState"],
init_callback=get_car_params_callback,
@@ -585,11 +566,10 @@ CONFIGS = [
pubs=["liveCalibration", "driverCameraState"],
subs=["driverStateV2"],
ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"],
should_recv_callback=dmonitoringmodeld_rcv_callback,
should_recv_callback=MessageBasedRcvCallback("driverCameraState"),
tolerance=NUMPY_TOLERANCE,
processing_time=0.020,
main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream),
main_pub_drained=False,
vision_pubs=["driverCameraState"],
ignore_alive_pubs=["driverCameraState"],
),
@@ -628,9 +608,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
if len(live_calibration) > 0:
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
if len(live_parameters) > 0:
lp_dict = live_parameters[msg_index].to_dict()
lp_dict["carFingerprint"] = CP.carFingerprint
custom_params["LiveParameters"] = json.dumps(lp_dict)
custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes()
if len(live_torque_parameters) > 0:
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
@@ -649,9 +627,9 @@ def replay_process_with_name(name: str | Iterable[str], lr: LogIterable, *args,
def replay_process(
cfg: ProcessConfig | Iterable[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] = None,
fingerprint: str = None, return_all_logs: bool = False, custom_params: dict[str, Any] = None,
captured_output_store: dict[str, dict[str, str]] = None, disable_progress: bool = False
cfg: ProcessConfig | Iterable[ProcessConfig], lr: LogIterable, frs: dict[str, FrameReader] | None = None,
fingerprint: str | None = None, return_all_logs: bool = False, custom_params: dict[str, Any] | None = None,
captured_output_store: dict[str, dict[str, str]] | None = None, disable_progress: bool = False
) -> list[capnp._DynamicStructReader]:
if isinstance(cfg, Iterable):
cfgs = list(cfg)
@@ -677,7 +655,7 @@ def replay_process(
def _replay_multi_process(
cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] | None, fingerprint: str | None,
cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, FrameReader] | None, fingerprint: str | None,
custom_params: dict[str, Any] | None, captured_output_store: dict[str, dict[str, str]] | None, disable_progress: bool
) -> list[capnp._DynamicStructReader]:
if fingerprint is not None:
@@ -699,8 +677,8 @@ def _replay_multi_process(
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
log_msgs = []
try:
containers = []
try:
for cfg in cfgs:
container = ProcessContainer(cfg)
containers.append(container)
@@ -735,6 +713,11 @@ def _replay_multi_process(
internal_pub_queue.append(m)
heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1))
log_msgs.extend(output_msgs)
# flush last set of messages from each process
for container in containers:
last_time = container.last_input_log_mono_time if container.last_input_log_mono_time > 0 else int(time.monotonic() * 1e9)
log_msgs.extend(container.get_output_msgs(last_time))
finally:
for container in containers:
container.stop()
@@ -762,15 +745,12 @@ def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=Non
params_dict["IsRhdDetected"] = is_rhd
if CP is not None:
if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS:
params_dict["DisengageOnAccelerator"] = False
if fingerprint is None:
if CP.fingerprintSource == "fw":
params_dict["CarParamsCache"] = CP.as_builder().to_bytes()
if CP.openpilotLongitudinalControl:
params_dict["ExperimentalLongitudinalEnabled"] = True
params_dict["AlphaLongitudinalEnabled"] = True
if CP.notCar:
params_dict["JoystickDebugMode"] = True

View File

@@ -3,43 +3,20 @@ import os
import argparse
import time
import capnp
import numpy as np
from typing import Any
from collections.abc import Iterable
from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \
check_openpilot_enabled, check_most_messages_valid, get_custom_params_from_lr
from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES
from openpilot.selfdrive.test.update_ci_routes import upload_route
from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType
from openpilot.tools.lib.framereader import FrameReader
from openpilot.tools.lib.logreader import LogReader, LogIterable, save_log
from openpilot.tools.lib.openpilotci import get_url
class DummyFrameReader(BaseFrameReader):
def __init__(self, w: int, h: int, frame_count: int, pix_val: int):
self.pix_val = pix_val
self.w, self.h = w, h
self.frame_count = frame_count
self.frame_type = FrameType.raw
def get(self, idx, count=1, pix_fmt="yuv420p"):
if pix_fmt == "rgb24":
shape = (self.h, self.w, 3)
elif pix_fmt == "nv12" or pix_fmt == "yuv420p":
shape = (int((self.h * self.w) * 3 / 2),)
else:
raise NotImplementedError
return [np.full(shape, self.pix_val, dtype=np.uint8) for _ in range(count)]
@staticmethod
def zero_dcamera():
return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0)
def regen_segment(
lr: LogIterable, frs: dict[str, Any] = None,
lr: LogIterable, frs: dict[str, Any] | None = None,
processes: Iterable[ProcessConfig] = CONFIGS, disable_tqdm: bool = False
) -> list[capnp._DynamicStructReader]:
all_msgs = sorted(lr, key=lambda m: m.logMonoTime)
@@ -64,7 +41,7 @@ def setup_data_readers(
frs['wideRoadCameraState'] = FrameReader(get_url(route, str(sidx), "ecamera.hevc"))
if needs_driver_cam:
if dummy_driver_cam:
frs['driverCameraState'] = DummyFrameReader.zero_dcamera()
frs['driverCameraState'] = FrameReader(get_url(route, str(sidx), "fcamera.hevc")) # Use fcam as dummy
else:
device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData")
assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera."

View File

@@ -2,7 +2,7 @@ import copy
import os
from hypothesis import given, HealthCheck, Phase, settings
import hypothesis.strategies as st
from parameterized import parameterized
from openpilot.common.parameterized import parameterized
from cereal import log
from opendbc.car.toyota.values import CAR as TOYOTA

View File

@@ -3,87 +3,87 @@ import argparse
import concurrent.futures
import os
import sys
import traceback
from collections import defaultdict
from tqdm import tqdm
from typing import Any
from opendbc.car.car_helpers import interface_names
from openpilot.common.git import get_commit
from openpilot.tools.lib.openpilotci import get_url, upload_file
from openpilot.tools.lib.openpilotci import get_url
from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff
from openpilot.selfdrive.test.process_replay.diff_report import diff_process, diff_report
from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \
check_most_messages_valid
from openpilot.tools.lib.filereader import FileReader
from openpilot.tools.lib.logreader import LogReader, save_log
from openpilot.tools.lib.url_file import URLFile
source_segments = [
("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.COMMA_BODY
("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.HYUNDAI_SONATA
("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.HYUNDAI_KIA_EV6 (+ QCOM GPS)
("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.TOYOTA_PRIUS
("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.TOYOTA_RAV4
("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.TOYOTA_COROLLA_TSS2
("TOYOTA3", "8011d605be1cbb77|000000cc--8e8d8ec716--6"), # TOYOTA.TOYOTA_COROLLA_TSS2
("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.HONDA_CIVIC (NIDEC)
("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.HONDA_ACCORD (BOSCH)
("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID
("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), # CHRYSLER.RAM_1500_5TH_GEN
("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.SUBARU_OUTBACK
("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.CHEVROLET_VOLT
("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.CHEVROLET_BOLT_EUV
("GM", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.CHEVROLET_BOLT_EUV
("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.NISSAN_XTRAIL
("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.VOLKSWAGEN_GOLF
# FIXME the sensor timings are bad in mazda segment, we're not fully testing it, but it should be replaced
("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.MAZDA_CX9_2021
("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.FORD_BRONCO_SPORT_MK1
("RIVIAN", "bc095dc92e101734|000000db--ee9fe46e57--1"), # RIVIAN.RIVIAN_R1_GEN1
("TESLA", "2c912ca5de3b1ee9|0000025d--6eb6bcbca4--4"), # TESLA.TESLA_MODEL_Y
# Enable when port is tested and dashcamOnly is no longer set
#("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.VOLKSWAGEN_PASSAT_NMS
]
segments = [
("BODY", "regenA67A128BCD8|2024-08-30--02-36-22--0"),
("HYUNDAI", "regenCCD47FEBC0C|2025-03-04--03-21-48--0"),
("HYUNDAI2", "regen306779F6870|2024-10-03--04-03-23--0"),
("TOYOTA", "regen4A5115B248D|2025-03-04--03-21-43--0"),
("TOYOTA2", "regen6E484EDAB96|2024-08-30--02-47-37--0"),
("TOYOTA3", "regen4CE950B0267|2024-08-30--02-51-30--0"),
("HONDA", "regenB8CABEC09CC|2025-03-04--03-32-55--0"),
("HONDA2", "regen4B38A7428CD|2024-08-30--02-56-31--0"),
("CHRYSLER", "regenF3DBBA9E8DF|2024-08-30--02-59-03--0"),
("RAM", "regenDB02684E00A|2024-08-30--03-02-54--0"),
("SUBARU", "regen5E3347D0A0F|2025-03-04--03-23-55--0"),
("GM", "regen720F2BA4CF6|2024-08-30--03-09-15--0"),
("GM2", "regen9ADBECBCD1C|2024-08-30--03-13-04--0"),
("NISSAN", "regen58464878D07|2024-08-30--03-15-31--0"),
("VOLKSWAGEN", "regenED976DEB757|2024-08-30--03-18-02--0"),
("HYUNDAI", "regenAA0FC4ED71E|2025-04-08--22-57-50--0"),
("HYUNDAI2", "regenAFB9780D823|2025-04-08--23-00-34--0"),
("TOYOTA", "regen218A4DCFAA1|2025-04-08--22-57-51--0"),
# TODO: get new RAV4 route without enableDsu
# ("TOYOTA2", "regen107352E20EB|2025-04-08--22-57-46--0"),
("TOYOTA3", "regen1455E3B4BDF|2025-04-09--03-26-06--0"),
("HONDA", "regenB328FF8BA0A|2025-04-08--22-57-45--0"),
("HONDA2", "regen6170C8C9A35|2025-04-08--22-57-46--0"),
("CHRYSLER", "regen5B28FC2A437|2025-04-08--23-04-24--0"),
("RAM", "regenBF81EA96E08|2025-04-08--23-06-54--0"),
("SUBARU", "regen7366F13F6A1|2025-04-08--23-07-07--0"),
("GM", "regen1271097D038|2025-04-09--03-26-00--0"),
("NISSAN", "regen15D60604EAB|2025-04-08--23-06-59--0"),
("VOLKSWAGEN", "regen0F2F06C9539|2025-04-08--23-06-56--0"),
("MAZDA", "regenACF84CCF482|2024-08-30--03-21-55--0"),
("FORD", "regenA75209BD115|2025-03-04--03-23-53--0"),
("RIVIAN", "bc095dc92e101734|000000db--ee9fe46e57--1"),
("FORD", "regen755D8CB1E1F|2025-04-08--23-13-43--0"),
("RIVIAN", "regen5FCAC896BBE|2025-04-08--23-13-35--0"),
("TESLA", "2c912ca5de3b1ee9|0000025d--6eb6bcbca4--4"),
]
# dashcamOnly makes don't need to be tested until a full port is done
excluded_interfaces = ["mock", "tesla", "rivian"]
excluded_interfaces = ["mock", "body", "psa"]
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
BASE_URL = "https://raw.githubusercontent.com/commaai/ci-artifacts/refs/heads/process-replay/"
REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit")
EXCLUDED_PROCS = {"modeld", "dmonitoringmodeld"}
def run_test_process(data):
segment, cfg, args, cur_log_fn, ref_log_path, lr_dat = data
res = None
if not args.upload_only:
ref_log_msgs = list(LogReader(ref_log_path))
lr = LogReader.from_bytes(lr_dat)
res, log_msgs = test_process(cfg, lr, segment, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs)
# save logs so we can upload when updating refs
res, log_msgs = test_process(cfg, lr, segment, ref_log_msgs, cur_log_fn, args.ignore_fields, args.ignore_msgs)
# save logs so we can update refs
save_log(cur_log_fn, log_msgs)
if args.update_refs or args.upload_only:
print(f'Uploading: {os.path.basename(cur_log_fn)}')
assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}"
upload_file(cur_log_fn, os.path.basename(cur_log_fn))
os.remove(cur_log_fn)
return (segment, cfg.proc_name, res)
try:
diff_data = diff_process(cfg, ref_log_msgs, log_msgs)
except Exception:
diff_data = traceback.format_exc()
return (segment, cfg.proc_name, res, diff_data)
def get_log_data(segment):
@@ -92,14 +92,12 @@ def get_log_data(segment):
return (segment, f.read())
def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None):
def test_process(cfg, lr, segment, ref_log_msgs, new_log_path, ignore_fields=None, ignore_msgs=None):
if ignore_fields is None:
ignore_fields = []
if ignore_msgs is None:
ignore_msgs = []
ref_log_msgs = list(LogReader(ref_log_path))
try:
log_msgs = replay_process(cfg, lr, disable_progress=True)
except Exception as e:
@@ -142,8 +140,6 @@ if __name__ == "__main__":
help="Msgs to ignore (e.g. carEvents)")
parser.add_argument("--update-refs", action="store_true",
help="Updates reference logs using current commit")
parser.add_argument("--upload-only", action="store_true",
help="Skips testing processes and uploads logs from previous test run")
parser.add_argument("-j", "--jobs", type=int, default=max(cpu_count - 2, 1),
help="Max amount of parallel jobs")
args = parser.parse_args()
@@ -153,18 +149,16 @@ if __name__ == "__main__":
tested_cars = {c.upper() for c in tested_cars}
full_test = (tested_procs == all_procs) and (tested_cars == all_cars) and all(len(x) == 0 for x in (args.ignore_fields, args.ignore_msgs))
upload = args.update_refs or args.upload_only
os.makedirs(os.path.dirname(FAKEDATA), exist_ok=True)
if upload:
if args.update_refs:
assert full_test, "Need to run full test when updating refs"
try:
with open(REF_COMMIT_FN) as f:
ref_commit = f.read().strip()
except FileNotFoundError:
print("Couldn't find reference commit")
sys.exit(1)
ref_commit = URLFile(BASE_URL + "ref_commit", cache=False).read().decode().strip()
cur_commit = get_commit()
if not cur_commit:
@@ -179,7 +173,6 @@ if __name__ == "__main__":
log_paths: defaultdict[str, dict[str, dict[str, str]]] = defaultdict(lambda: defaultdict(dict))
with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool:
if not args.upload_only:
download_segments = [seg for car, seg in segments if car in tested_cars]
log_data: dict[str, LogReader] = {}
p1 = pool.map(get_log_data, download_segments)
@@ -196,38 +189,42 @@ if __name__ == "__main__":
continue
# to speed things up, we only test all segments on card
if cfg.proc_name != 'card' and car_brand not in ('HYUNDAI', 'TOYOTA', 'HONDA', 'SUBARU', 'FORD'):
if cfg.proc_name not in ('card', 'controlsd', 'lagd') and car_brand not in ('HYUNDAI', 'TOYOTA'):
continue
cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst")
cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst".replace("|", "_"))
if args.update_refs: # reference logs will not exist if routes were just regenerated
ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.zst")
route, seg_num = segment.rsplit("--", 1)
ref_log_path = get_url(route, seg_num, "rlog.zst")
else:
ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.zst")
ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.zst".replace("|", "_"))
ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
dat = None if args.upload_only else log_data[segment]
pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat))
pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, log_data[segment]))
log_paths[segment][cfg.proc_name]['ref'] = ref_log_path
log_paths[segment][cfg.proc_name]['new'] = cur_log_fn
results: Any = defaultdict(dict)
diffs: list = []
p2 = pool.map(run_test_process, pool_args)
for (segment, proc, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)):
if not args.upload_only:
for (segment, proc, result, diff_data) in tqdm(p2, desc="Running Tests", total=len(pool_args)):
results[segment][proc] = result
diffs.append((segment, proc, diff_data))
diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit)
if not upload:
if not args.update_refs:
with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f:
f.write(diff_long)
print(diff_short)
try:
diff_report(diffs, segments)
except Exception:
print(f"failed to generate diff report:\n{traceback.format_exc()}")
if failed:
print("TEST FAILED")
print("\n\nTo push the new reference logs for this commit run:")
print("./test_processes.py --upload-only")
else:
print("TEST SUCCEEDED")

View File

@@ -1,6 +1,6 @@
from parameterized import parameterized
from openpilot.common.parameterized import parameterized
from openpilot.selfdrive.test.process_replay.regen import regen_segment, DummyFrameReader
from openpilot.selfdrive.test.process_replay.regen import regen_segment
from openpilot.selfdrive.test.process_replay.process_replay import check_openpilot_enabled
from openpilot.tools.lib.openpilotci import get_url
from openpilot.tools.lib.logreader import LogReader
@@ -18,7 +18,7 @@ def ci_setup_data_readers(route, sidx):
lr = LogReader(get_url(route, sidx, "rlog.bz2"))
frs = {
'roadCameraState': FrameReader(get_url(route, sidx, "fcamera.hevc")),
'driverCameraState': DummyFrameReader.zero_dcamera()
'driverCameraState': FrameReader(get_url(route, sidx, "fcamera.hevc")),
}
if next((True for m in lr if m.which() == "wideRoadCameraState"), False):
frs["wideRoadCameraState"] = FrameReader(get_url(route, sidx, "ecamera.hevc"))

View File

@@ -18,6 +18,9 @@ if [ -z "$TEST_DIR" ]; then
exit 1
fi
# prevent storage from filling up
rm -rf /data/media/0/realdata/*
rm -rf /data/safe_staging/ || true
if [ -d /data/safe_staging/ ]; then
sudo umount /data/safe_staging/merged/ || true
@@ -39,6 +42,7 @@ sudo systemctl restart NetworkManager
sudo systemctl disable ssh-param-watcher.path
sudo systemctl disable ssh-param-watcher.service
sudo mount -o ro,remount /
sudo systemctl stop power_monitor
while true; do
if ! sudo systemctl is-active -q ssh; then

View File

@@ -2,7 +2,11 @@
# Sets up a virtual display for running map renderer and simulator without an X11 display
DISP_ID=99
if uname -r | grep -q "WSL2"; then
DISP_ID=0 # WSLg uses display :0
else
DISP_ID=99 # Standard Xvfb display
fi
export DISPLAY=:$DISP_ID
sudo Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null &
@@ -16,4 +20,3 @@ done
touch ~/.Xauthority
export XDG_SESSION_TYPE="x11"
xset -q

View File

@@ -8,7 +8,7 @@ import time
import numpy as np
from collections import Counter, defaultdict
from pathlib import Path
from tabulate import tabulate
from openpilot.common.utils import tabulate
from cereal import log
import cereal.messaging as messaging
@@ -18,7 +18,6 @@ from openpilot.common.timeout import Timeout
from openpilot.common.params import Params
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
from openpilot.selfdrive.test.helpers import set_params_enabled, release_only
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.hw import Paths
from openpilot.tools.lib.logreader import LogReader
from openpilot.tools.lib.log_time_series import msgs_to_time_series
@@ -33,51 +32,43 @@ CPU usage budget
TEST_DURATION = 25
LOG_OFFSET = 8
MAX_TOTAL_CPU = 280. # total for all 8 cores
MAX_TOTAL_CPU = 350. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 16.0,
"selfdrive.selfdrived.selfdrived": 16.0,
"selfdrive.car.card": 26.0,
"./loggerd": 14.0,
"./encoderd": 17.0,
"./encoderd": 13.0,
"./camerad": 10.0,
"selfdrive.controls.plannerd": 9.0,
"./ui": 18.0,
"selfdrive.locationd.paramsd": 9.0,
"./sensord": 7.0,
"selfdrive.controls.plannerd": 8.0,
"selfdrive.ui.ui": 40.0,
"system.sensord.sensord": 13.0,
"selfdrive.controls.radard": 2.0,
"selfdrive.modeld.modeld": 22.0,
"selfdrive.modeld.dmonitoringmodeld": 21.0,
"selfdrive.modeld.dmonitoringmodeld": 18.0,
"system.hardware.hardwared": 4.0,
"selfdrive.locationd.calibrationd": 2.0,
"selfdrive.locationd.torqued": 5.0,
"selfdrive.locationd.locationd": 25.0,
"selfdrive.locationd.paramsd": 9.0,
"selfdrive.locationd.lagd": 11.0,
"selfdrive.ui.soundd": 3.0,
"selfdrive.ui.feedback.feedbackd": 1.0,
"selfdrive.monitoring.dmonitoringd": 4.0,
"./proclogd": 2.0,
"system.proclogd": 7.0,
"system.logmessaged": 1.0,
"system.tombstoned": 0,
"./logcatd": 1.0,
"system.journald": 1.0,
"system.micd": 5.0,
"system.timed": 0,
"selfdrive.pandad.pandad": 0,
"system.statsd": 1.0,
"system.loggerd.uploader": 15.0,
"system.loggerd.deleter": 1.0,
}
PROCS.update({
"tici": {
"./pandad": 5.0,
"./ubloxd": 1.0,
"system.ubloxd.pigeond": 6.0,
},
"tizi": {
"./pandad": 19.0,
"system.qcomgpsd.qcomgpsd": 1.0,
}
}.get(HARDWARE.get_device_type(), {}))
}
TIMINGS = {
# rtols: max/min, rsd
@@ -95,6 +86,7 @@ TIMINGS = {
"modelV2": [2.5, 0.35],
"driverStateV2": [2.5, 0.40],
"livePose": [2.5, 0.35],
"liveParameters": [2.5, 0.35],
"wideRoadCameraState": [1.5, 0.35],
}
@@ -129,6 +121,7 @@ class TestOnroad:
params.put_bool("RecordFront", True)
set_params_enabled()
os.environ['REPLAY'] = '1'
os.environ['MSGQ_PREALLOC'] = '1'
os.environ['TESTING_CLOSET'] = '1'
if os.path.exists(Paths.log_root()):
shutil.rmtree(Paths.log_root())
@@ -145,7 +138,7 @@ class TestOnroad:
while not sm.seen['carState']:
sm.update(1000)
route = params.get("CurrentRoute", encoding="utf-8")
route = params.get("CurrentRoute")
assert route is not None
segs = list(Path(Paths.log_root()).glob(f"{route}--*"))
@@ -187,7 +180,7 @@ class TestOnroad:
def test_manager_starting_time(self):
st = self.ts['managerState']['t'][0]
assert (st - self.manager_st) < 10, f"manager.py took {st - self.manager_st}s to publish the first 'managerState' msg"
assert (st - self.manager_st) < 15.0, f"manager.py took {st - self.manager_st}s to publish the first 'managerState' msg"
def test_cloudlog_size(self):
msgs = self.msgs['logMessage']
@@ -214,7 +207,9 @@ class TestOnroad:
result += "-------------- UI Draw Timing ------------------\n"
result += "------------------------------------------------\n"
ts = self.ts['uiDebug']['drawTimeMillis']
# other processes preempt ui while starting up
offset = int(20 * LOG_OFFSET)
ts = self.ts['uiDebug']['drawTimeMillis'][offset:]
result += f"min {min(ts):.2f}ms\n"
result += f"max {max(ts):.2f}ms\n"
result += f"std {np.std(ts):.2f}ms\n"
@@ -223,7 +218,7 @@ class TestOnroad:
print(result)
assert max(ts) < 250.
assert np.mean(ts) < 10.
assert np.mean(ts) < 20. # TODO: ~6-11ms, increase consistency
#self.assertLess(np.std(ts), 5.)
# some slow frames are expected since camerad/modeld can preempt ui
@@ -287,13 +282,17 @@ class TestOnroad:
print("\n------------------------------------------------")
print("--------------- Memory Usage -------------------")
print("------------------------------------------------")
from openpilot.selfdrive.debug.mem_usage import print_report
print_report(self.msgs['procLog'], self.msgs['deviceState'])
offset = int(SERVICE_LIST['deviceState'].frequency * LOG_OFFSET)
mems = [m.deviceState.memoryUsagePercent for m in self.msgs['deviceState'][offset:]]
print("Memory usage: ", mems)
print("MSGQ (/dev/shm/) usage: ", subprocess.check_output(["du", "-hs", "/dev/shm"]).split()[0].decode())
# check for big leaks. note that memory usage is
# expected to go up while the MSGQ buffers fill up
assert np.average(mems) <= 65, "Average memory usage above 65%"
assert np.average(mems) <= 80, "Average memory usage too high"
assert np.max(np.diff(mems)) <= 4, "Max memory increase too high"
assert np.average(np.diff(mems)) <= 1, "Average memory increase too high"
@@ -330,25 +329,28 @@ class TestOnroad:
assert np.all(eof_sof_diff > 0)
assert np.all(eof_sof_diff < 50*1e6)
first_fid = {c: min(self.ts[c]['frameId']) for c in cams}
first_fid = {min(self.ts[c]['frameId']) for c in cams}
assert len(first_fid) == 1, "Cameras don't start on same frame ID"
if cam.endswith('CameraState'):
# camerad guarantees that all cams start on frame ID 0
# (note loggerd also needs to start up fast enough to catch it)
assert set(first_fid.values()) == {0, }, "Cameras don't start on frame ID 0"
else:
# encoder guarantees all cams start on the same frame ID
assert len(set(first_fid.values())) == 1, "Cameras don't start on same frame ID"
assert next(iter(first_fid)) < 100, "Cameras start on frame ID too high"
# we don't do a full segment rotation, so these might not match exactly
last_fid = {c: max(self.ts[c]['frameId']) for c in cams}
assert max(last_fid.values()) - min(last_fid.values()) < 10
last_fid = {max(self.ts[c]['frameId']) for c in cams}
assert max(last_fid) - min(last_fid) < 10
start, end = min(first_fid.values()), min(last_fid.values())
start, end = min(first_fid), min(last_fid)
for i in range(end-start):
ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams}
# road and wide cameras (first two) should be synced within 2ms
ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams[:2]}
diff = (max(ts.values()) - min(ts.values()))
assert diff < 2, f"Cameras not synced properly: frame_id={start+i}, {diff=:.1f}ms, {ts=}"
# driver camera should be staggered ~25ms from road camera
offset_ms = abs(self.ts[cams[2]]['timestampSof'][i] - self.ts[cams[0]]['timestampSof'][i]) / 1e6
assert 20 < offset_ms < 30, f"driver camera stagger out of range at frame {start+i}: {offset_ms:.1f}ms"
def test_camera_encoder_matches(self, subtests):
# sanity check that the frame metadata is consistent with the encoded frames
pairs = [('roadCameraState', 'roadEncodeIdx'),
@@ -391,8 +393,12 @@ class TestOnroad:
result += "----------------- Model Timing -----------------\n"
result += "------------------------------------------------\n"
cfgs = [
("modelV2", 0.045, 0.035),
("driverStateV2", 0.045, 0.035),
# since multiple processes use the GPU and can preempt each other,
# these numbers are not fully self-contained.
("modelV2", 0.06, 0.040),
# can miss cycles here and there, just important the avg frequency is 20Hz
("driverStateV2", 0.3, 0.05),
]
for (s, instant_max, avg_max) in cfgs:
ts = [getattr(m, s).modelExecutionTime for m in self.msgs[s]]

View File

@@ -105,7 +105,7 @@ class TestUpdated:
ret = None
start_time = time.monotonic()
while ret is None:
ret = self.params.get(key, encoding='utf8')
ret = self.params.get(key)
if time.monotonic() - start_time > timeout:
break
time.sleep(0.01)
@@ -162,8 +162,7 @@ class TestUpdated:
def _check_update_state(self, update_available):
# make sure LastUpdateTime is recent
t = self._read_param("LastUpdateTime")
last_update_time = datetime.datetime.fromisoformat(t)
last_update_time = self._read_param("LastUpdateTime")
td = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - last_update_time
assert td.total_seconds() < 10
self.params.remove("LastUpdateTime")
@@ -174,7 +173,7 @@ class TestUpdated:
# check params
update = self._read_param("UpdateAvailable")
assert update == "1" == update_available, f"UpdateAvailable: {repr(update)}"
assert self._read_param("UpdateFailedCount") == "0"
assert self._read_param("UpdateFailedCount") == 0
# TODO: check that the finalized update actually matches remote
# check the .overlay_init and .overlay_consistent flags

View File

@@ -19,7 +19,7 @@ SOURCES: list[AzureContainer] = [
DEST = OpenpilotCIContainer
def upload_route(path: str, exclude_patterns: Iterable[str] = None) -> None:
def upload_route(path: str, exclude_patterns: Iterable[str] | None = None) -> None:
if exclude_patterns is None:
exclude_patterns = [r'dcamera\.hevc']

View File

@@ -1,3 +1,5 @@
import time
from collections import deque
import pyray as rl
from dataclasses import dataclass
from openpilot.common.constants import CV
@@ -137,6 +139,12 @@ class HudRenderer(Widget):
self._traffic_red_icon = gui_app.texture('images/traffic_red.png')
self._traffic_green_icon = gui_app.texture('images/traffic_green.png')
self._ic_turn_l = gui_app.texture('images/turn_l.png')
self._ic_turn_r = gui_app.texture('images/turn_r.png')
self._ic_lane_change_l = gui_app.texture('images/lane_change_l.png')
self._ic_lane_change_r = gui_app.texture('images/lane_change_r.png')
self._ic_turn_u = gui_app.texture('images/turn_u.png')
self._set_speed_override = SetSpeedOverride()
self._debug_speed_panel = False
self._engaged = False
@@ -149,6 +157,7 @@ class HudRenderer(Widget):
self._memory_usage = 0
self._free_space = 0.0
self._voltage = 0.0
self._plot_renderer = None
def _update_state(self) -> None:
"""Update HUD state based on car state and controls state."""
@@ -201,6 +210,12 @@ class HudRenderer(Widget):
button_y = rect.y + UI_CONFIG.border_size
self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size))
if self._plot_renderer is None:
self._plot_renderer = PlotRenderer()
self._plot_renderer.draw(rect, self._font_display)
self._draw_date_time(rect)
self._draw_tpms_top_right(rect)
def user_interacting(self) -> bool:
return self._exp_button.is_pressed
@@ -344,14 +359,6 @@ class HudRenderer(Widget):
return gap
def _get_driving_mode_text_and_color(self) -> tuple[str, rl.Color]:
carState = ui_state.sm["carState"]
if carState.brakeHoldActive:
return tr("brake hold"), rl.Color(255, 0, 0, 230)
elif carState.softHoldActive:
return tr("soft hold"), rl.Color(255, 165, 0, 230)
elif carState.carrotCruise:
return tr("carrot"), rl.Color(0, 255, 0, 230)
try:
mode_val = int(ui_state.sm["longitudinalPlan"].myDrivingMode)
except Exception:
@@ -376,6 +383,7 @@ class HudRenderer(Widget):
self._memory_usage = 0
self._free_space = 0.0
self._voltage = 0.0
#self._plot_renderer = None
try:
device_state = sm["deviceState"]
@@ -589,8 +597,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
mode_text, dx, dy - 2, 32, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -598,8 +606,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
"GPS", dx, dy - 45, 30, rl.GREEN,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -608,8 +616,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
str(gap), bx + 220, by + 77, 40, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -647,8 +655,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
gear, gx, gy + 5, 70, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -669,8 +677,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
"APN", dx, dy, 40, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
elif active_carrot >= 1:
@@ -685,8 +693,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
"APM", dx, dy, 40, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -694,8 +702,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
"ROUTE", dx, dy - 45, 30, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -726,8 +734,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
label, dx, dy - 45, 30, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -743,8 +751,8 @@ class HudRenderer(Widget):
draw_text_ui_style(
str(disp_speed), dx, dy, 40, rl.WHITE,
font=self._font_display,
border_width=0.0,
shadow_offset=0.0,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
@@ -793,26 +801,357 @@ class HudRenderer(Widget):
# CPU
cpu_fill = rl.Color(255, 0, 0, 255) if (self._cpu_temp > 80 and self._blink_timer <= 8) else ok_color
self._draw_round_box(dx - 65, dy - 38, 130, 90, cpu_fill, line_color=rl.WHITE, roundness=0.16, segments=8, line_thickness=2)
draw_text_ui_style("CPU", dx, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style(f"{self._cpu_temp:.0f}°C", dx, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style("CPU", dx, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
draw_text_ui_style(f"{self._cpu_temp:.0f}°C", dx, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
# MEM
dx2 = dx + 150
mem_fill = rl.Color(255, 0, 0, 255) if (self._memory_usage > 85 and self._blink_timer <= 8) else ok_color
self._draw_round_box(dx2 - 65, dy - 38, 130, 90, mem_fill, line_color=rl.WHITE, roundness=0.16, segments=8, line_thickness=2)
draw_text_ui_style("MEM", dx2, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style(f"{self._memory_usage}%", dx2, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style("MEM", dx2, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
draw_text_ui_style(f"{self._memory_usage}%", dx2, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
# DISK / VOLT
dx3 = dx2 + 150
self._draw_round_box(dx3 - 65, dy - 38, 130, 90, ok_color, line_color=rl.WHITE, roundness=0.16, segments=8, line_thickness=2)
if self._disp_timer < 32:
draw_text_ui_style("DISK", dx3, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style(f"{100 - self._free_space:.0f}%", dx3, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style("DISK", dx3, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
draw_text_ui_style(f"{100 - self._free_space:.0f}%", dx3, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
else:
draw_text_ui_style("VOLT", dx3, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style(f"{self._voltage:.1f}V", dx3, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=0.0, shadow_offset=0.0, align="center_bottom")
draw_text_ui_style("VOLT", dx3, dy - 5, 25, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
draw_text_ui_style(f"{self._voltage:.1f}V", dx3, dy + 40, 40, rl.WHITE, font=self._font_display, border_width=1.0, shadow_offset=4.0, align="center_bottom")
def _draw_date_time(self, rect: rl.Rectangle) -> None:
show_datetime = ui_state.params.get_int("ShowDateTime")
if show_datetime <= 0:
return
now = time.localtime()
weekdays_ko = ["", "", "", "", "", "", ""]
x = int(rect.x + 170)
y = int(rect.y + 120)
if show_datetime in (1, 2):
draw_text_ui_style(
time.strftime("%H:%M", now), x, y, 100, rl.WHITE,
font=self._font_display,
border_width=3.0,
shadow_offset=8.0,
align="center_bottom",
)
if show_datetime in (1, 3):
weekday = weekdays_ko[(now.tm_wday + 1) % 7]
date_text = f"{time.strftime('%m-%d', now)}({weekday})"
draw_text_ui_style(
date_text, x, y + 70, 60, rl.WHITE,
font=self._font_display,
border_width=3.0,
shadow_offset=8.0,
align="center_bottom",
)
def _get_tpms_color(self, tpms: float) -> rl.Color:
if tpms < 5 or tpms > 60:
return rl.Color(255, 255, 255, 220)
if tpms < 31:
return rl.Color(255, 90, 90, 220)
return rl.Color(255, 255, 255, 220)
def _get_tpms_text(self, tpms: float) -> str:
if tpms < 5 or tpms > 60:
return ' -'
return f'{round(tpms):.0f}'
def _draw_tpms_top_right(self, rect: rl.Rectangle) -> None:
show_tpms = 1 #ui_state.params.get_int('ShowTpms')
if show_tpms not in (1, 3):
return
try:
tpms = ui_state.sm['carState'].tpms
fl = float(tpms.fl)
fr = float(tpms.fr)
rl_v = float(tpms.rl)
rr = float(tpms.rr)
except Exception:
return
bx = rect.x + rect.width - 125
by = rect.y + 130
dw = 80
draw_text_ui_style(
self._get_tpms_text(fl), bx - dw, by - 55, 40, self._get_tpms_color(fl),
font=self._font_display, border_width=1.0, shadow_offset=4.0, align='center_bottom',
)
draw_text_ui_style(
self._get_tpms_text(fr), bx + dw, by - 55, 40, self._get_tpms_color(fr),
font=self._font_display, border_width=1.0, shadow_offset=4.0, align='center_bottom',
)
draw_text_ui_style(
self._get_tpms_text(rl_v), bx - dw, by + 70, 40, self._get_tpms_color(rl_v),
font=self._font_display, border_width=1.0, shadow_offset=4.0, align='center_bottom',
)
draw_text_ui_style(
self._get_tpms_text(rr), bx + dw, by + 70, 40, self._get_tpms_color(rr),
font=self._font_display, border_width=1.0, shadow_offset=4.0, align='center_bottom',
)
def _get_turn_info_hud_data(self) -> dict:
try:
cm = ui_state.sm["carrotMan"]
except Exception:
return {
"active_carrot": 0,
"x_turn_info": 0,
"x_dist_to_turn": 0,
"n_go_pos_dist": 0,
"n_go_pos_time": 0,
"atc_type": "",
"sdi_descr": "",
"road_name": "",
"tbt_main_text": "",
}
try:
active_carrot = int(cm.activeCarrot)
except Exception:
active_carrot = 0
try:
x_turn_info = int(cm.xTurnInfo)
except Exception:
x_turn_info = 0
try:
x_dist_to_turn = int(cm.xDistToTurn)
except Exception:
x_dist_to_turn = 0
try:
n_go_pos_dist = int(cm.nGoPosDist)
except Exception:
n_go_pos_dist = 0
try:
n_go_pos_time = int(cm.nGoPosTime)
except Exception:
n_go_pos_time = 0
try:
atc_type = str(cm.atcType or "")
except Exception:
atc_type = ""
try:
sdi_descr = str(cm.szSdiDescr or "")
except Exception:
sdi_descr = ""
try:
road_name = str(cm.szPosRoadName or "")
except Exception:
road_name = ""
try:
tbt_main_text = str(cm.szTBTMainText or "")
except Exception:
tbt_main_text = ""
return {
"active_carrot": active_carrot,
"x_turn_info": x_turn_info,
"x_dist_to_turn": x_dist_to_turn,
"n_go_pos_dist": n_go_pos_dist,
"n_go_pos_time": n_go_pos_time,
"atc_type": atc_type,
"sdi_descr": sdi_descr,
"road_name": road_name,
"tbt_main_text": tbt_main_text,
}
def _format_turn_distance_text(self, dist_m: int) -> str:
if dist_m <= 0:
return ""
if ui_state.is_metric:
if dist_m < 1000:
return f"{dist_m} m"
return f"{dist_m / 1000.0:.1f} km"
else:
if dist_m < 1609:
return f"{int(dist_m * 3.28084)} ft"
return f"{dist_m / 1609.344:.1f} mi"
def _format_eta_text(self, remain_sec: int) -> str:
if remain_sec <= 0:
return ""
eta_tm = time.localtime(time.time() + remain_sec)
remain_min = remain_sec / 60.0
return f"도착: {remain_min:.1f}분({eta_tm.tm_hour:02d}:{eta_tm.tm_min:02d})"
def _format_go_pos_distance_text(self, dist_m: int) -> str:
if dist_m <= 0:
return ""
if ui_state.is_metric:
return f"{dist_m / 1000.0:.1f}km"
else:
return f"{dist_m / 1000.0 * KM_TO_MILE:.1f}mile"
def _draw_text_left_bottom(self, text: str, x: float, y: float, size: int, color, font=None, border_width: float = 2.0, shadow_offset: float = 4.0):
if not text:
return
draw_text_ui_style(
text, x, y, size, color,
font=font or self._font_display,
border_width=border_width,
shadow_offset=shadow_offset,
align="left_bottom",
)
def _draw_turn_icon(self, turn_info: int, bx: int, by: int, icon_size: int = 140):
if turn_info == 1:
self._draw_texture_rect(self._ic_turn_l, bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size)
elif turn_info == 2:
self._draw_texture_rect(self._ic_turn_r, bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size)
elif turn_info == 3:
self._draw_texture_rect(self._ic_lane_change_l, bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size)
elif turn_info == 4:
self._draw_texture_rect(self._ic_lane_change_r, bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size)
elif turn_info == 7:
self._draw_texture_rect(self._ic_turn_u, bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size)
elif turn_info == 6:
draw_text_ui_style(
"TG", bx, by + 20, 35, rl.WHITE,
font=self._font_display,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
elif turn_info == 8:
draw_text_ui_style(
"목적지", bx, by + 20, 35, rl.WHITE,
font=self._font_display,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
else:
draw_text_ui_style(
f"감속:{turn_info}", bx, by + 20, 35, rl.WHITE,
font=self._font_display,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
def _draw_turn_info_hud(self, rect: rl.Rectangle):
if rect.width < 1200:
return
info = self._get_turn_info_hud_data()
n_go_pos_dist = info["n_go_pos_dist"]
n_go_pos_time = info["n_go_pos_time"]
if not (n_go_pos_dist > 0 and n_go_pos_time > 0):
return
tbt_x = int(rect.x + rect.width - 800)
tbt_y = int(rect.y + rect.height - 250)
self._draw_round_box(
tbt_x,
tbt_y - 60,
790,
300,
rl.Color(0, 0, 0, 120),
line_color=rl.WHITE,
roundness=30.0 / 300.0,
segments=12,
line_thickness=2,
)
if info["tbt_main_text"]:
self._draw_text_left_bottom(
info["tbt_main_text"], tbt_x + 20, tbt_y - 15, 40, rl.WHITE,
font=self._font_bold, border_width=2.0, shadow_offset=4.0,
)
x_turn_info = info["x_turn_info"]
x_dist_to_turn = info["x_dist_to_turn"]
if x_turn_info > 0:
bx = tbt_x + 100
by = tbt_y + 85
if info["atc_type"]:
fill_color = rl.Color(0, 255, 0, 100) if "prepare" in info["atc_type"] else rl.GREEN
self._draw_round_box(
bx - 80, by - 90, 160, 230,
fill_color,
line_color=rl.BLACK,
roundness=15.0 / 230.0,
segments=8,
line_thickness=1,
)
self._draw_turn_icon(x_turn_info, bx, by, 140)
dist_text = self._format_turn_distance_text(x_dist_to_turn)
if dist_text:
draw_text_ui_style(
dist_text,
bx, by + 120, 40, rl.WHITE,
font=self._font_bold,
border_width=2.0,
shadow_offset=4.0,
align="center_bottom",
)
if info["sdi_descr"]:
label_x = tbt_x + 200
label_y = tbt_y + 200
size = measure_text_cached(self._font_bold, info["sdi_descr"], 40)
box_h = max(48, int(size.y + 13))
self._draw_round_box(
label_x - 10,
label_y - int(size.y) - 2,
int(size.x) + 20,
box_h,
rl.GREEN,
roundness=10.0 / box_h,
segments=8,
line_thickness=0,
)
self._draw_text_left_bottom(
info["sdi_descr"], label_x, label_y, 40, rl.WHITE,
font=self._font_bold, border_width=1.5, shadow_offset=3.0,
)
elif info["road_name"]:
self._draw_text_left_bottom(
info["road_name"], tbt_x + 200, tbt_y + 200, 40, rl.WHITE,
font=self._font_bold, border_width=1.5, shadow_offset=3.0,
)
eta_text = self._format_eta_text(n_go_pos_time)
if eta_text:
self._draw_text_left_bottom(
eta_text, tbt_x + 190, tbt_y + 80, 50, rl.WHITE,
font=self._font_bold, border_width=2.0, shadow_offset=4.0,
)
go_dist_text = self._format_go_pos_distance_text(n_go_pos_dist)
if go_dist_text:
self._draw_text_left_bottom(
go_dist_text, tbt_x + 310, tbt_y + 130, 50, rl.WHITE,
font=self._font_bold, border_width=2.0, shadow_offset=4.0,
)
def _draw_set_speed_carrot(self, rect: rl.Rectangle) -> None:
self._blink_timer = (self._blink_timer + 1) % 16
@@ -828,4 +1167,242 @@ class HudRenderer(Widget):
self._draw_carrot_lower_status(bx, by)
self._draw_carrot_speed_limit_box(bx, by)
self._draw_carrot_device_state(bx, by)
self._draw_turn_info_hud(rect)
class PlotRenderer:
PLOT_MAX = 400
def __init__(self):
self._plot_size = 0
self._plot_index = 0
self._plot_queue = [[0.0] * self.PLOT_MAX for _ in range(3)]
self._plot_min = 0.0
self._plot_max = 0.0
self._plot_x = 350.0
self._plot_width = 1000.0
self._plot_y = 40.0
self._plot_height = 300.0
self._plot_dx = 2.0
self._show_plot_mode_prev = -1
def _clear(self):
self._plot_size = 0
self._plot_index = 0
self._plot_min = 0.0
self._plot_max = 0.0
self._plot_queue = [[0.0] * self.PLOT_MAX for _ in range(3)]
def _make_plot_data(self, sm, show_plot_mode: int):
car_state = sm['carState']
lp = sm['longitudinalPlan']
car_control = sm['carControl']
controls_state = sm['controlsState']
a_ego = float(car_state.aEgo)
v_ego = float(car_state.vEgo)
accel = 0.0
try:
accel = float(lp.accels[0])
except Exception:
pass
speeds_0 = 0.0
try:
speeds_0 = float(lp.speeds[0])
except Exception:
pass
accel_out = 0.0
try:
accel_out = float(car_control.actuators.accel)
except Exception:
pass
if show_plot_mode in (0, 1):
return [a_ego, accel, accel_out], '1.Accel (Y:a_ego, G:a_target, O:a_out)'
if show_plot_mode == 2:
return [speeds_0, v_ego, a_ego], '2.Speed/Accel(Y:speed_0, G:v_ego, O:a_ego)'
if show_plot_mode == 3:
pos_32 = 0.0
vel_32 = 0.0
vel_0 = 0.0
try:
pos_32 = float(sm['modelV2'].position.x[32])
except Exception:
pass
try:
vel_32 = float(sm['modelV2'].velocity.x[32])
except Exception:
pass
try:
vel_0 = float(sm['modelV2'].velocity.x[0])
except Exception:
pass
return [pos_32, vel_32, vel_0], '3.Model(Y:pos_32, G:vel_32, O:vel_0)'
if show_plot_mode == 4:
a_lead_k = 0.0
v_rel = 0.0
try:
a_lead_k = float(sm['radarState'].leadOne.aLeadK)
except Exception:
pass
try:
v_rel = float(sm['radarState'].leadOne.vRel)
except Exception:
pass
return [accel, a_lead_k, v_rel], '4.Lead(Y:accel, G:a_lead, O:v_rel)'
if show_plot_mode == 5:
a_lead = 0.0
j_lead = 0.0
try:
a_lead = float(sm['radarState'].leadOne.aLead)
except Exception:
pass
try:
j_lead = float(sm['radarState'].leadOne.jLead)
except Exception:
pass
return [a_ego, a_lead, j_lead], '5.Lead(Y:a_ego, G:a_lead, O:j_lead)'
if show_plot_mode == 6:
actual_lat_accel = 0.0
desired_lat_accel = 0.0
output = 0.0
try:
actual_lat_accel = float(controls_state.lateralControlState.torqueState.actualLateralAccel) * 10.0
except Exception:
pass
try:
desired_lat_accel = float(controls_state.lateralControlState.torqueState.desiredLateralAccel) * 10.0
except Exception:
pass
try:
output = float(controls_state.lateralControlState.torqueState.output) * 10.0
except Exception:
pass
return [actual_lat_accel, desired_lat_accel, output], '6.Steer(Y:actual, G:desire, O:output)'
if show_plot_mode == 7:
actual_angle = float(car_state.steeringAngleDeg)
target_angle = 0.0
angle_offset = 0.0
try:
target_angle = float(car_control.actuators.steeringAngleDeg)
except Exception:
pass
try:
angle_offset = float(sm['liveParameters'].angleOffsetDeg) * 10.0
except Exception:
pass
return [actual_angle, target_angle, angle_offset], '7.SteerA (Y:Actual, G:Target, O:Offset*10)'
if show_plot_mode == 8:
curvature = 0.0
try:
curvature = float(car_control.actuators.curvature) * 10000.0
except Exception:
pass
return [curvature, curvature, curvature], '8.Curvature (*10000)'
return [0.0, 0.0, 0.0], 'no data'
def _update_plot_queue(self, plot_data):
self._plot_index = (self._plot_index + 1) % self.PLOT_MAX
for i in range(3):
self._plot_queue[i][self._plot_index] = float(plot_data[i])
if self._plot_size < self.PLOT_MAX:
self._plot_size += 1
self._plot_min = float('inf')
self._plot_max = float('-inf')
for i in range(3):
values = self._plot_queue[i][:self._plot_size] if self._plot_size < self.PLOT_MAX else self._plot_queue[i]
self._plot_min = min(self._plot_min, min(values))
self._plot_max = max(self._plot_max, max(values))
if self._plot_min == float('inf'):
self._plot_min = -2.0
if self._plot_max == float('-inf'):
self._plot_max = 2.0
if self._plot_min > -2.0:
self._plot_min = -2.0
if self._plot_max < 2.0:
self._plot_max = 2.0
def _draw_plotting(self, index: int, x_base: float, y_base: float, color, font):
if self._plot_size <= 0:
return
plot_range = self._plot_max - self._plot_min
plot_ratio = self._plot_height if plot_range < 1.0 else (self._plot_height / plot_range)
prev = None
latest_x = None
latest_y = None
latest_value = 0.0
for i in range(self._plot_size):
data = self._plot_queue[index][(self._plot_index - i + self.PLOT_MAX) % self.PLOT_MAX]
plot_y = y_base + self._plot_height - (data - self._plot_min) * plot_ratio
plot_x = x_base + (self._plot_size - i) * self._plot_dx
pt = rl.Vector2(plot_x, plot_y)
if prev is not None:
rl.draw_line_ex(prev, pt, 3.0, color)
else:
latest_x = plot_x
latest_y = plot_y
latest_value = data
prev = pt
if latest_x is not None and latest_y is not None:
draw_text_ui_style(
f'{latest_value:.2f}', latest_x + 50, latest_y + (40 if index > 0 else 0), 40, color,
font=font, border_width=2.0, shadow_offset=4.0, align='center_bottom',
)
def draw(self, rect: rl.Rectangle, font) -> None:
show_plot_mode = ui_state.params.get_int('ShowPlotMode')
if show_plot_mode == 0:
return
try:
if not ui_state.sm.alive['carState'] or not ui_state.sm.alive['longitudinalPlan']:
return
except Exception:
return
if show_plot_mode != self._show_plot_mode_prev:
self._clear()
self._show_plot_mode_prev = show_plot_mode
try:
plot_data, title = self._make_plot_data(ui_state.sm, show_plot_mode)
except Exception:
return
self._update_plot_queue(plot_data)
if rect.width < 1200:
return
x_base = rect.x + self._plot_x
y_base = rect.y + self._plot_y
colors = [rl.YELLOW, rl.GREEN, rl.Color(255, 165, 0, 255)]
for i in range(3):
self._draw_plotting(i, x_base, y_base, colors[i], font)
draw_text_ui_style(
title, x_base + 400, y_base - 20, 25, rl.WHITE,
font=font, border_width=2.0, shadow_offset=4.0, align='center_bottom',
)

View File

@@ -2,7 +2,7 @@ import math
import colorsys
import numpy as np
import pyray as rl
from cereal import messaging, car
from cereal import messaging, car, log
from dataclasses import dataclass, field
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
@@ -17,6 +17,8 @@ CLIP_MARGIN = 500
MIN_DRAW_DISTANCE = 10.0
MAX_DRAW_DISTANCE = 100.0
LaneChangeState = log.LaneChangeState
THROTTLE_COLORS = [
rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4)
rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35)
@@ -149,6 +151,7 @@ class ModelRenderer(Widget):
# self._draw_lead_indicator()
self._draw_path_carrot(sm)
self._draw_lane_lines_carrot(sm)
self._draw_blind_spot_carrot(sm)
self._draw_radar_info_carrot(sm)
def _update_raw_points(self, model):
@@ -520,6 +523,12 @@ class ModelRenderer(Widget):
]
self._carrot_lane_barrier_vertices = [
np.empty((0, 2), dtype=np.float32),
np.empty((0, 2), dtype=np.float32),
]
def _refresh_carrot_params(self):
self._carrot_show_lane_info = ui_state.params.get_int("ShowLaneInfo")
self._carrot_show_radar_info = ui_state.params.get_int("ShowRadarInfo")
@@ -642,10 +651,31 @@ class ModelRenderer(Widget):
return tris
def _draw_quad_fill_carrot(self, p0, p1, p2, p3, color: rl.Color):
pts = np.array([p0, p1, p2, p3], dtype=np.float32)
draw_polygon(self._rect, pts, color)
def _draw_two_quads_from_6pts_carrot(self, x, y, fill_color: rl.Color, brake_valid: bool, color_idx: int):
pts = np.array(list(zip(x, y)), dtype=np.float32)
if pts.shape[0] != 6:
return
left_pts = np.array([pts[0], pts[1], pts[2], pts[5]], dtype=np.float32)
right_pts = np.array([pts[5], pts[2], pts[3], pts[4]], dtype=np.float32)
draw_polygon(self._rect, left_pts, fill_color)
draw_polygon(self._rect, right_pts, fill_color)
if color_idx >= 10 or brake_valid:
self._draw_polygon_outline_carrot(
pts,
rl.Color(255, 0, 0, 255) if brake_valid else rl.Color(255, 255, 255, 255),
2.0,
)
def _draw_polygon_from_xy_carrot(self, xs, ys, fill_color: rl.Color, brake_valid: bool, color_idx: int):
pts = np.array(list(zip(xs, ys)), dtype=np.float32)
# 연속 중복점 제거
if pts.shape[0] >= 2:
keep = [0]
for i in range(1, pts.shape[0]):
@@ -653,7 +683,6 @@ class ModelRenderer(Widget):
keep.append(i)
pts = pts[keep]
# 마지막 점이 첫 점과 거의 같으면 제거
if pts.shape[0] >= 3:
if abs(float(pts[0][0] - pts[-1][0])) < 1e-3 and abs(float(pts[0][1] - pts[-1][1])) < 1e-3:
pts = pts[:-1]
@@ -661,18 +690,6 @@ class ModelRenderer(Widget):
if pts.shape[0] < 3:
return
# shader_polygon.draw_polygon은 일부 concave/복잡한 도형에서 채움이 깨질 수 있어서
# carrot path용은 ear clipping으로 삼각형 분할 후 직접 채움
tris = self._triangulate_polygon_carrot(pts)
if tris:
for a, b, c in tris:
rl.draw_triangle(
rl.Vector2(float(a[0]), float(a[1])),
rl.Vector2(float(b[0]), float(b[1])),
rl.Vector2(float(c[0]), float(c[1])),
fill_color,
)
else:
draw_polygon(self._rect, pts, fill_color)
if color_idx >= 10 or brake_valid:
@@ -701,7 +718,7 @@ class ModelRenderer(Widget):
def _draw_text_box_carrot(self, x: int, y: int, text: str, font_size: int, box_color: rl.Color):
w = max(40, int(len(text) * font_size * 0.8))
h = 42
self._draw_rect_fill_outline_carrot(x - w / 2, y - 35, w, h, box_color, box_color, 0.0)
self._draw_rect_fill_outline_carrot(x - w / 2, y - 22, w, h, box_color, box_color, 0.0)
draw_text_ui_style(
text,
x,
@@ -963,6 +980,133 @@ class ModelRenderer(Widget):
draw_polygon(self._rect, road_vertices[i], color)
def _update_blind_spot_barriers_carrot(self, sm):
if not sm.valid['modelV2']:
self._carrot_lane_barrier_vertices[0] = np.empty((0, 2), dtype=np.float32)
self._carrot_lane_barrier_vertices[1] = np.empty((0, 2), dtype=np.float32)
return
model = sm['modelV2']
if len(model.position.x) == 0:
self._carrot_lane_barrier_vertices[0] = np.empty((0, 2), dtype=np.float32)
self._carrot_lane_barrier_vertices[1] = np.empty((0, 2), dtype=np.float32)
return
model_position = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T
max_idx_barrier = self._get_path_length_idx(model_position[:, 0], 40.0)
self._carrot_lane_barrier_vertices[0] = self._build_path_polygon_update_line_data2_carrot(
model_position,
1.2 - 0.05,
1.2 - 0.6,
1.2 - 0.6,
max_idx_barrier,
False,
)
self._carrot_lane_barrier_vertices[0][:, 0] = self._carrot_lane_barrier_vertices[0][:, 0]
self._carrot_lane_barrier_vertices[1] = self._build_path_polygon_update_line_data2_carrot(
model_position,
1.2 - 0.05,
1.2 - 0.6,
1.2 - 0.6,
max_idx_barrier,
False,
)
# Shift left/right from vehicle center, matching carrot.cc offset usage
if self._carrot_lane_barrier_vertices[0].size != 0:
left_points = []
right_points = []
for i in range(0, max_idx_barrier + 1):
if model_position[i, 0] < 0:
continue
z_off = self._carrot_interp(float(model_position[i, 0]), [0.0, 100.0], [1.2 - 0.6, 1.2 - 0.6])
y_off = self._carrot_interp(z_off, [-3.0, 0.0, 3.0], [1.5, 0.5, 1.5]) * (1.2 - 0.05)
left = self._map_to_screen(model_position[i, 0], model_position[i, 1] - y_off - 1.7, model_position[i, 2] + z_off)
right = self._map_to_screen(model_position[i, 0], model_position[i, 1] + y_off - 1.7, model_position[i, 2] + z_off)
if left is not None and right is not None:
if len(left_points) > 0 and left[1] > left_points[-1][1]:
continue
left_points.append(left)
right_points.insert(0, right)
self._carrot_lane_barrier_vertices[0] = np.array(left_points + right_points, dtype=np.float32) if len(left_points) > 0 else np.empty((0, 2), dtype=np.float32)
if self._carrot_lane_barrier_vertices[1].size != 0:
left_points = []
right_points = []
for i in range(0, max_idx_barrier + 1):
if model_position[i, 0] < 0:
continue
z_off = self._carrot_interp(float(model_position[i, 0]), [0.0, 100.0], [1.2 - 0.6, 1.2 - 0.6])
y_off = self._carrot_interp(z_off, [-3.0, 0.0, 3.0], [1.5, 0.5, 1.5]) * (1.2 - 0.05)
left = self._map_to_screen(model_position[i, 0], model_position[i, 1] - y_off + 1.7, model_position[i, 2] + z_off)
right = self._map_to_screen(model_position[i, 0], model_position[i, 1] + y_off + 1.7, model_position[i, 2] + z_off)
if left is not None and right is not None:
if len(left_points) > 0 and left[1] > left_points[-1][1]:
continue
left_points.append(left)
right_points.insert(0, right)
self._carrot_lane_barrier_vertices[1] = np.array(left_points + right_points, dtype=np.float32) if len(left_points) > 0 else np.empty((0, 2), dtype=np.float32)
def _draw_blind_spot_segments_carrot(self, points: np.ndarray, color: rl.Color):
if points.size == 0:
return
count = points.shape[0]
half = count // 2
if half < 3:
return
for i in range(0, half - 2, 2):
p0 = points[i + 0]
p1 = points[i + 1]
p2 = points[count - i - 3]
p3 = points[count - i - 2]
self._draw_quad_fill_carrot(p0, p1, p2, p3, color)
self._draw_line_segment_carrot(p0, p1, color, 3.0)
self._draw_line_segment_carrot(p1, p2, color, 3.0)
self._draw_line_segment_carrot(p2, p3, color, 3.0)
self._draw_line_segment_carrot(p3, p0, color, 3.0)
def _draw_blind_spot_carrot(self, sm):
if not sm.valid['modelV2'] or not sm.valid['carState'] or not sm.valid['radarState']:
return
self._update_blind_spot_barriers_carrot(sm)
warn_color = rl.Color(255, 215, 0, 150)
assist_color = rl.Color(0, 204, 0, 150)
car_state = sm['carState']
radar_state = sm['radarState']
meta = sm['modelV2'].meta
left_blindspot = bool(car_state.leftBlindspot)
right_blindspot = bool(car_state.rightBlindspot)
lead_left = radar_state.leadLeft
lead_right = radar_state.leadRight
lane_change_state = meta.laneChangeState
lane_change_direction = str(meta.laneChangeDirection).lower()
right_lane_change = (lane_change_state == LaneChangeState.preLaneChange) and ("right" in lane_change_direction)
left_lane_change = (lane_change_state == LaneChangeState.preLaneChange) and ("left" in lane_change_direction)
if left_blindspot:
self._draw_blind_spot_segments_carrot(self._carrot_lane_barrier_vertices[0], warn_color)
elif lead_left.status and float(lead_left.dRel) < float(car_state.vEgo) * 3.0 and left_lane_change:
self._draw_blind_spot_segments_carrot(self._carrot_lane_barrier_vertices[0], assist_color)
if right_blindspot:
self._draw_blind_spot_segments_carrot(self._carrot_lane_barrier_vertices[1], warn_color)
elif lead_right.status and float(lead_right.dRel) < float(car_state.vEgo) * 3.0 and right_lane_change:
self._draw_blind_spot_segments_carrot(self._carrot_lane_barrier_vertices[1], assist_color)
def _draw_radar_info_carrot(self, sm):
if self._carrot_show_radar_info <= 0:
return
@@ -1316,7 +1460,7 @@ class ModelRenderer(Widget):
x[3] = float(track_vertices[e - 1][0]); y[3] = float(track_vertices[e - 1][1])
x[4] = float(track_vertices[e][0]); y[4] = float(track_vertices[e][1])
x[5] = (x[1] + x[3]) / 2.0; y[5] = (y[1] + y[3]) / 2.0
self._draw_polygon_from_xy_carrot(x, y, self._carrot_colors[color_idx % 10], brake_valid, color_idx)
self._draw_two_quads_from_6pts_carrot(x, y, self._carrot_colors[color_idx % 10], brake_valid, color_idx)
color_n += 1
if color_n > 6:
color_n = 0
@@ -1381,7 +1525,7 @@ class ModelRenderer(Widget):
if draw_seg:
idx = color_n if mode in (4, 8) else color_idx % 10
self._draw_polygon_from_xy_carrot(x, y, self._carrot_colors[idx], brake_valid, color_idx)
self._draw_two_quads_from_6pts_carrot(x, y, self._carrot_colors[idx], brake_valid, color_idx)
if i > 1:
color_n += 1

View File

@@ -72,7 +72,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
AudibleAlert.audio9: ("audio_9.wav", None, MAX_VOLUME),
AudibleAlert.audio10: ("audio_10.wav", None, MAX_VOLUME),
}
if HARDWARE.get_device_type() == "tizi" or True:
if HARDWARE.get_device_type() == "tizi":
sound_list.update({
AudibleAlert.engage: ("engage_tizi.wav", 1, float(Params().get_int("SoundVolumeAdjustEngage"))/100.),
AudibleAlert.disengage: ("disengage_tizi.wav", 1, float(Params().get_int("SoundVolumeAdjustEngage"))/100.),

View File

@@ -18,6 +18,6 @@ class FanController:
self.last_ignition = ignition
return int(self.controller.update(
error=(cur_temp - 75), # temperature setpoint in C
error=(cur_temp - 70), # temperature setpoint in C
feedforward=np.interp(cur_temp, [60.0, 100.0], [0, 100])
))

View File

@@ -23,7 +23,8 @@ from openpilot.system.ui.lib.multilang import multilang
from openpilot.common.realtime import Ratekeeper
import datetime
_DEFAULT_FPS = int(os.getenv("FPS", {'tizi': 20}.get(HARDWARE.get_device_type(), 60)))
#_DEFAULT_FPS = int(os.getenv("FPS", {'tizi': 20}.get(HARDWARE.get_device_type(), 60)))
_DEFAULT_FPS = 20
FPS_LOG_INTERVAL = 5 # Seconds between logging FPS drops
FPS_DROP_THRESHOLD = 0.9 # FPS drop threshold for triggering a warning
FPS_CRITICAL_THRESHOLD = 0.5 # Critical threshold for triggering strict actions

View File

@@ -383,6 +383,9 @@ function op_switch() {
git submodule update --init --recursive
git submodule foreach git reset --hard
git submodule foreach git clean -df
# remove openpilot update flag if present
rm -f .overlay_init
}
function op_start() {