#!/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/")
[docs]
class DummySocket:
def __init__(self):
self.data: list[bytes] = []
[docs]
def receive(self, non_blocking: bool = False) -> bytes | None:
if non_blocking:
return None
return self.data.pop()
[docs]
def send(self, data: bytes):
self.data.append(data)
[docs]
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)
[docs]
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()
[docs]
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)}
[docs]
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()]
[docs]
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()
[docs]
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
[docs]
def wait_for_recv_called(self):
messaging.wait_for_one_event(self.all_recv_called_events)
[docs]
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()
[docs]
@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)
[docs]
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()
[docs]
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)
[docs]
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()
[docs]
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
[docs]
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)
[docs]
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
[docs]
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
[docs]
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'
[docs]
def torqued_rcv_callback(msg, cfg, frame):
# should_recv always true to increment frame
return (frame - 1) == 0 or msg.which() == 'liveLocationKalman'
[docs]
def dmonitoringmodeld_rcv_callback(msg, cfg, frame):
return msg.which() == "driverCameraState"
[docs]
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
[docs]
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
[docs]
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))
[docs]
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())
[docs]
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=["deviceState", "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"],
),
]
[docs]
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
[docs]
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
[docs]
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)
[docs]
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
[docs]
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
[docs]
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
[docs]
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)