Skip to content

Commit

Permalink
merge
Browse files Browse the repository at this point in the history
  • Loading branch information
@gijskoning
gijskoning committed Jun 20, 2022
1 parent 1c4b145 commit b18e613
Show file tree
Hide file tree
Showing 4 changed files with 131 additions and 99 deletions.
20 changes: 15 additions & 5 deletions selfdrive/locationd/laikad.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import json
import os
import time
from collections import defaultdict
from concurrent.futures import Future, ProcessPoolExecutor
Expand Down Expand Up @@ -44,9 +45,13 @@ def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephe
self.last_pos_fix_t = None

def load_cache(self):
if not self.save_ephemeris:
return

cache = Params().get(EPHEMERIS_CACHE)
if not cache:
return

try:
cache = json.loads(cache, object_hook=deserialize_hook)
self.astro_dog.add_orbits(cache['orbits'])
Expand Down Expand Up @@ -251,17 +256,22 @@ class EphemerisSourceType(IntEnum):
glonassIacUltraRapid = 2


def main():
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['ubloxGnss'])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])

replay = "REPLAY" in os.environ

# todo get last_known_position
laikad = Laikad(save_ephemeris=True)
laikad = Laikad(save_ephemeris=not replay)
while True:
sm.update()

if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'])
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'], block=replay)
if msg is not None:
pm.send('gnssMeasurements', msg)

Expand Down
3 changes: 2 additions & 1 deletion selfdrive/test/process_replay/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Use `FILEREADER_CACHE='1' test_processes.py` to cache log files.

Currently the following processes are tested:

Expand All @@ -15,6 +15,7 @@ Currently the following processes are tested:
* calibrationd
* dmonitoringd
* locationd
* laikad
* paramsd
* ubloxd

Expand Down
204 changes: 111 additions & 93 deletions selfdrive/test/process_replay/process_replay.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,107 +236,125 @@ def ublox_rcv_callback(msg):
return []


def laika_rcv_callback(msg, CP, cfg, fsm):
if msg.ubloxGnss.which() == "measurementReport":
return ["gnssMeasurements"], True
else:
return [], False


