Add openpilot tests
This commit is contained in:
FrogAi
800
selfdrive/test/process_replay/process_replay.py
Normal file
800
selfdrive/test/process_replay/process_replay.py
Normal file
@@ -0,0 +1,800 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import copy
|
||||
import json
|
||||
import heapq
|
||||
import signal
|
||||
import platform
|
||||
from collections import OrderedDict
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
from collections.abc import Callable, Iterable
|
||||
from tqdm import tqdm
|
||||
import capnp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car
|
||||
from cereal.services import SERVICE_LIST
|
||||
from cereal.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name
|
||||
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 panda.python import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.selfdrive.car.car_helpers import get_car, interfaces
|
||||
from openpilot.selfdrive.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
|
||||
|
||||
# Numpy gives different results based on CPU features after version 19
|
||||
NUMPY_TOLERANCE = 1e-7
|
||||
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):
|
||||
self.capture = capture
|
||||
self.launcher = launcher
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
self.capture.link_with_current_proc()
|
||||
self.launcher(*args, **kwargs)
|
||||
|
||||
|
||||
class ReplayContext:
|
||||
def __init__(self, cfg):
|
||||
self.proc_name = cfg.proc_name
|
||||
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)
|
||||
|
||||
def __enter__(self):
|
||||
self.open_context()
|
||||
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_obj, exc_tb):
|
||||
self.close_context()
|
||||
|
||||
def open_context(self):
|
||||
messaging.toggle_fake_events(True)
|
||||
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[pub] = messaging.fake_event_handle(pub, enable=True)
|
||||
else:
|
||||
self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)}
|
||||
|
||||
def close_context(self):
|
||||
del self.events
|
||||
|
||||
messaging.toggle_fake_events(False)
|
||||
messaging.delete_fake_prefix()
|
||||
|
||||
@property
|
||||
def all_recv_called_events(self):
|
||||
return [man.recv_called_event for man in self.events.values()]
|
||||
|
||||
@property
|
||||
def all_recv_ready_events(self):
|
||||
return [man.recv_ready_event for man in self.events.values()]
|
||||
|
||||
def send_sync(self, pm, endpoint, dat):
|
||||
self.events[endpoint].recv_called_event.wait()
|
||||
self.events[endpoint].recv_called_event.clear()
|
||||
pm.send(endpoint, dat)
|
||||
self.events[endpoint].recv_ready_event.set()
|
||||
|
||||
def unlock_sockets(self):
|
||||
expected_sets = len(self.events)
|
||||
while expected_sets > 0:
|
||||
index = messaging.wait_for_one_event(self.all_recv_called_events)
|
||||
self.all_recv_called_events[index].clear()
|
||||
self.all_recv_ready_events[index].set()
|
||||
expected_sets -= 1
|
||||
|
||||
def wait_for_recv_called(self):
|
||||
messaging.wait_for_one_event(self.all_recv_called_events)
|
||||
|
||||
def wait_for_next_recv(self, trigger_empty_recv):
|
||||
index = messaging.wait_for_one_event(self.all_recv_called_events)
|
||||
if self.main_pub is not None and self.main_pub_drained and trigger_empty_recv:
|
||||
self.all_recv_called_events[index].clear()
|
||||
self.all_recv_ready_events[index].set()
|
||||
self.all_recv_called_events[index].wait()
|
||||
|
||||
|
||||
@dataclass
|
||||
class ProcessConfig:
|
||||
proc_name: str
|
||||
pubs: list[str]
|
||||
subs: list[str]
|
||||
ignore: list[str]
|
||||
config_callback: Callable | None = None
|
||||
init_callback: Callable | None = None
|
||||
should_recv_callback: Callable | None = None
|
||||
tolerance: float | None = None
|
||||
processing_time: float = 0.001
|
||||
timeout: int = 30
|
||||
simulation: bool = True
|
||||
main_pub: str | None = None
|
||||
main_pub_drained: bool = True
|
||||
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)
|
||||
|
||||
|
||||
class ProcessContainer:
|
||||
def __init__(self, cfg: ProcessConfig):
|
||||
self.prefix = OpenpilotPrefix(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.cnt = 0
|
||||
self.pm: messaging.PubMaster | None = None
|
||||
self.sockets: list[messaging.SubSocket] | None = None
|
||||
self.rc: ReplayContext | None = None
|
||||
self.vipc_server: VisionIpcServer | None = None
|
||||
self.environ_config: dict[str, Any] | None = None
|
||||
self.capture: ProcessOutputCapture | None = None
|
||||
|
||||
@property
|
||||
def has_empty_queue(self) -> bool:
|
||||
return len(self.msg_queue) == 0
|
||||
|
||||
@property
|
||||
def pubs(self) -> list[str]:
|
||||
return self.cfg.pubs
|
||||
|
||||
@property
|
||||
def subs(self) -> list[str]:
|
||||
return self.cfg.subs
|
||||
|
||||
def _clean_env(self):
|
||||
for k in self.environ_config.keys():
|
||||
if k in os.environ:
|
||||
del os.environ[k]
|
||||
|
||||
for k in ["PROC_NAME", "SIMULATION"]:
|
||||
if k in os.environ:
|
||||
del os.environ[k]
|
||||
|
||||
def _setup_env(self, params_config: dict[str, Any], environ_config: dict[str, Any]):
|
||||
for k, v in environ_config.items():
|
||||
if len(v) != 0:
|
||||
os.environ[k] = v
|
||||
elif k in os.environ:
|
||||
del os.environ[k]
|
||||
|
||||
os.environ["PROC_NAME"] = self.cfg.proc_name
|
||||
if self.cfg.simulation:
|
||||
os.environ["SIMULATION"] = "1"
|
||||
elif "SIMULATION" in os.environ:
|
||||
del os.environ["SIMULATION"]
|
||||
|
||||
params = Params()
|
||||
for k, v in params_config.items():
|
||||
if isinstance(v, bool):
|
||||
params.put_bool(k, v)
|
||||
else:
|
||||
params.put(k, v)
|
||||
|
||||
self.environ_config = environ_config
|
||||
|
||||
def _setup_vision_ipc(self, all_msgs: LogIterable, frs: dict[str, Any]):
|
||||
assert len(self.cfg.vision_pubs) != 0
|
||||
|
||||
vipc_server = VisionIpcServer("camerad")
|
||||
streams_metas = available_streams(all_msgs)
|
||||
for meta in streams_metas:
|
||||
if meta.camera_state in self.cfg.vision_pubs:
|
||||
frame_size = (frs[meta.camera_state].w, frs[meta.camera_state].h)
|
||||
vipc_server.create_buffers(meta.stream, 2, False, *frame_size)
|
||||
vipc_server.start_listener()
|
||||
|
||||
self.vipc_server = vipc_server
|
||||
self.cfg.vision_pubs = [meta.camera_state for meta in streams_metas if meta.camera_state in self.cfg.vision_pubs]
|
||||
|
||||
def _start_process(self):
|
||||
if self.capture is not None:
|
||||
self.process.launcher = LauncherWithCapture(self.capture, self.process.launcher)
|
||||
self.process.prepare()
|
||||
self.process.start()
|
||||
|
||||
def start(
|
||||
self, params_config: dict[str, Any], environ_config: dict[str, Any],
|
||||
all_msgs: LogIterable, frs: dict[str, BaseFrameReader] | None,
|
||||
fingerprint: str | None, capture_output: bool
|
||||
):
|
||||
with self.prefix as p:
|
||||
self._setup_env(params_config, environ_config)
|
||||
|
||||
if self.cfg.config_callback is not None:
|
||||
params = Params()
|
||||
self.cfg.config_callback(params, self.cfg, all_msgs)
|
||||
|
||||
self.rc = ReplayContext(self.cfg)
|
||||
self.rc.open_context()
|
||||
|
||||
self.pm = messaging.PubMaster(self.cfg.pubs)
|
||||
self.sockets = [messaging.sub_sock(s, timeout=100) for s in self.cfg.subs]
|
||||
|
||||
if len(self.cfg.vision_pubs) != 0:
|
||||
assert frs is not None
|
||||
self._setup_vision_ipc(all_msgs, frs)
|
||||
assert self.vipc_server is not None
|
||||
|
||||
if capture_output:
|
||||
self.capture = ProcessOutputCapture(self.cfg.proc_name, p.prefix)
|
||||
|
||||
self._start_process()
|
||||
|
||||
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)
|
||||
self.process.stop()
|
||||
self.rc.close_context()
|
||||
self.prefix.clean_dirs()
|
||||
self._clean_env()
|
||||
|
||||
def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, BaseFrameReader] | 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()
|
||||
|
||||
# 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
|
||||
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)
|
||||
|
||||
for m in self.msg_queue:
|
||||
self.pm.send(m.which(), m.as_builder())
|
||||
# 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(),
|
||||
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())
|
||||
self.cnt += 1
|
||||
assert self.process.proc.is_alive()
|
||||
|
||||
return output_msgs
|
||||
|
||||
|
||||
def controlsd_fingerprint_callback(rc, pm, msgs, fingerprint):
|
||||
print("start fingerprinting")
|
||||
params = Params()
|
||||
canmsgs = [msg for msg in msgs if msg.which() == "can"][:300]
|
||||
|
||||
# controlsd expects one arbitrary can and pandaState
|
||||
rc.send_sync(pm, "can", messaging.new_message("can", 1))
|
||||
pm.send("pandaStates", messaging.new_message("pandaStates", 1))
|
||||
rc.send_sync(pm, "can", messaging.new_message("can", 1))
|
||||
rc.wait_for_next_recv(True)
|
||||
|
||||
# fingerprinting is done, when CarParams is set
|
||||
while params.get("CarParams") is None:
|
||||
if len(canmsgs) == 0:
|
||||
raise ValueError("Fingerprinting failed. Run out of can msgs")
|
||||
|
||||
m = canmsgs.pop(0)
|
||||
rc.send_sync(pm, "can", m.as_builder().to_bytes())
|
||||
rc.wait_for_next_recv(False)
|
||||
|
||||
|
||||
def get_car_params_callback(rc, pm, msgs, fingerprint):
|
||||
params = Params()
|
||||
if 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"]
|
||||
has_cached_cp = params.get("CarParamsCache") 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, \
|
||||
"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())
|
||||
_, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled"))
|
||||
params.put("CarParams", CP.to_bytes())
|
||||
return CP
|
||||
|
||||
|
||||
def controlsd_rcv_callback(msg, cfg, frame):
|
||||
# no sendcan until controlsd is initialized
|
||||
if msg.which() != "can":
|
||||
return False
|
||||
|
||||
socks = [
|
||||
s for s in cfg.subs if
|
||||
frame % int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency) == 0
|
||||
]
|
||||
if "sendcan" in socks and (frame - 1) < 2000:
|
||||
socks.remove("sendcan")
|
||||
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() == 'liveLocationKalman'
|
||||
|
||||
|
||||
def dmonitoringmodeld_rcv_callback(msg, cfg, frame):
|
||||
return msg.which() == "driverCameraState"
|
||||
|
||||
|
||||
class ModeldCameraSyncRcvCallback:
|
||||
def __init__(self):
|
||||
self.road_present = False
|
||||
self.wide_road_present = False
|
||||
self.is_dual_camera = True
|
||||
|
||||
def __call__(self, msg, cfg, frame):
|
||||
self.is_dual_camera = len(cfg.vision_pubs) == 2
|
||||
if msg.which() == "roadCameraState":
|
||||
self.road_present = True
|
||||
elif msg.which() == "wideRoadCameraState":
|
||||
self.wide_road_present = True
|
||||
|
||||
if self.road_present and self.wide_road_present:
|
||||
self.road_present, self.wide_road_present = False, False
|
||||
return True
|
||||
elif self.road_present and not self.is_dual_camera:
|
||||
self.road_present = False
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
class MessageBasedRcvCallback:
|
||||
def __init__(self, trigger_msg_type):
|
||||
self.trigger_msg_type = trigger_msg_type
|
||||
|
||||
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))
|
||||
|
||||
|
||||
def controlsd_config_callback(params, cfg, lr):
|
||||
controlsState = None
|
||||
initialized = False
|
||||
for msg in lr:
|
||||
if msg.which() == "controlsState":
|
||||
controlsState = msg.controlsState
|
||||
if initialized:
|
||||
break
|
||||
elif msg.which() == "onroadEvents":
|
||||
initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.onroadEvents]
|
||||
|
||||
assert controlsState is not None and initialized, "controlsState never initialized"
|
||||
params.put("ReplayControlsState", controlsState.as_builder().to_bytes())
|
||||
|
||||
|
||||
def locationd_config_pubsub_callback(params, cfg, lr):
|
||||
ublox = params.get_bool("UbloxAvailable")
|
||||
sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
|
||||
|
||||
cfg.pubs = set(cfg.pubs) - sub_keys
|
||||
|
||||
|
||||
CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="controlsd",
|
||||
pubs=[
|
||||
"can", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
|
||||
"longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState",
|
||||
"modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
|
||||
"testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope"
|
||||
],
|
||||
subs=["controlsState", "carState", "carControl", "sendcan", "onroadEvents", "carParams"],
|
||||
ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
|
||||
config_callback=controlsd_config_callback,
|
||||
init_callback=controlsd_fingerprint_callback,
|
||||
should_recv_callback=controlsd_rcv_callback,
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
processing_time=0.004,
|
||||
main_pub="can",
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="radard",
|
||||
pubs=["can", "carState", "modelV2"],
|
||||
subs=["radarState", "liveTracks"],
|
||||
ignore=["logMonoTime", "radarState.cumLagMs"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=MessageBasedRcvCallback("can"),
|
||||
main_pub="can",
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="plannerd",
|
||||
pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"],
|
||||
subs=["longitudinalPlan", "uiPlan"],
|
||||
ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=FrequencyBasedRcvCallback("modelV2"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="calibrationd",
|
||||
pubs=["carState", "cameraOdometry", "carParams"],
|
||||
subs=["liveCalibration"],
|
||||
ignore=["logMonoTime"],
|
||||
should_recv_callback=calibration_rcv_callback,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="dmonitoringd",
|
||||
pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"],
|
||||
subs=["driverMonitoringState"],
|
||||
ignore=["logMonoTime"],
|
||||
should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="locationd",
|
||||
pubs=[
|
||||
"cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
|
||||
"liveCalibration", "carState", "gpsLocation"
|
||||
],
|
||||
subs=["liveLocationKalman"],
|
||||
ignore=["logMonoTime"],
|
||||
config_callback=locationd_config_pubsub_callback,
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="paramsd",
|
||||
pubs=["liveLocationKalman", "carState"],
|
||||
subs=["liveParameters"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=FrequencyBasedRcvCallback("liveLocationKalman"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
processing_time=0.004,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="ubloxd",
|
||||
pubs=["ubloxRaw"],
|
||||
subs=["ubloxGnss", "gpsLocationExternal"],
|
||||
ignore=["logMonoTime"],
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="torqued",
|
||||
pubs=["liveLocationKalman", "carState", "carControl"],
|
||||
subs=["liveTorqueParameters"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=torqued_rcv_callback,
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="modeld",
|
||||
pubs=["roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
|
||||
subs=["modelV2", "cameraOdometry"],
|
||||
ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.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,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="dmonitoringmodeld",
|
||||
pubs=["liveCalibration", "driverCameraState"],
|
||||
subs=["driverStateV2"],
|
||||
ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.dspExecutionTime"],
|
||||
should_recv_callback=dmonitoringmodeld_rcv_callback,
|
||||
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"],
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def get_process_config(name: str) -> ProcessConfig:
|
||||
try:
|
||||
return copy.deepcopy(next(c for c in CONFIGS if c.proc_name == name))
|
||||
except StopIteration as ex:
|
||||
raise Exception(f"Cannot find process config with name: {name}") from ex
|
||||
|
||||
|
||||
def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> dict[str, Any]:
|
||||
"""
|
||||
Use this to get custom params dict based on provided logs.
|
||||
Useful when replaying following processes: calibrationd, paramsd, torqued
|
||||
The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs.
|
||||
"""
|
||||
|
||||
car_params = [m for m in lr if m.which() == "carParams"]
|
||||
live_calibration = [m for m in lr if m.which() == "liveCalibration"]
|
||||
live_parameters = [m for m in lr if m.which() == "liveParameters"]
|
||||
live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"]
|
||||
|
||||
assert initial_state in ["first", "last"]
|
||||
msg_index = 0 if initial_state == "first" else -1
|
||||
|
||||
assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute"
|
||||
CP = car_params[msg_index].carParams
|
||||
|
||||
custom_params = {
|
||||
"CarParamsPrevRoute": CP.as_builder().to_bytes()
|
||||
}
|
||||
|
||||
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)
|
||||
if len(live_torque_parameters) > 0:
|
||||
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
|
||||
|
||||
return custom_params
|
||||
|
||||
|
||||
def replay_process_with_name(name: str | Iterable[str], lr: LogIterable, *args, **kwargs) -> list[capnp._DynamicStructReader]:
|
||||
if isinstance(name, str):
|
||||
cfgs = [get_process_config(name)]
|
||||
elif isinstance(name, Iterable):
|
||||
cfgs = [get_process_config(n) for n in name]
|
||||
else:
|
||||
raise ValueError("name must be str or collections of strings")
|
||||
|
||||
return replay_process(cfgs, lr, *args, **kwargs)
|
||||
|
||||
|
||||
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
|
||||
) -> list[capnp._DynamicStructReader]:
|
||||
if isinstance(cfg, Iterable):
|
||||
cfgs = list(cfg)
|
||||
else:
|
||||
cfgs = [cfg]
|
||||
|
||||
all_msgs = migrate_all(lr, old_logtime=True,
|
||||
manager_states=True,
|
||||
panda_states=any("pandaStates" in cfg.pubs for cfg in cfgs),
|
||||
camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs))
|
||||
process_logs = _replay_multi_process(cfgs, all_msgs, frs, fingerprint, custom_params, captured_output_store, disable_progress)
|
||||
|
||||
if return_all_logs:
|
||||
keys = {m.which() for m in process_logs}
|
||||
modified_logs = [m for m in all_msgs if m.which() not in keys]
|
||||
modified_logs.extend(process_logs)
|
||||
modified_logs.sort(key=lambda m: int(m.logMonoTime))
|
||||
log_msgs = modified_logs
|
||||
else:
|
||||
log_msgs = process_logs
|
||||
|
||||
return log_msgs
|
||||
|
||||
|
||||
def _replay_multi_process(
|
||||
cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] | 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:
|
||||
params_config = generate_params_config(lr=lr, fingerprint=fingerprint, custom_params=custom_params)
|
||||
env_config = generate_environ_config(fingerprint=fingerprint)
|
||||
else:
|
||||
CP = next((m.carParams for m in lr if m.which() == "carParams"), None)
|
||||
params_config = generate_params_config(lr=lr, CP=CP, custom_params=custom_params)
|
||||
env_config = generate_environ_config(CP=CP)
|
||||
|
||||
# validate frs and vision pubs
|
||||
all_vision_pubs = [pub for cfg in cfgs for pub in cfg.vision_pubs]
|
||||
if len(all_vision_pubs) != 0:
|
||||
assert frs is not None, "frs must be provided when replaying process using vision streams"
|
||||
assert all(meta_from_camera_state(st) is not None for st in all_vision_pubs), \
|
||||
f"undefined vision stream spotted, probably misconfigured process: (vision pubs: {all_vision_pubs})"
|
||||
required_vision_pubs = {m.camera_state for m in available_streams(lr)} & set(all_vision_pubs)
|
||||
assert all(st in frs for st in required_vision_pubs), f"frs for this process must contain following vision streams: {required_vision_pubs}"
|
||||
|
||||
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
|
||||
log_msgs = []
|
||||
try:
|
||||
containers = []
|
||||
for cfg in cfgs:
|
||||
container = ProcessContainer(cfg)
|
||||
containers.append(container)
|
||||
container.start(params_config, env_config, all_msgs, frs, fingerprint, captured_output_store is not None)
|
||||
|
||||
all_pubs = {pub for container in containers for pub in container.pubs}
|
||||
all_subs = {sub for container in containers for sub in container.subs}
|
||||
lr_pubs = all_pubs - all_subs
|
||||
pubs_to_containers = {pub: [container for container in containers if pub in container.pubs] for pub in all_pubs}
|
||||
|
||||
pub_msgs = [msg for msg in all_msgs if msg.which() in lr_pubs]
|
||||
# external queue for messages taken from logs; internal queue for messages generated by processes, which will be republished
|
||||
external_pub_queue: list[capnp._DynamicStructReader] = pub_msgs.copy()
|
||||
internal_pub_queue: list[capnp._DynamicStructReader] = []
|
||||
# heap for maintaining the order of messages generated by processes, where each element: (logMonoTime, index in internal_pub_queue)
|
||||
internal_pub_index_heap: list[tuple[int, int]] = []
|
||||
|
||||
pbar = tqdm(total=len(external_pub_queue), disable=disable_progress)
|
||||
while len(external_pub_queue) != 0 or (len(internal_pub_index_heap) != 0 and not all(c.has_empty_queue for c in containers)):
|
||||
if len(internal_pub_index_heap) == 0 or (len(external_pub_queue) != 0 and external_pub_queue[0].logMonoTime < internal_pub_index_heap[0][0]):
|
||||
msg = external_pub_queue.pop(0)
|
||||
pbar.update(1)
|
||||
else:
|
||||
_, index = heapq.heappop(internal_pub_index_heap)
|
||||
msg = internal_pub_queue[index]
|
||||
|
||||
target_containers = pubs_to_containers[msg.which()]
|
||||
for container in target_containers:
|
||||
output_msgs = container.run_step(msg, frs)
|
||||
for m in output_msgs:
|
||||
if m.which() in all_pubs:
|
||||
internal_pub_queue.append(m)
|
||||
heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1))
|
||||
log_msgs.extend(output_msgs)
|
||||
finally:
|
||||
for container in containers:
|
||||
container.stop()
|
||||
if captured_output_store is not None:
|
||||
assert container.capture is not None
|
||||
out, err = container.capture.read_outerr()
|
||||
captured_output_store[container.cfg.proc_name] = {"out": out, "err": err}
|
||||
|
||||
return log_msgs
|
||||
|
||||
|
||||
def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=None) -> dict[str, Any]:
|
||||
params_dict = {
|
||||
"OpenpilotEnabledToggle": True,
|
||||
"DisengageOnAccelerator": True,
|
||||
"DisableLogging": False,
|
||||
}
|
||||
|
||||
if custom_params is not None:
|
||||
params_dict.update(custom_params)
|
||||
if lr is not None:
|
||||
has_ublox = any(msg.which() == "ubloxGnss" for msg in lr)
|
||||
params_dict["UbloxAvailable"] = has_ublox
|
||||
is_rhd = next((msg.driverMonitoringState.isRHD for msg in lr if msg.which() == "driverMonitoringState"), False)
|
||||
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
|
||||
|
||||
if CP.notCar:
|
||||
params_dict["JoystickDebugMode"] = True
|
||||
|
||||
return params_dict
|
||||
|
||||
|
||||
def generate_environ_config(CP=None, fingerprint=None, log_dir=None) -> dict[str, Any]:
|
||||
environ_dict = {}
|
||||
if platform.system() != "Darwin":
|
||||
environ_dict["PARAMS_ROOT"] = "/dev/shm/params"
|
||||
if log_dir is not None:
|
||||
environ_dict["LOG_ROOT"] = log_dir
|
||||
|
||||
environ_dict["REPLAY"] = "1"
|
||||
|
||||
# Regen or python process
|
||||
if CP is not None and fingerprint is None:
|
||||
if CP.fingerprintSource == "fw":
|
||||
environ_dict['SKIP_FW_QUERY'] = ""
|
||||
environ_dict['FINGERPRINT'] = ""
|
||||
else:
|
||||
environ_dict['SKIP_FW_QUERY'] = "1"
|
||||
environ_dict['FINGERPRINT'] = CP.carFingerprint
|
||||
elif fingerprint is not None:
|
||||
environ_dict['SKIP_FW_QUERY'] = "1"
|
||||
environ_dict['FINGERPRINT'] = fingerprint
|
||||
else:
|
||||
environ_dict["SKIP_FW_QUERY"] = ""
|
||||
environ_dict["FINGERPRINT"] = ""
|
||||
|
||||
return environ_dict
|
||||
|
||||
|
||||
def check_openpilot_enabled(msgs: LogIterable) -> bool:
|
||||
cur_enabled_count = 0
|
||||
max_enabled_count = 0
|
||||
for msg in msgs:
|
||||
if msg.which() == "carParams":
|
||||
if msg.carParams.notCar:
|
||||
return True
|
||||
elif msg.which() == "controlsState":
|
||||
if msg.controlsState.active:
|
||||
cur_enabled_count += 1
|
||||
else:
|
||||
cur_enabled_count = 0
|
||||
max_enabled_count = max(max_enabled_count, cur_enabled_count)
|
||||
|
||||
return max_enabled_count > int(10. / DT_CTRL)
|
||||
Reference in New Issue
Block a user