Sentry and Tunes

system/camerad/snapshot.py

@@ -73,7 +73,7 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
   return rear, front
 
 
-def snapshot():
+def snapshot(allow_existing=False):
   params = Params()
 
   if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
@@ -86,25 +86,29 @@ def snapshot():
   time.sleep(2.0)  # Give hardwared time to read the param, or if just started give camerad time to start
 
   # Check if camerad is already started
+  camerad_already_running = False
   try:
     subprocess.check_call(["pgrep", "camerad"])
-    print("Camerad already running")
-    params.put_bool("IsTakingSnapshot", False)
-    params.remove("Offroad_IsTakingSnapshot")
-    return None, None
+    camerad_already_running = True
+    if not allow_existing:
+      print("Camerad already running")
+      params.put_bool("IsTakingSnapshot", False)
+      params.remove("Offroad_IsTakingSnapshot")
+      return None, None
   except subprocess.CalledProcessError:
     pass
 
   try:
     # Allow testing on replay on PC
-    if not PC:
+    if not PC and not camerad_already_running:
       managed_processes['camerad'].start()
 
     frame = "wideRoadCameraState"
     front_frame = "driverCameraState" if front_camera_allowed else None
     rear, front = get_snapshots(frame, front_frame)
   finally:
-    managed_processes['camerad'].stop()
+    if not camerad_already_running:
+      managed_processes['camerad'].stop()
     params.put_bool("IsTakingSnapshot", False)
     set_offroad_alert("Offroad_IsTakingSnapshot", False)
 

system/manager/process_config.py

@@ -62,6 +62,15 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams, starpilot_togg
 def only_offroad(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
   return not started
 
+def sentry_mode(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
+  return not started and params.get_bool("SentryModeEnabled")
+
+def sensord_run(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
+  return started or params.get_bool("SentryModeEnabled")
+
+def camera_run(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
+  return driverview(started, params, CP, starpilot_toggles) or params.get_bool("SentryModeCapture")
+
 def livestream(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
   return params.get_bool("IsLiveStreaming")
 
@@ -182,7 +191,7 @@ procs = [
   NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], or_(and_(livestream, not_(iscar)), notcar)),
   PythonProcess("logmessaged", "system.logmessaged", always_run),
 
-  NativeProcess("camerad", "system/camerad", ["./camerad"], or_(driverview, livestream), enabled=not WEBCAM),
+  NativeProcess("camerad", "system/camerad", ["./camerad"], or_(camera_run, livestream), enabled=not WEBCAM),
   PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM),
   PythonProcess("proclogd", "system.proclogd", and_(allow_logging, only_onroad), enabled=platform.system() != "Darwin"),
   PythonProcess("journald", "system.journald", and_(allow_logging, only_onroad), platform.system() != "Darwin"),
@@ -192,7 +201,8 @@ procs = [
   PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad),
   PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)),
 
-  PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
+  PythonProcess("sensord", "system.sensord.sensord", sensord_run, enabled=not PC),
+  PythonProcess("sentryd", "system.sentryd.sentryd", sentry_mode, enabled=not PC),
   PythonProcess("soundd", "selfdrive.ui.soundd", driverview),
   PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
   NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),

system/sentryd/__init__.py

@@ -0,0 +1 @@
+"""Offroad sentry-mode daemon."""

system/sentryd/detector.py

@@ -0,0 +1,83 @@
+from __future__ import annotations
+
+import math
+import time
+from collections.abc import Callable, Sequence
+
+
+class MotionDetector:
+  """Detect sustained changes in the accelerometer magnitude.
+
+  The detector deliberately returns edge events instead of owning any I/O. That
+  keeps the movement policy testable and lets the daemon decide how to capture
+  frames or notify Galaxy.
+  """
+
+  def __init__(
+    self,
+    *,
+    sensitivity: float = 0.04,
+    warning_trigger_count: int = 10,
+    alarm_trigger_count: int = 25,
+    alarm_time: float = 30.0,
+    reset_time: float = 60.0,
+    clock: Callable[[], float] = time.monotonic,
+  ):
+    self.sensitivity = sensitivity
+    self.warning_trigger_count = warning_trigger_count
+    self.alarm_trigger_count = alarm_trigger_count
+    self.alarm_time = alarm_time
+    self.reset_time = reset_time
+    self.clock = clock
+    self.previous_acceleration: tuple[float, float, float] | None = None
+    self.trigger_count = 0
+    self.trigger_started_at: float | None = None
+    self.alarm_triggered = False
+
+  @staticmethod
+  def _magnitude(acceleration: Sequence[float]) -> float:
+    if len(acceleration) < 3:
+      raise ValueError("accelerometer samples must contain x, y, and z")
+    return math.sqrt(sum(float(component) ** 2 for component in acceleration[:3]))
+
+  def reset(self) -> None:
+    self.previous_acceleration = None
+    self.trigger_count = 0
+    self.trigger_started_at = None
+    self.alarm_triggered = False
+
+  def update(self, acceleration: Sequence[float], now: float | None = None) -> str | None:
+    now = self.clock() if now is None else now
+    current = tuple(float(component) for component in acceleration[:3])
+    if len(current) < 3:
+      raise ValueError("accelerometer samples must contain x, y, and z")
+
+    if self.previous_acceleration is None:
+      self.previous_acceleration = current
+      return None
+
+    delta = abs(self._magnitude(current) - self._magnitude(self.previous_acceleration))
+    self.previous_acceleration = current
+
+    if delta > self.sensitivity:
+      self.trigger_count += 1
+      if self.trigger_started_at is None:
+        self.trigger_started_at = now
+
+      if self.trigger_count == self.warning_trigger_count:
+        return "warning"
+
+      if (
+        self.trigger_count > self.alarm_trigger_count
+        and now - self.trigger_started_at >= self.alarm_time
+        and not self.alarm_triggered
+      ):
+        self.alarm_triggered = True
+        return "alarm"
+
+    if self.trigger_started_at is not None and now - self.trigger_started_at >= self.reset_time:
+      self.trigger_count = 0
+      self.trigger_started_at = None
+      self.alarm_triggered = False
+
+    return None