CONFIGS = [
# ProcessConfig(
# proc_name="controlsd",
# pub_sub={
# "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
# "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
# "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [],
# },
# ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
# init_callback=fingerprint,
# should_recv_callback=controlsd_rcv_callback,
# tolerance=NUMPY_TOLERANCE,
# fake_pubsubmaster=True,
# submaster_config={
# 'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz
# 'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen
# }
# ),
# ProcessConfig(
# proc_name="radard",
# pub_sub={
# "can": ["radarState", "liveTracks"],
# "liveParameters": [], "carState": [], "modelV2": [],
# },
# ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
# init_callback=get_car_params,
# should_recv_callback=radar_rcv_callback,
# tolerance=None,
# fake_pubsubmaster=True,
# ),
# ProcessConfig(
# proc_name="plannerd",
# pub_sub={
# "modelV2": ["lateralPlan", "longitudinalPlan"],
# "carState": [], "controlsState": [], "radarState": [],
# },
# ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"],
# init_callback=get_car_params,
# should_recv_callback=None,
# tolerance=NUMPY_TOLERANCE,
# fake_pubsubmaster=True,
# ),
# ProcessConfig(
# proc_name="calibrationd",
# pub_sub={
# "carState": ["liveCalibration"],
# "cameraOdometry": []
# },
# ignore=["logMonoTime", "valid"],
# init_callback=get_car_params,
# should_recv_callback=calibration_rcv_callback,
# tolerance=None,
# fake_pubsubmaster=True,
# ),
# ProcessConfig(
# proc_name="dmonitoringd",
# pub_sub={
# "driverState": ["driverMonitoringState"],
# "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
# },
# ignore=["logMonoTime", "valid"],
# init_callback=get_car_params,
# should_recv_callback=None,
# tolerance=NUMPY_TOLERANCE,
# fake_pubsubmaster=True,
# ),
# ProcessConfig(
# proc_name="locationd",
# pub_sub={
# "cameraOdometry": ["liveLocationKalman"],
# "sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
# },
# ignore=["logMonoTime", "valid"],
# init_callback=get_car_params,
# should_recv_callback=None,
# tolerance=NUMPY_TOLERANCE,
# fake_pubsubmaster=False,
# ),
# ProcessConfig(
# proc_name="paramsd",
# pub_sub={
# "liveLocationKalman": ["liveParameters"],
# "carState": []
# },
# ignore=["logMonoTime", "valid"],
# init_callback=get_car_params,
# should_recv_callback=None,
# tolerance=NUMPY_TOLERANCE,
# fake_pubsubmaster=True,
# ),
# ProcessConfig(
# proc_name="ubloxd",
# pub_sub={
# "ubloxRaw": ["ubloxGnss", "gpsLocationExternal"],
# },
# ignore=["logMonoTime"],
# init_callback=None,
# should_recv_callback=ublox_rcv_callback,
# tolerance=None,
# fake_pubsubmaster=False,
# ),
ProcessConfig(
proc_name="controlsd",
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
"modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [],
},
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
init_callback=fingerprint,
should_recv_callback=controlsd_rcv_callback,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
submaster_config={
'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz
'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen
}
),
ProcessConfig(
proc_name="radard",
pub_sub={
"can": ["radarState", "liveTracks"],
"liveParameters": [], "carState": [], "modelV2": [],
},
ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params,
should_recv_callback=radar_rcv_callback,
tolerance=None,
fake_pubsubmaster=True,
),
ProcessConfig(
proc_name="plannerd",
pub_sub={
"modelV2": ["lateralPlan", "longitudinalPlan"],
"carState": [], "controlsState": [], "radarState": [],
},
ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"],
init_callback=get_car_params,
should_recv_callback=None,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
),
ProcessConfig(
proc_name="calibrationd",
pub_sub={
"carState": ["liveCalibration"],
"cameraOdometry": []
},
ignore=["logMonoTime", "valid"],
init_callback=get_car_params,
should_recv_callback=calibration_rcv_callback,
tolerance=None,
fake_pubsubmaster=True,
),
ProcessConfig(
proc_name="dmonitoringd",
pub_sub={
"driverState": ["driverMonitoringState"],
"liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
},
ignore=["logMonoTime", "valid"],
init_callback=get_car_params,
should_recv_callback=None,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
),
ProcessConfig(
proc_name="locationd",
proc_name="laikad",
pub_sub={
"cameraOdometry": ["liveLocationKalman"],
"sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
"ubloxGnss": ["gnssMeasurements"],
},
ignore=["logMonoTime", "valid"],
init_callback=get_car_params,
should_recv_callback=None,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=False,
),
ProcessConfig(
proc_name="paramsd",
pub_sub={
"liveLocationKalman": ["liveParameters"],
"carState": []
},
ignore=["logMonoTime", "valid"],
ignore=["logMonoTime"],
init_callback=get_car_params,
should_recv_callback=None,
should_recv_callback=laika_rcv_callback,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
),
ProcessConfig(
proc_name="ubloxd",
pub_sub={
"ubloxRaw": ["ubloxGnss", "gpsLocationExternal"],
},
ignore=["logMonoTime"],
init_callback=None,
should_recv_callback=ublox_rcv_callback,
tolerance=None,
fake_pubsubmaster=False,
),
]


Expand Down
3 changes: 3 additions & 0 deletions selfdrive/test/profiling/profiler.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ def profile(proc, func, car='toyota'):
msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1"))

os.environ['FINGERPRINT'] = fingerprint
os.environ['REPLAY'] = "1"

def run(sm, pm, can_sock):
try:
Expand Down Expand Up @@ -81,12 +82,14 @@ def run(sm, pm, can_sock):
from selfdrive.controls.radard import radard_thread
from selfdrive.locationd.paramsd import main as paramsd_thread
from selfdrive.controls.plannerd import main as plannerd_thread
from selfdrive.locationd.laikad import main as laikad_thread

procs = {
'radard': radard_thread,
'controlsd': controlsd_thread,
'paramsd': paramsd_thread,
'plannerd': plannerd_thread,
'laikad': laikad_thread,
}

proc = sys.argv[1]
Expand Down

0 comments on commit b18e613

Please sign in to comment.