Pause vision inference while parked

Co-Authored-By: Prabhaav Pillai <143428353+prabhaavp@users.noreply.github.com>

starpilot/system/adj_spot_monitor_vision.py

@@ -52,6 +52,7 @@ class VASMDaemon:
 
     self.followup_until = 0.0
     self.onroad_prev = False
+    self.parked_prev = False
 
     self._last_pub_left = False
     self._last_pub_right = False
@@ -179,12 +180,15 @@ class VASMDaemon:
         onroad = self.sm["deviceState"].started if self.sm.valid.get("deviceState", False) else False
         parked = self.sm["starpilotCarState"].isParked if self.sm.valid.get("starpilotCarState", False) else False
 
-        if not onroad or not self._enabled or not self.inference.valid or not self._annotation_loaded:
-          self._update_inactive(reset_inference=(onroad != self.onroad_prev))
-          if onroad != self.onroad_prev:
+        if not onroad or parked or not self._enabled or not self.inference.valid or not self._annotation_loaded:
+          state_changed = onroad != self.onroad_prev or parked != self.parked_prev
+          self._update_inactive(reset_inference=state_changed)
+          if state_changed:
             self.last_inference_at = 0.0
+            self.followup_until = 0.0
           self.onroad_prev = onroad
-          if now - self._last_status_log >= STATUS_LOG_INTERVAL and not onroad:
+          self.parked_prev = parked
+          if now - self._last_status_log >= STATUS_LOG_INTERVAL and (not onroad or parked):
             cpu = list(self.sm["deviceState"].cpuUsagePercent) if self.sm.valid.get("deviceState", False) else []
             cpu_str = f"avg={sum(cpu)/len(cpu):.0f}% cores={','.join(f'{c:.0f}' for c in cpu)}" if cpu else "?"
             status = f"[VASM] idle | {cpu_str} | onroad={onroad} parked={parked}"
@@ -195,6 +199,7 @@ class VASMDaemon:
           continue
 
         self.onroad_prev = onroad
+        self.parked_prev = parked
 
         if not self._connect_camera():
           self._update_inactive(reset_inference=True)

starpilot/system/speed_limit_vision.py

@@ -320,7 +320,7 @@ class SpeedLimitVisionDaemon:
       self.Ratekeeper = Ratekeeper
       self.VisionIpcClient = VisionIpcClient
       self.VisionStreamType = VisionStreamType
-      self.sm = messaging.SubMaster(["deviceState", "mapdOut", "userBookmark", "livePose"])
+      self.sm = messaging.SubMaster(["deviceState", "mapdOut", "userBookmark", "livePose", "starpilotCarState"])
 
     self.client = None
     self.stream_name = ""
@@ -344,6 +344,7 @@ class SpeedLimitVisionDaemon:
     self.track_start_count = 0
     self.max_track_proposal_confidence = 0.0
     self.started_prev = False
+    self.parked_prev = False
 
     self.history: deque[HistoryEntry] = deque()
     self.published_speed_limit_mph = 0
@@ -2316,6 +2317,16 @@ class SpeedLimitVisionDaemon:
       self.params_memory.remove("VisionSpeedLimitSupportCount")
       self.params_memory.remove("VisionSpeedLimitSupportSpeed")
 
+  def _enter_parked(self, now):
+    self.current_frame_bgr = None
+    self.latest_detector_proposal = None
+    self._clear_proposal_track()
+    self.pending_auto_bookmark = None
+    self.pending_training_capture = None
+    self.followup_until = 0.0
+    self._publish_status("Idle - parked", clear_speed=False)
+    self._publish_runtime_telemetry(now, "parked", force=True)
+
   def _publish_detection_support(self, speed_limit_mph, support_count):
     if self.params_memory is None:
       return
@@ -2563,6 +2574,7 @@ class SpeedLimitVisionDaemon:
           self._disconnect_camera()
         self.last_road_name = ""
         self.started_prev = False
+        self.parked_prev = False
         self.current_frame_bgr = None
         self.pending_auto_bookmark = None
         self._publish_status("Idle - offroad", clear_speed=True)
@@ -2582,6 +2594,20 @@ class SpeedLimitVisionDaemon:
         self._write_debug_event("session_recovered", reason="missing_debug_session_while_onroad")
         self._publish_runtime_telemetry(now, "session_recovered", force=True)
 