system/sentryd/sentryd.py

@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+from datetime import datetime, timezone
+import json
+import os
+import threading
+import time
+from pathlib import Path
+from uuid import uuid4
+
+import cereal.messaging as messaging
+import requests
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.system.camerad.snapshot import jpeg_write, snapshot
+from openpilot.system.hardware import PC
+from openpilot.system.hardware.hw import Paths
+
+from openpilot.system.sentryd.detector import MotionDetector
+
+
+ARM_DELAY_SECONDS = 90.0
+LOOP_INTERVAL_SECONDS = 0.1
+SENSITIVITY = 0.04
+WARNING_TRIGGER_COUNT = 10
+ALARM_TRIGGER_COUNT = 25
+ALARM_TIME_SECONDS = 30.0
+RESET_TIME_SECONDS = 60.0
+MAX_EVENT_DIRECTORIES = 100
+
+
+def event_root() -> Path:
+  if PC:
+    return Path(Paths.comma_home()) / "starpilot" / "data" / "sentryd"
+  return Path("/data/media/0/sentryd")
+
+
+def galaxy_event_url() -> str:
+  default_port = "8083" if PC else "8082"
+  port = os.environ.get("SP_GALAXY_PORT", default_port)
+  return f"http://127.0.0.1:{port}/api/sentry/events"
+
+
+def _utc_now() -> str:
+  return datetime.now(timezone.utc).isoformat()
+
+
+class SentryMode:
+  def __init__(self, params: Params | None = None, sm=None, clock=time.monotonic):
+    self.params = params or Params(return_defaults=True)
+    self.sm = sm or messaging.SubMaster(["accelerometer"])
+    self.clock = clock
+    self.detector = MotionDetector(
+      sensitivity=SENSITIVITY,
+      warning_trigger_count=WARNING_TRIGGER_COUNT,
+      alarm_trigger_count=ALARM_TRIGGER_COUNT,
+      alarm_time=ALARM_TIME_SECONDS,
+      reset_time=RESET_TIME_SECONDS,
+      clock=clock,
+    )
+    self.started_at = clock()
+    self.armed = False
+    self._last_status = None
+
+  def _write_status(self, state: str, **extra) -> None:
+    status_values = {"state": state, **extra}
+    if status_values == self._last_status:
+      return
+    status = {**status_values, "updatedAt": _utc_now()}
+    try:
+      self.params.put("SentryModeStatus", json.dumps(status, separators=(",", ":")))
+    except Exception:
+      cloudlog.exception("sentryd: failed to write status")
+    self._last_status = status_values
+
+  def _capture_images(self, event_id: str) -> list[str]:
+    self.params.put_bool("SentryModeCapture", True)
+    try:
+      rear, front = snapshot(allow_existing=True)
+    except Exception:
+      cloudlog.exception("sentryd: snapshot failed")
+      return []
+    finally:
+      self.params.put_bool("SentryModeCapture", False)
+
+    if rear is None and front is None:
+      return []
+
+    directory = event_root() / event_id
+    directory.mkdir(parents=True, exist_ok=True)
+    paths = []
+    if rear is not None:
+      rear_path = directory / "wide.jpg"
+      jpeg_write(str(rear_path), rear)
+      paths.append(str(rear_path))
+    if front is not None:
+      front_path = directory / "driver.jpg"
+      jpeg_write(str(front_path), front)
+      paths.append(str(front_path))
+    return paths
+
+  def _trim_old_events(self) -> None:
+    root = event_root()
+    if not root.exists():
+      return
+    try:
+      directories = sorted((path for path in root.iterdir() if path.is_dir()), key=lambda path: path.stat().st_mtime)
+      for directory in directories[:-MAX_EVENT_DIRECTORIES]:
+        for child in directory.iterdir():
+          child.unlink(missing_ok=True)
+        directory.rmdir()
+    except OSError:
+      cloudlog.exception("sentryd: failed to trim old events")
+
+  def _publish_event(self, event: dict) -> None:
+    self.params.put("SentryModeLastEvent", json.dumps(event, separators=(",", ":")))
+    self._write_status(event["kind"], eventId=event["eventId"])
+
+    def publish():
+      for attempt in range(3):
+        try:
+          response = requests.post(galaxy_event_url(), json=event, timeout=3)
+          response.raise_for_status()
+          return
+        except requests.RequestException as error:
+          if attempt == 2:
+            cloudlog.warning(f"sentryd: Galaxy notification unavailable: {error}")
+          else:
+            time.sleep(1.0)
+
+    threading.Thread(target=publish, name="sentryd-galaxy-publish", daemon=True).start()
+
+  def _handle_detection(self, kind: str) -> None:
+    event_id = f"{int(time.time())}-{uuid4().hex[:8]}"
+    event = {
+      "eventId": event_id,
+      "kind": kind,
+      "detectedAt": _utc_now(),
+      "imagePaths": [],
+      "message": "Movement detected while parked." if kind == "warning" else "Sustained movement detected while parked.",
+    }
+    if kind == "alarm":
+      event["imagePaths"] = self._capture_images(event_id)
+      self._trim_old_events()
+    self._publish_event(event)
+
+  def update(self) -> None:
+    now = self.clock()
+    if now - self.started_at < ARM_DELAY_SECONDS:
+      self._write_status("arming", secondsRemaining=max(0, int(ARM_DELAY_SECONDS - (now - self.started_at))))
+      return
+
+    if not self.armed:
+      self.armed = True
+      self._write_status("armed")
+
+    message = self.sm["accelerometer"]
+    if message is None or message.acceleration is None:
+      self._write_status("sensor_unavailable")
+      return
+
+    try:
+      detection = self.detector.update(message.acceleration.v, now=now)
+    except (TypeError, ValueError):
+      self._write_status("sensor_unavailable")
+      return
+
+    if detection is not None:
+      self._handle_detection(detection)
+
+  def run(self) -> None:
+    self._write_status("starting")
+    while self.params.get_bool("SentryModeEnabled"):
+      self.sm.update(0)
+      self.update()
+      time.sleep(LOOP_INTERVAL_SECONDS)
+    self._write_status("disabled")
+
+
+def main() -> None:
+  SentryMode().run()
+
+
+if __name__ == "__main__":
+  main()

