SubMaster: poll a single service (#585)

* SubMaster: poll a single service

* smarter min/max

* cleanup

---------

Co-authored-by: Comma Device <device@comma.ai>

messaging/__init__.py

@@ -154,9 +154,9 @@ def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
 
 
 class SubMaster:
-  def __init__(self, services: List[str], poll: Optional[List[str]] = None,
+  def __init__(self, services: List[str], poll: Optional[str] = None,
                ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
-               ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", freq: Optional[float] = None):
+               ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None):
     self.frame = -1
     self.updated = {s: False for s in services}
     self.recv_time = {s: 0. for s in services}
@@ -169,8 +169,11 @@ class SubMaster:
     self.valid = {}
     self.logMonoTime = {}
 
+    self.max_freq = {}
+    self.min_freq = {}
+
     self.poller = Poller()
-    polled_services = set(poll if poll is not None and len(poll) else services)
+    polled_services = set([poll, ] if poll is not None else services)
     self.non_polled_services = set(services) - polled_services
 
     self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
@@ -180,7 +183,9 @@ class SubMaster:
       self.ignore_alive = services
       self.ignore_average_freq = services
 
-    self.update_freq = freq or min([SERVICE_LIST[s].frequency for s in polled_services])
+    # if freq and poll aren't specified, assume the max to be conservative
+    assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used."
+    self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services])
 
     for s in services:
       p = self.poller if s not in self.non_polled_services else None
@@ -196,6 +201,19 @@ class SubMaster:
       self.valid[s] = True  # FIXME: this should default to False
 
       freq = max(min([SERVICE_LIST[s].frequency, self.update_freq]), 1.)
+      if s == poll:
+        max_freq = freq
+        min_freq = freq
+      else:
+        max_freq = min(freq, self.update_freq)
+        if SERVICE_LIST[s].frequency >= 2*self.update_freq:
+          min_freq = self.update_freq
+        elif self.update_freq >= 2*SERVICE_LIST[s].frequency:
+          min_freq = freq
+        else:
+          min_freq = min(freq, freq / 2.)
+      self.max_freq[s] = max_freq*1.2
+      self.min_freq[s] = min_freq*0.8
       self.recv_dts[s] = deque(maxlen=int(5*freq))
 
   def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
@@ -224,7 +242,6 @@ class SubMaster:
       s = msg.which()
       self.updated[s] = True
 
-
       if self.recv_time[s] > 1e-5:
         self.recv_dts[s].append(cur_time - self.recv_time[s])
       self.recv_time[s] = cur_time
@@ -244,7 +261,7 @@ class SubMaster:
         except ZeroDivisionError:
           avg_freq = 0
         expected_freq = min(SERVICE_LIST[s].frequency, self.update_freq)
-        self.freq_ok[s] = (len(self.recv_dts[s]) >= 2*expected_freq) and (avg_freq > expected_freq*0.8) and (avg_freq < expected_freq*1.2)
+        self.freq_ok[s] = (len(self.recv_dts[s]) >= 2*expected_freq) and (avg_freq > self.min_freq[s]) and (avg_freq < self.max_freq[s])
       else:
         self.freq_ok[s] = True
         self.alive[s] = True

messaging/tests/test_pub_sub_master.py

@@ -74,6 +74,28 @@ class TestSubMaster(unittest.TestCase):
       self.assertLess(t, 5)
       self.assertFalse(any(sm.updated.values()))
 
+  def test_avg_frequency_checks(self):
+    for poll in (True, False):
+      sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"],
+                               poll=("modelV2" if poll else None),
+                               frequency=(20. if not poll else None))
+
+      checks = {
+        "carState": (20, 20),
+        "modelV2": (20, 20 if poll else 10),
+        "cameraOdometry": (20, 10),
+        "liveCalibration": (4, 4),
+        "carParams": (None, None),
+      }
+
+      for service, (max_freq, min_freq) in checks.items():
+        if max_freq is not None:
+          assert sm._check_avg_freq(service)
+          assert sm.max_freq[service] == max_freq*1.2
+          assert sm.min_freq[service] == min_freq*0.8
+        else:
+          assert not sm._check_avg_freq(service)
+
   def test_alive(self):
     pass