diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index b4084fe5bc..1841208d59 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -277,7 +277,9 @@ def main(): learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) while True: + print(' PARAMSD before sm.update') sm.update() + print(' PARAMSD after sm.update') if sm.all_checks(): for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1e1598b46f..25f1b79d82 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -73,6 +73,7 @@ class ReplayContext: for pub in pubs_with_events: self.events[pub] = messaging.fake_event_handle(pub, enable=True) else: + print(self.proc_name, "main pub", self.main_pub, self.main_pub_drained) self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)} def close_context(self): @@ -128,11 +129,18 @@ class ProcessConfig: timeout: int = 30 simulation: bool = True 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 self.main_pub is None: + if hasattr(self.should_recv_callback, "trigger_msg_type"): + self.main_pub = self.should_recv_callback.trigger_msg_type + else: + self.main_pub = self.pubs[0] if len(self.pubs) > 0 else None + class ProcessContainer: def __init__(self, cfg: ProcessConfig): @@ -292,7 +300,9 @@ class ProcessContainer: trigger_empty_recv = any(m.which() == self.cfg.main_pub for m in self.msg_queue) # get output msgs from previous inputs + input('about to get output msgs, press enter to continue...') output_msgs = self.get_output_msgs(msg.logMonoTime) + input('got output msgs, press enter to continue...') for m in self.msg_queue: self.pm.send(m.which(), m.as_builder()) @@ -306,9 +316,11 @@ class ProcessContainer: camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) self.msg_queue = [] + input('about to unlock sockets, press enter to continue...') self.rc.unlock_sockets() if trigger_empty_recv: self.rc.unlock_sockets() + input('unlocked sockets, press enter to continue...') self.cnt += 1 assert self.process.proc.is_alive() @@ -472,6 +484,7 @@ CONFIGS = [ "driverMonitoringState", "onroadEvents", "driverAssistance"], subs=["carControl", "controlsState"], ignore=["logMonoTime", ], + main_pub="selfdriveState", init_callback=get_car_params_callback, should_recv_callback=MessageBasedRcvCallback("selfdriveState"), tolerance=NUMPY_TOLERANCE, @@ -486,6 +499,7 @@ CONFIGS = [ tolerance=NUMPY_TOLERANCE, processing_time=0.004, main_pub="can", + main_pub_drained=True, ), ProcessConfig( proc_name="radard", @@ -536,6 +550,7 @@ CONFIGS = [ pubs=["livePose", "liveCalibration", "carState"], subs=["liveParameters"], ignore=["logMonoTime"], + main_pub="livePose", init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("livePose"), tolerance=NUMPY_TOLERANCE, @@ -574,7 +589,6 @@ CONFIGS = [ 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, @@ -588,7 +602,6 @@ CONFIGS = [ 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"], ),