Add a thermal guard that slows the robot before the Pi throttles

hermes_thermal reads the CPU temperature and throttle flags, publishes /health/*, and at
72 / 77 / 82 C scales the camera, face and detector rates down through their live
parameters, restoring them as it cools one level at a time with a margin and a dwell so it
does not flap. The face and detector gained live parameter callbacks for this, the bridge's
robot://status carries a health block (temperature, level, throttle flags now and since
boot), and a bringup test asserts the guard's normal rates equal the real ones.

Live testing on the robot (forcing 73 / 78 / 85 C and back) caught a bug the unit tests
missed: one logging line whose severity changed between warn and info raised in rclpy and
killed the guard on its first cooling step, and the respawn hid it. Fixed with separate call
sites; the tick now survives any error; a full heat-then-cool test drives the real node, and
a mutation check confirmed it fails on the original bug.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs

ros/hermes_thermal/hermes_thermal/policy.py

@@ -0,0 +1,39 @@
+"""When to shed load. No ROS imports.
+
+Four levels by temperature. Heating up is acted on at once; cooling is slow and one step at a time, only
+once clearly below the level's threshold (a margin) and after a dwell, so the load does not flap around a
+threshold. The Pi 4 starts throttling its CPU at about 80 C, so the levels act before that.
+"""
+from __future__ import annotations
+
+LEVELS = ('ok', 'warm', 'hot', 'critical')
+# Fraction of each node's normal rate at each level (never below the node's floor, never above normal).
+SCALE = {'ok': 1.0, 'warm': 0.7, 'hot': 0.45, 'critical': 0.2}
+
+
+def scaled(normal: float, level: str, floor: float) -> float:
+    """A node's rate at `level`: its normal rate scaled down, but not below `floor` (and never above normal)."""
+    return min(normal, max(floor, normal * SCALE[level]))
+
+
+class ThermalPolicy:
+    def __init__(self, thresholds=(72.0, 77.0, 82.0), margin_c: float = 3.0, dwell_s: float = 20.0):
+        self.thresholds = tuple(thresholds)          # entry temperatures of warm, hot, critical
+        self.margin_c, self.dwell_s = margin_c, dwell_s
+        self._level = 0
+        self._changed = -1e9
+
+    @property
+    def level(self) -> str:
+        return LEVELS[self._level]
+
+    def update(self, temp_c: float, now: float) -> str:
+        """The level for this temperature at time `now` (seconds, any monotonic clock)."""
+        asks = sum(temp_c >= t for t in self.thresholds)        # what the temperature alone asks for
+        if asks > self._level:                                  # heating: act at once, even several steps
+            self._level, self._changed = asks, now
+        elif asks < self._level and now - self._changed >= self.dwell_s:
+            if temp_c < self.thresholds[self._level - 1] - self.margin_c:   # clearly below: one step down
+                self._level -= 1
+                self._changed = now
+        return self.level

ros/hermes_thermal/hermes_thermal/sensors.py

@@ -0,0 +1,43 @@
+"""Reading the Pi's temperature and throttle flags. No ROS imports."""
+from __future__ import annotations
+
+import re
+import subprocess
+from typing import Callable, Optional
+
+THERMAL_ZONE = '/sys/class/thermal/thermal_zone0/temp'
+
+# vcgencmd get_throttled bits: "now" in the low bits, "has occurred since boot" 16 higher
+_NOW = {0: 'under_voltage', 1: 'arm_frequency_capped', 2: 'throttled', 3: 'soft_temperature_limit'}
+
+
+def read_temp_c(path: str = THERMAL_ZONE) -> Optional[float]:
+    """CPU temperature in Celsius, or None if it cannot be read."""
+    try:
+        with open(path) as f:
+            return int(f.read().strip()) / 1000.0
+    except (OSError, ValueError):
+        return None
+
+
+def parse_throttled(text: str) -> Optional[int]:
+    m = re.search(r'throttled=(0x[0-9a-fA-F]+)', text)
+    return int(m.group(1), 16) if m else None
+
+
+def read_throttled(run: Callable = subprocess.run) -> Optional[int]:
+    """The raw `vcgencmd get_throttled` flags, or None if vcgencmd is unavailable."""
+    try:
+        out = run(['vcgencmd', 'get_throttled'], capture_output=True, text=True, timeout=2)
+        return parse_throttled(out.stdout)
+    except (OSError, subprocess.SubprocessError):
+        return None
+
+
+def decode_throttled(flags: Optional[int]) -> dict:
+    """Named flags: what is happening now, and what has happened since boot."""
+    if flags is None:
+        return {'available': False}
+    return {'available': True,
+            'now': {name: bool(flags >> bit & 1) for bit, name in _NOW.items()},
+            'since_boot': {name: bool(flags >> (bit + 16) & 1) for bit, name in _NOW.items()}}

ros/hermes_thermal/hermes_thermal/thermal_node.py

@@ -0,0 +1,143 @@
+"""ROS node: watch the Pi's temperature and shed load before it throttles.
+
+Publishes:  /health/temperature (sensor_msgs/Temperature), /health/thermal_level (std_msgs/String: ok |
+            warm | hot | critical), /health/throttled (std_msgs/UInt32: vcgencmd get_throttled flags)
+Acts on:    the camera's FrameDurationLimits, the face's fps and the detector's rate_hz, set as live
+            parameters, scaled down by level (hermes_thermal.policy) and restored when it cools. The
+            normal values below must equal the ones in hermes_bringup's robot.yaml (a test checks it), so
+            restoring is exact and survives this node restarting.
+Test it without heating the Pi: `ros2 param set /hermes_thermal fake_temp_c 85.0` (-1 turns it off).
+"""
+import time
+
+import rclpy
+from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue
+from rcl_interfaces.srv import SetParameters
+from rclpy.node import Node
+from sensor_msgs.msg import Temperature
+from std_msgs.msg import String, UInt32
+
+from .policy import LEVELS, ThermalPolicy, scaled
+from .sensors import read_temp_c, read_throttled
+
+
+def _double(v):
+    return ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=float(v))
+
+
+def _frame_duration(fps):
+    d = int(round(1e6 / fps))
+    return ParameterValue(type=ParameterType.PARAMETER_INTEGER_ARRAY, integer_array_value=[d, d])
+
+
+class Target:
+    """One node whose rate is scaled: its name, parameter, normal rate, floor, and how to encode a rate."""
+
+    def __init__(self, node, name, param, normal, floor, encode):
+        self.name, self.param, self.normal, self.floor, self.encode = name, param, normal, floor, encode
+        self.applied = None                      # the level last applied successfully
+        self.pending = False
+        self.client = node.create_client(SetParameters, f'{name}/set_parameters')
+
+
+class ThermalNode(Node):
+    def __init__(self):
+        super().__init__('hermes_thermal')
+        d = lambda name, default: self.declare_parameter(name, default).value  # noqa: E731
+        self.poll_s = min(30.0, max(0.5, float(d('poll_s', 2.0))))
+        for name, default in (('warm_c', 72.0), ('hot_c', 77.0), ('critical_c', 82.0), ('margin_c', 3.0),
+                              ('dwell_s', 20.0), ('fake_temp_c', -1.0)):
+            self.declare_parameter(name, default)
+        self.act = bool(d('act', True))
+        self.policy = ThermalPolicy()
+        self.targets = [
+            Target(self, '/camera', 'FrameDurationLimits', float(d('camera_fps', 15.0)), float(d('camera_floor_fps', 2.0)), _frame_duration),
+            Target(self, '/hermes_face', 'fps', float(d('face_fps', 30.0)), float(d('face_floor_fps', 8.0)), _double),
+            Target(self, '/hermes_face_detector', 'rate_hz', float(d('detector_hz', 4.0)), float(d('detector_floor_hz', 1.0)), _double),
+        ]
+        self.temp_pub = self.create_publisher(Temperature, '/health/temperature', 5)
+        self.level_pub = self.create_publisher(String, '/health/thermal_level', 5)
+        self.throttle_pub = self.create_publisher(UInt32, '/health/throttled', 5)
+        self.create_timer(self.poll_s, self._tick)
+        self.get_logger().info(f'thermal guard up: warm {self.get_parameter("warm_c").value:g}, hot '
+                               f'{self.get_parameter("hot_c").value:g}, critical {self.get_parameter("critical_c").value:g} C')
+
+    def _p(self, name):
+        return float(self.get_parameter(name).value)
+
+    def _temperature(self):
+        fake = self._p('fake_temp_c')
+        return fake if fake >= 0 else read_temp_c()
+
+    def _tick(self) -> None:
+        """Never let an unexpected error kill the guard: it exists to protect the robot."""
+        try:
+            self._poll()
+        except Exception as exc:                          # noqa: BLE001
+            self.get_logger().error(f'thermal guard tick failed: {exc!r}', throttle_duration_sec=30.0)
+
+    def _poll(self) -> None:
+        temp = self._temperature()
+        if temp is None:
+            self.get_logger().warn('cannot read the CPU temperature', throttle_duration_sec=60.0)
+            return
+        self.policy.thresholds = (self._p('warm_c'), self._p('hot_c'), self._p('critical_c'))
+        self.policy.margin_c, self.policy.dwell_s = self._p('margin_c'), self._p('dwell_s')
+        before = self.policy.level
+        level = self.policy.update(temp, time.monotonic())
+        if level != before:
+            # rclpy needs each logging call site to keep one severity: a single line that warned when
+            # heating and informed when cooling crashed this node on the first cooling step.
+            if LEVELS.index(level) > LEVELS.index(before):
+                self.get_logger().warn(f'{temp:.1f} C: {before} -> {level}')
+            else:
+                self.get_logger().info(f'{temp:.1f} C: {before} -> {level}')
+        msg = Temperature(temperature=float(temp), variance=0.0)
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'cpu'
+        self.temp_pub.publish(msg)
+        self.level_pub.publish(String(data=level))
+        flags = read_throttled()
+        if flags is not None:
+            self.throttle_pub.publish(UInt32(data=flags))
+        if self.act:
+            for t in self.targets:
+                self._apply(t, level)
+
+    def _apply(self, t: Target, level: str) -> None:
+        if t.applied == level or t.pending or not t.client.service_is_ready():
+            return                                        # done, in flight, or the node is not up yet: retry next tick
+        rate = scaled(t.normal, level, t.floor)
+        t.pending = True
+        future = t.client.call_async(SetParameters.Request(parameters=[Parameter(name=t.param, value=t.encode(rate))]))
+
+        def done(fut, t=t, level=level, rate=rate):
+            t.pending = False
+            try:
+                ok = fut.result().results[0].successful
+            except Exception as exc:                      # noqa: BLE001 - the node may have gone away
+                self.get_logger().warn(f'{t.name}: {exc!r}', throttle_duration_sec=30.0)
+                return
+            if ok:
+                t.applied = level
+                self.get_logger().info(f'{t.name} {t.param}: {rate:g} ({level})')
+            else:
+                self.get_logger().warn(f'{t.name} refused {t.param}={rate:g}', throttle_duration_sec=30.0)
+        future.add_done_callback(done)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ThermalNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros/hermes_thermal/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>hermes_thermal</name>
+  <version>0.1.0</version>
+  <description>Watches the Pi's temperature and sheds load before it throttles.</description>
+  <maintainer email="noreply@example.com">K. Isom</maintainer>
+  <license>UNLICENSED</license>
+
+  <depend>rclpy</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros/hermes_thermal/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/hermes_thermal
+[install]
+install_scripts=$base/lib/hermes_thermal

ros/hermes_thermal/setup.py

@@ -0,0 +1,24 @@
+from setuptools import find_packages, setup
+
+package_name = 'hermes_thermal'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='K. Isom',
+    maintainer_email='noreply@example.com',
+    description='Thermal guard for the Hermes robot.',
+    license='UNLICENSED',
+    entry_points={
+        'console_scripts': [
+            'thermal_guard = hermes_thermal.thermal_node:main',
+        ],
+    },
+)

ros/hermes_thermal/test/test_node.py

@@ -0,0 +1,49 @@
+import pytest
+
+rclpy = pytest.importorskip('rclpy')
+
+
+def test_the_node_constructs_and_has_the_three_targets():
+    """Declaring a parameter twice raises at construction, and nothing else exercises the node."""
+    from hermes_thermal.thermal_node import ThermalNode
+    rclpy.init()
+    try:
+        node = ThermalNode()
+        assert [t.name for t in node.targets] == ['/camera', '/hermes_face', '/hermes_face_detector']
+        assert node.policy.level == 'ok'
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()
+
+
+def test_a_full_heat_then_cool_run_through_the_real_node_does_not_crash_and_steps_down_one_level_at_a_time():
+    """The bug this guards against: a logging call site whose severity changed (warn heating, info cooling)
+    raised in the timer callback, so the node died on its first cooling step."""
+    from rclpy.parameter import Parameter
+    from hermes_thermal.thermal_node import ThermalNode
+    rclpy.init()
+    try:
+        node = ThermalNode()
+        node.act = False                                          # no other nodes here; this is about the policy path
+        node.set_parameters([Parameter('dwell_s', value=0.0)])
+        levels = []
+        for temp in (60.0, 73.0, 78.0, 85.0, 60.0, 60.0, 60.0, 60.0):
+            node.set_parameters([Parameter('fake_temp_c', value=temp)])
+            node._poll()                                          # _poll, not _tick: let any exception surface
+            levels.append(node.policy.level)
+        assert levels == ['ok', 'warm', 'hot', 'critical', 'hot', 'warm', 'ok', 'ok']
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()
+
+
+def test_the_tick_survives_an_unexpected_error():
+    from hermes_thermal.thermal_node import ThermalNode
+    rclpy.init()
+    try:
+        node = ThermalNode()
+        node._poll = lambda: (_ for _ in ()).throw(RuntimeError('boom'))
+        node._tick()                                              # must not raise
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()

ros/hermes_thermal/test/test_policy.py

@@ -0,0 +1,57 @@
+import pytest
+
+from hermes_thermal.policy import LEVELS, SCALE, ThermalPolicy, scaled
+
+
+def run(p, temps, t0=0.0, dt=1.0):
+    t, out = t0, []
+    for temp in temps:
+        t += dt
+        out.append(p.update(temp, t))
+    return out, t
+
+
+def test_cool_stays_ok():
+    out, _ = run(ThermalPolicy(), [55, 60, 65, 71.9])
+    assert set(out) == {'ok'}
+
+
+def test_heating_escalates_at_once_even_several_levels():
+    p = ThermalPolicy()
+    assert p.update(73, 1.0) == 'warm'
+    assert p.update(78, 2.0) == 'hot'
+    assert p.update(90, 3.0) == 'critical'         # straight to the top if it asks for it
+    assert ThermalPolicy().update(84, 1.0) == 'critical'
+
+
+def test_cooling_needs_the_margin_and_the_dwell_and_goes_one_step_at_a_time():
+    p = ThermalPolicy(dwell_s=20.0, margin_c=3.0)
+    p.update(84, 0.0)                              # critical
+    assert p.update(75, 5.0) == 'critical'         # cooler, but inside the dwell
+    assert p.update(75, 25.0) == 'hot'             # dwell over, 75 < 82-3: one step down
+    assert p.update(75, 26.0) == 'hot'             # not another step at once
+    assert p.update(73, 46.0) == 'warm'            # 73 < 77-3
+    assert p.update(70, 47.0) == 'warm'
+    assert p.update(68, 67.0) == 'ok'              # 68 < 72-3
+
+
+def test_a_temperature_hovering_at_a_threshold_does_not_flap():
+    p = ThermalPolicy(dwell_s=20.0, margin_c=3.0)
+    out, _ = run(p, [72.5, 71.5, 72.2, 71.0, 72.1, 70.0, 72.3] * 6, dt=1.0)
+    assert out[0] == 'warm' and set(out) == {'warm'}     # never drops back to ok while it stays within the margin
+
+
+def test_a_reheat_during_recovery_escalates_again():
+    p = ThermalPolicy(dwell_s=10.0)
+    p.update(84, 0.0)
+    assert p.update(70, 20.0) == 'hot'
+    assert p.update(83, 21.0) == 'critical'
+
+
+def test_scaling_respects_floors_and_never_exceeds_normal():
+    assert scaled(15.0, 'ok', 2.0) == 15.0
+    assert scaled(15.0, 'hot', 2.0) == pytest.approx(15 * 0.45)
+    assert scaled(15.0, 'critical', 4.0) == 4.0            # the floor wins over 15*0.2 = 3
+    assert scaled(1.0, 'critical', 2.0) == 1.0             # a floor above normal must not raise the rate
+    assert [scaled(30.0, lv, 8.0) for lv in LEVELS] == sorted([scaled(30.0, lv, 8.0) for lv in LEVELS], reverse=True)
+    assert SCALE['ok'] == 1.0

ros/hermes_thermal/test/test_sensors.py

@@ -0,0 +1,34 @@
+import subprocess
+
+from hermes_thermal.sensors import decode_throttled, parse_throttled, read_temp_c, read_throttled
+
+
+def test_read_temp(tmp_path):
+    f = tmp_path / 'temp'
+    f.write_text('61850\n')
+    assert read_temp_c(str(f)) == 61.85
+    assert read_temp_c(str(tmp_path / 'missing')) is None
+    f.write_text('garbage')
+    assert read_temp_c(str(f)) is None
+
+
+def test_parse_and_decode_throttled():
+    assert parse_throttled('throttled=0x0\n') == 0
+    assert parse_throttled('throttled=0x50005') == 0x50005
+    assert parse_throttled('nonsense') is None
+    d = decode_throttled(0x50005)                 # under-voltage and throttled, now and since boot
+    assert d['now'] == {'under_voltage': True, 'arm_frequency_capped': False, 'throttled': True,
+                        'soft_temperature_limit': False}
+    assert d['since_boot']['under_voltage'] and d['since_boot']['throttled'] and not d['since_boot']['arm_frequency_capped']
+    assert decode_throttled(0)['now'] == {k: False for k in d['now']}
+    assert decode_throttled(None) == {'available': False}
+
+
+def test_read_throttled_handles_a_missing_vcgencmd():
+    def boom(*a, **k):
+        raise FileNotFoundError
+    assert read_throttled(boom) is None
+
+    def ok(*a, **k):
+        return subprocess.CompletedProcess(a, 0, stdout='throttled=0x20002\n', stderr='')
+    assert read_throttled(ok) == 0x20002