Restore Dom model publication guard after dropped camera frames

openpilot/selfdrive/modeld/modeld.py

@@ -592,7 +592,7 @@ def main(demo=False):
         receipt_owner.loaded(small_prepared, ModelVariant.SMALL, model_id=small_model.model_id)
       cloudlog.event("jetlinkHandover", state=HOOKS.state(model), handovers=model.handovers)
 
-    if model_output is not None:
+    if model_output is not None and vipc_dropped_frames == 0:
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')

openpilot/starpilot/flm/producer_clock_equivalence.py

@@ -15,7 +15,7 @@ PRODUCER_SOURCE_HASHES = (
   ('openpilot', 'system/camerad/cameras/spectra.cc', '4fa26c17f0fb792a4ab6815657beccddee1d641faa2fd02e5aa7393f717a2f3a'),
   ('openpilot', 'system/camerad/cameras/camera_qcom2.cc', 'b34c24c9984a0ac086f7b961cb7ff09d0501231da50aaad0e676bb3dc3f2edd9'),
   ('openpilot', 'system/camerad/cameras/camera_common.cc', 'bd9a416a6ee21047b4deec6df04276d8717cd92fe14de43f3e9daff86b62bb8f'),
-  ('openpilot', 'selfdrive/modeld/modeld.py', 'ed30b4380d60924f830a9ec1c6d1eebfa833eed9324120a0c553dc748a1a8130'),
+  ('openpilot', 'selfdrive/modeld/modeld.py', 'cfdbe868719e32afbeaad8b02677c303df7eb490361f9575c5a208b6370703f8'),
   ('openpilot', 'selfdrive/modeld/fill_model_msg.py', 'b3916c34d1073d0c26a7a8adc51e7b148f63a1ac68b388226170fd0f8b8b56b4'),
   ('openpilot', 'system/sensord/sensord.py', '7c79ab07d775622a43001e5a9b9ccc9a7bafb065940185849b33eefc2fd26420'),
   ('openpilot', 'system/sensord/sensors/lsm6ds3_gyro.py', '3e561f3d8572e1477c9b4b810c7570837d51e81c5672fe21ec67a68cae00dd92'),

openpilot/starpilot/flm/tests/test_upstream_producer_transport.py

@@ -8,7 +8,7 @@ from openpilot.starpilot.flm import producer_clock_equivalence as clock
 
 
 APPROVED = {
-  'selfdrive/modeld/modeld.py': 'ed30b4380d60924f830a9ec1c6d1eebfa833eed9324120a0c553dc748a1a8130',
+  'selfdrive/modeld/modeld.py': 'cfdbe868719e32afbeaad8b02677c303df7eb490361f9575c5a208b6370703f8',
   'visionipc/visionipc_server.cc': 'a4325a5171eacb931eb271d06a992d2f27216040df113d92099f62cdfa67d81a',
   'visionipc/visionipc_client.cc': 'd142cd60eedabfc581e9770fcd6f0ca81b762ba9dbc608b54dd20a27a317a52d',
 }

openpilot/starpilot/tests/test_modeld_publication.py

@@ -0,0 +1,99 @@
+"""Exercise modeld's real publication statements without loading a GPU model."""
+import ast
+import copy
+import unittest
+from pathlib import Path
+from types import SimpleNamespace
+
+ROOT = Path(__file__).resolve().parents[2]
+
+
+def publication_program():
+  tree = ast.parse((ROOT / "selfdrive/modeld/modeld.py").read_text())
+  def sends(node):
+    return isinstance(node, ast.Expr) and isinstance(node.value, ast.Call) and isinstance(node.value.func, ast.Attribute) and ast.unparse(node.value.func) == "pm.send"
+  def slice_statements(statements):
+    result = []
+    for statement in statements:
+      if sends(statement):
+        result.append(copy.deepcopy(statement))
+      elif isinstance(statement, ast.If):
+        body = slice_statements(statement.body)
+        other = slice_statements(statement.orelse)
+        if body or other:
+          result.append(ast.If(test=copy.deepcopy(statement.test), body=body or [ast.Pass()], orelse=other))
+    return result
+  main = next(node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name == "main")
+  loop = next(node for node in ast.walk(main) if isinstance(node, ast.While) and any(
+    isinstance(child, ast.Assign) and ast.unparse(child.targets[0]) == "vipc_dropped_frames" for child in node.body))
+  count = next(node for node in loop.body if isinstance(node, ast.Assign) and ast.unparse(node.targets[0]) == "vipc_dropped_frames")
+  advance = next(node for node in loop.body if isinstance(node, ast.Assign) and ast.unparse(node.targets[0]) == "last_vipc_frame_id")
+  program = ast.Module(body=[copy.deepcopy(count), *slice_statements(loop.body), copy.deepcopy(advance)], type_ignores=[])
+  services = [node.value.args[0].value for node in ast.walk(program) if sends(node)]
+  assert sorted(services) == sorted(["modelV2", "drivingModelData", "cameraOdometry", "laneChangeAssistWire"])
+  return compile(ast.fix_missing_locations(program), "<actual modeld publication slice>", "exec")
+
+
+class TestModeldPublication(unittest.TestCase):
+  def replay(self, frames, *, model_output=True, lane_status=True):
+    emitted = []
+    namespace = {"max": max, "last_vipc_frame_id": frames[0] - 1, "model_output": object() if model_output else None,
+                 "pm": SimpleNamespace(send=lambda service, payload: emitted.append((service, payload)))}
+    program = publication_program()
+    for frame in frames:
+      namespace["meta_main"] = SimpleNamespace(frame_id=frame)
+      for name in ("modelv2_send", "drivingdata_send", "posenet_send", "lane_status_msg"):
+        namespace[name] = (frame, frame * 50_000_000) if name != "lane_status_msg" or lane_status else None
+      exec(program, namespace)  # noqa: S102 - execute the repository publication AST, without GPU imports.
+      self.assertEqual(namespace["last_vipc_frame_id"], frame)
+    return emitted
+
+  def test_recorded_route15_frame_gap_recovers_on_next_frame(self):
+    # Original dense route: frame 1900 was skipped; invalid odometry 1901 preceded inputsOK=false.
+    emitted = self.replay([1899, 1901, 1902])
+    self.assertEqual(len(emitted), 8)
+    self.assertEqual({payload[0] for _, payload in emitted}, {1899, 1902})
+    self.assertTrue(all(stamp == frame * 50_000_000 for _, (frame, stamp) in emitted))
+
+  def test_contiguous_frames_keep_all_publications(self):
+    self.assertEqual(len(self.replay([1899, 1900, 1901])), 12)
+
+  def test_continuous_drops_do_not_fabricate_fresh_messages(self):
+    emitted = self.replay([1899, 1901, 1903, 1905])
+    self.assertEqual(len(emitted), 4)
+    self.assertEqual({payload[0] for _, payload in emitted}, {1899})
+
+  def test_missing_model_output_never_publishes(self):
+    self.assertEqual(self.replay([1899, 1900, 1902], model_output=False), [])
+
+  def test_optional_lane_status_stays_optional(self):
+    emitted = self.replay([1899, 1901, 1902], lane_status=False)
+    self.assertEqual(len(emitted), 6)
+    self.assertNotIn("laneChangeAssistWire", {service for service, _ in emitted})
+
+  def test_pose_validity_still_rejects_missing_calibration_and_drops(self):
+    tree = ast.parse((ROOT / "selfdrive/modeld/fill_model_msg.py").read_text())
+    function = next(node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name == "fill_pose_msg")
+    function.returns = None
+    for argument in function.args.args:
+      argument.annotation = None
+    namespace = {}
+    exec(compile(ast.fix_missing_locations(ast.Module(body=[function], type_ignores=[])), "<actual fill_pose_msg>", "exec"), namespace)  # noqa: S102 - execute the repository producer function.
+    class Array:
+      def __getitem__(self, key):
+        return self
+      def tolist(self):
+        return [1., 2., 3.]
+    output = {name: Array() for name in ("pose", "wide_from_device_euler", "road_transform", "pose_stds", "wide_from_device_euler_stds", "road_transform_stds")}
+    for calibration in (False, True):
+      for drops in (0, 1, 3):
+        with self.subTest(calibration=calibration, drops=drops):
+          message = SimpleNamespace(cameraOdometry=SimpleNamespace())
+          namespace["fill_pose_msg"](message, output, 1901, drops, 3958001402699, calibration)
+          self.assertEqual(bool(message.valid), calibration and drops == 0)
+          self.assertEqual(message.cameraOdometry.frameId, 1901)
+          self.assertEqual(message.cameraOdometry.timestampEof, 3958001402699)
+
+
+if __name__ == "__main__":
+  unittest.main()

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "abac4853cce6685bbfbd9e7396fc3ccf10cfc569d5e9c88b92d565093b02f50d", "schema_version": 1}
+{"protected_inventory_sha256": "16c01ae299e401184af7d8715c5d7da2cd0834d761ec773b59e516494fa0f2d5", "schema_version": 1}