+      parked = self.sm["starpilotCarState"].isParked if self.sm.valid.get("starpilotCarState", False) else False
+      if parked:
+        if not self.parked_prev:
+          self._enter_parked(now)
+        else:
+          self._publish_runtime_telemetry(now, "parked")
+        self.parked_prev = True
+        ratekeeper.keep_time()
+        continue
+      if self.parked_prev:
+        self.parked_prev = False
+        self.last_inference_at = -float("inf")
+        self._publish_runtime_telemetry(now, "parked_exit", force=True)
+
       road_name = self.sm["mapdOut"].roadName
       if self.last_road_name and road_name and road_name != self.last_road_name:
         self._write_debug_event("road_change", previousRoadName=self.last_road_name, roadName=road_name)

starpilot/system/tests/test_speed_limit_vision.py

@@ -111,6 +111,33 @@ def test_vasm_coexistence_mode_is_conditional(monkeypatch):
   assert daemon.detector_classifier_expansions == slv.DETECTOR_CLASSIFIER_EXPANSIONS
 
 
+def test_enter_parked_preserves_published_limit_and_clears_transient_work():
+  daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon)
+  daemon.current_frame_bgr = np.ones((2, 2, 3), dtype=np.uint8)
+  daemon.latest_detector_proposal = object()
+  daemon.proposal_track = object()
+  daemon.pending_auto_bookmark = object()
+  daemon.pending_training_capture = object()
+  daemon.followup_until = 100.0
+  daemon.published_speed_limit_mph = 55
+  published = []
+  telemetry = []
+  daemon._publish_status = lambda status, clear_speed=False: published.append((status, clear_speed))
+  daemon._publish_runtime_telemetry = lambda now, phase, force=False: telemetry.append((now, phase, force))
+
+  daemon._enter_parked(10.0)
+
+  assert daemon.current_frame_bgr is None
+  assert daemon.latest_detector_proposal is None
+  assert daemon.proposal_track is None
+  assert daemon.pending_auto_bookmark is None
+  assert daemon.pending_training_capture is None
+  assert daemon.followup_until == 0.0
+  assert daemon.published_speed_limit_mph == 55
+  assert published == [("Idle - parked", False)]
+  assert telemetry == [(10.0, "parked", True)]
+
+
 def test_receive_frame_does_not_retain_vision_buffer(monkeypatch):
   buffer_refs = []
 

starpilot/system/the_galaxy/tests/test_vasm.py

@@ -1,6 +1,6 @@
 import pytest
 
-from openpilot.starpilot.system.the_galaxy.the_galaxy import _normalize_vasm_config
+from openpilot.starpilot.system.the_galaxy.the_galaxy import _decode_json_object, _normalize_vasm_config
 
 
 def test_normalize_vasm_config_accepts_bounded_polygons():
@@ -28,3 +28,21 @@ def test_normalize_vasm_config_accepts_bounded_polygons():
 def test_normalize_vasm_config_rejects_unsafe_config(config):
   with pytest.raises(ValueError):
     _normalize_vasm_config(config)
+
+
+@pytest.mark.parametrize("raw", (
+  '{"width":1928,"height":1208,"poly_left":[],"poly_right":[]}',
+  b'{"width":1928,"height":1208,"poly_left":[],"poly_right":[]}',
+))
+def test_decode_vasm_config_from_legacy_params_payload(raw):
+  assert _decode_json_object(raw) == {
+    "width": 1928,
+    "height": 1208,
+    "poly_left": [],
+    "poly_right": [],
+  }
+
+
+@pytest.mark.parametrize("raw", (None, "", "invalid", [], 1))
+def test_decode_vasm_config_rejects_non_objects(raw):
+  assert _decode_json_object(raw) == {}

starpilot/system/the_galaxy/the_galaxy.py

@@ -2596,6 +2596,18 @@ def _normalize_vasm_config(data):
     raise ValueError("At least one window polygon is required.")
   return config
 
+
+def _decode_json_object(value):
+  if isinstance(value, bytes):
+    value = value.decode("utf-8", errors="replace")
+  if isinstance(value, str):
+    try:
+      value = json.loads(value)
+    except json.JSONDecodeError:
+      return {}
+  return value if isinstance(value, dict) else {}
+
+
 def _is_blank_param_raw(raw_value):
   if raw_value is None:
     return True
@@ -7462,8 +7474,7 @@ def setup(app):
 
   @app.route("/api/v_asm/config", methods=["GET"])
   def v_asm_get_config():
-    config = params.get("VASMAnnotationConfig")
-    return jsonify(config if isinstance(config, dict) else {})
+    return jsonify(_decode_json_object(params.get("VASMAnnotationConfig")))
 
   @app.route("/api/v_asm/config", methods=["POST"])
   def v_asm_save_config():