system/sentryd/test/test_detector.py

@@ -0,0 +1,46 @@
+from openpilot.system.sentryd.detector import MotionDetector
+
+
+def test_motion_detector_ignores_small_changes():
+  detector = MotionDetector(sensitivity=0.1)
+  assert detector.update((0.0, 0.0, 9.8), now=0.0) is None
+  assert detector.update((0.0, 0.0, 9.85), now=0.1) is None
+  assert detector.trigger_count == 0
+
+
+def test_motion_detector_warns_once_after_sustained_motion():
+  detector = MotionDetector(sensitivity=0.1, warning_trigger_count=3)
+  detector.update((0.0, 0.0, 9.8), now=0.0)
+
+  assert detector.update((0.0, 0.0, 10.0), now=0.1) is None
+  assert detector.update((0.0, 0.0, 9.8), now=0.2) is None
+  assert detector.update((0.0, 0.0, 10.0), now=0.3) == "warning"
+  assert detector.update((0.0, 0.0, 9.8), now=0.4) is None
+
+
+def test_motion_detector_alarms_after_time_threshold():
+  detector = MotionDetector(
+    sensitivity=0.1,
+    warning_trigger_count=2,
+    alarm_trigger_count=3,
+    alarm_time=1.0,
+  )
+  detector.update((0.0, 0.0, 9.8), now=0.0)
+  detector.update((0.0, 0.0, 10.0), now=0.1)
+  assert detector.update((0.0, 0.0, 9.8), now=0.2) == "warning"
+
+  assert detector.update((0.0, 0.0, 10.0), now=0.5) is None
+  assert detector.update((0.0, 0.0, 9.8), now=1.0) is None
+  assert detector.update((0.0, 0.0, 10.0), now=1.1) == "alarm"
+  assert detector.update((0.0, 0.0, 9.8), now=1.2) is None
+
+
+def test_motion_detector_resets_after_quiet_period():
+  detector = MotionDetector(sensitivity=0.1, warning_trigger_count=2, reset_time=1.0)
+  detector.update((0.0, 0.0, 9.8), now=0.0)
+  detector.update((0.0, 0.0, 10.0), now=0.1)
+  detector.update((0.0, 0.0, 9.8), now=0.2)
+  detector.update((0.0, 0.0, 9.8), now=1.3)
+
+  assert detector.trigger_count == 0
+  assert detector.trigger_started_at is None