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_bringup/config/robot.yaml

@@ -39,3 +39,14 @@ hermes_mcp:
     speakers: false                    # no speakers fitted yet: reported honestly in robot://status
     connected_grace_s: 30.0            # an authenticated client heard within this long keeps the heartbeat going
     look_timeout_s: 3.0
+
+hermes_thermal:
+  ros__parameters:
+    # Sheds load before the Pi throttles (it starts at about 80 C). These "normal" rates MUST equal the
+    # ones above (camera 66666 us = 15 fps, face fps, detector rate_hz): a test checks it.
+    camera_fps: 15.0
+    face_fps: 30.0
+    detector_hz: 4.0
+    warm_c: 72.0
+    hot_c: 77.0
+    critical_c: 82.0

ros/hermes_bringup/launch/robot.launch.py

@@ -1,6 +1,6 @@
-"""Bring up the robot: camera, face, face detection, audio levels, and the MCP bridge.
+"""Bring up the robot: camera, face, face detection, audio levels, the MCP bridge and the thermal guard.
 
-  ros2 launch hermes_bringup robot.launch.py [camera:=false] [face:=false] [vision:=false] [audio:=false] [mcp:=false]
+  ros2 launch hermes_bringup robot.launch.py [camera:=false] [face:=false] [vision:=false] [audio:=false] [mcp:=false] [thermal:=false]
                                               [start_delay:=6.0]
 
 Every node is its own process and is respawned if it exits, so one crash cannot take the others
@@ -25,10 +25,11 @@ NODES = [
     ('vision', 'hermes_vision', 'face_detector', 'hermes_face_detector'),
     ('audio', 'hermes_audio', 'audio_state', 'hermes_audio_state'),
     ('mcp', 'hermes_mcp', 'mcp_server', 'hermes_mcp'),
+    ('thermal', 'hermes_thermal', 'thermal_guard', 'hermes_thermal'),
 ]
 
 
-DELAYED = {'camera', 'vision', 'audio', 'mcp'}
+DELAYED = {'camera', 'vision', 'audio', 'mcp', 'thermal'}
 
 
 def generate_launch_description():

ros/hermes_bringup/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>hermes_vision</exec_depend>
   <exec_depend>hermes_audio</exec_depend>
   <exec_depend>hermes_mcp</exec_depend>
+  <exec_depend>hermes_thermal</exec_depend>
 
   <test_depend>python3-pytest</test_depend>
 

ros/hermes_bringup/test/test_launch.py

@@ -19,8 +19,8 @@ def test_launch_description_has_one_respawning_process_per_node():
     ld = mod.generate_launch_description()
     nodes = [e for e in ld.entities if isinstance(e, Node)]
     delayed = [n for t in ld.entities if isinstance(t, TimerAction) for n in t.actions if isinstance(n, Node)]
-    assert len(nodes) + len(delayed) == len(mod.NODES) == 5
-    assert len(nodes) == 1 and 'face' in mod.NODES[1] and len(delayed) == 4   # the face starts at once
+    assert len(nodes) + len(delayed) == len(mod.NODES) == 6
+    assert len(nodes) == 1 and 'face' in mod.NODES[1] and len(delayed) == 5   # the face starts at once
     # respawn lives on a private attribute that differs between ROS releases, so check the source
     src = open(os.path.join(ROOT, 'launch', 'robot.launch.py')).read()
     assert 'respawn=True' in src and 'respawn_delay' in src
@@ -28,7 +28,7 @@ def test_launch_description_has_one_respawning_process_per_node():
 
 def test_every_node_in_the_launch_file_has_parameters_and_the_face_is_mounted_portrait():
     cfg = yaml.safe_load(open(os.path.join(ROOT, 'config', 'robot.yaml')))
-    for name in ('camera', 'hermes_face', 'hermes_face_detector', 'hermes_audio_state', 'hermes_mcp'):
+    for name in ('camera', 'hermes_face', 'hermes_face_detector', 'hermes_audio_state', 'hermes_mcp', 'hermes_thermal'):
         assert 'ros__parameters' in cfg[name], name
     assert cfg['hermes_face']['ros__parameters']['rotation'] == 270
     assert cfg['camera']['ros__parameters']['FrameDurationLimits'] == [66666, 66666]
@@ -38,3 +38,13 @@ def test_the_mcp_bridge_binds_the_usb_link_and_has_no_head_yet():
     cfg = yaml.safe_load(open(os.path.join(ROOT, 'config', 'robot.yaml')))['hermes_mcp']['ros__parameters']
     assert cfg['bind'] == '10.55.0.2' and cfg['port'] == 8765 and cfg['head'] is False
     assert 'token' not in ' '.join(cfg)                  # the secret lives in an env file, not here
+
+
+def test_the_thermal_guards_normal_rates_equal_the_real_ones():
+    cfg = yaml.safe_load(open(os.path.join(ROOT, 'config', 'robot.yaml')))
+    t = cfg['hermes_thermal']['ros__parameters']
+    d = cfg['camera']['ros__parameters']['FrameDurationLimits'][0]
+    assert abs(t['camera_fps'] - 1e6 / d) < 0.01
+    assert t['face_fps'] == cfg['hermes_face']['ros__parameters']['fps']
+    assert t['detector_hz'] == cfg['hermes_face_detector']['ros__parameters']['rate_hz']
+    assert t['warm_c'] < t['hot_c'] < t['critical_c'] < 85.0       # all below the Pi's hard limit

ros/hermes_face/hermes_face/face_node.py

@@ -20,6 +20,7 @@ import math
 import time
 
 import rclpy
+from rcl_interfaces.msg import SetParametersResult
 from rclpy.node import Node
 from sensor_msgs.msg import CameraInfo, JointState
 from std_msgs.msg import Float32MultiArray, String
@@ -73,7 +74,8 @@ class FaceNode(Node):
             self.create_subscription(CameraInfo, '/camera/camera_info', self._on_camera_info, 1)
         self._last = time.monotonic()
         self._frames, self._stat_t = 0, self._last
-        self.create_timer(1.0 / self.fps, self._tick)
+        self._timer = self.create_timer(1.0 / self.fps, self._tick)
+        self.add_on_set_parameters_callback(self._on_params)       # the thermal guard slows the face when hot
         self.get_logger().info(f'face up: {output}, {self.fps:g} fps, state {self.renderer.state}')
         self._resolve(time.monotonic())
 
@@ -109,6 +111,19 @@ class FaceNode(Node):
             self.renderer.set_pose(math.degrees(pos.get(self.pan_joint, 0.0)),
                                    math.degrees(pos.get(self.tilt_joint, 0.0)))
 
+    def _on_params(self, params) -> SetParametersResult:
+        for p in params:
+            if p.name == 'fps':
+                try:
+                    fps = min(60.0, max(1.0, float(p.value)))
+                except (TypeError, ValueError):
+                    return SetParametersResult(successful=False, reason='fps must be a number')
+                if fps != self.fps:
+                    self.fps = fps
+                    self.destroy_timer(self._timer)
+                    self._timer = self.create_timer(1.0 / fps, self._tick)
+        return SetParametersResult(successful=True)
+
     def _on_camera_info(self, msg: CameraInfo) -> None:
         if msg.width and msg.height:
             self.image_size = (float(msg.width), float(msg.height))

ros/hermes_face/test/test_live_params.py

@@ -0,0 +1,18 @@
+import pytest
+
+rclpy = pytest.importorskip('rclpy')
+
+
+def test_face_fps_can_be_changed_live_and_is_clamped():
+    from rclpy.parameter import Parameter
+    from hermes_face.face_node import FaceNode
+    rclpy.init(args=['--ros-args', '-p', 'output:=none'])
+    try:
+        node = FaceNode()
+        assert node.fps == 30.0
+        assert node.set_parameters([Parameter('fps', value=12.0)])[0].successful and node.fps == 12.0
+        assert node.set_parameters([Parameter('fps', value=500.0)])[0].successful and node.fps == 60.0   # clamped
+        assert not node.set_parameters([Parameter('fps', value='fast')])[0].successful
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()

ros/hermes_mcp/hermes_mcp/bridge.py

@@ -109,6 +109,26 @@ class FacesBuffer:
             return self._snapshot(time.monotonic())
 
 
+class HealthBuffer:
+    """The newest temperature, thermal level and throttle flags, for robot://status."""
+
+    def __init__(self) -> None:
+        self._lock = threading.Lock()
+        self._d: dict = {}
+        self._t = -1.0
+
+    def update(self, **kw) -> None:
+        with self._lock:
+            self._d.update(kw)
+            self._t = time.monotonic()
+
+    def summary(self, max_age_s: float = 15.0) -> dict:
+        with self._lock:
+            if self._t < 0 or time.monotonic() - self._t > max_age_s:
+                return {'available': False}
+            return {'available': True, **self._d}
+
+
 def people_summary(path: str) -> dict:
     """Who is enrolled on this robot: names and how many samples, read straight from the registry file
     (JSON), never the face data itself."""
@@ -128,8 +148,9 @@ class Head(Protocol):
 def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: None,
               head: Optional[Head] = None, look_timeout_s: float = 3.0,
               faces_buf: Optional[FacesBuffer] = None, people_path: str = '',
-              hardware: Optional[dict] = None) -> MCPServer:
+              hardware: Optional[dict] = None, health: Optional[HealthBuffer] = None) -> MCPServer:
     faces_buf = faces_buf or FacesBuffer()
+    health = health or HealthBuffer()
     hw = {'camera': True, 'head': head is not None, 'speakers': False, **(hardware or {})}
     mcp = MCPServer(
         "robot",
@@ -203,6 +224,7 @@ def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: N
             'camera': {'image_size': live.get('image_size') if live['available'] else None},
             'detects': ['faces'], 'not_detected': ['people', 'objects', 'motion'],
             'face_detection_running': live['available'],
+            'health': health.summary(),
             'recognition': {'enrolled': people_summary(people_path)['count'] if people_path else 0,
                             'registry': 'on the robot; enrolled by the owner, not through MCP; embeddings never leave it'},
             'tools': sorted(['faces', 'look', 'list_people'] + (['turn_head', 'head_position'] if head else [])),

ros/hermes_mcp/hermes_mcp/mcp_node.py

@@ -17,11 +17,13 @@ import time
 import rclpy
 import uvicorn
 from rclpy.node import Node
-from sensor_msgs.msg import CameraInfo, CompressedImage
-from std_msgs.msg import String
+from sensor_msgs.msg import CameraInfo, CompressedImage, Temperature
+from std_msgs.msg import String, UInt32
 from vision_msgs.msg import Detection2DArray
 
-from .bridge import FacesBuffer, LatestFrame, build_app, build_mcp
+from hermes_thermal.sensors import decode_throttled
+
+from .bridge import FacesBuffer, HealthBuffer, LatestFrame, build_app, build_mcp
 
 
 class McpNode(Node):
@@ -41,6 +43,12 @@ class McpNode(Node):
         self.faces = FacesBuffer()
         self.create_subscription(Detection2DArray, '/vision/faces', self._on_faces, 5)
         self.create_subscription(CameraInfo, '/camera/camera_info', self._on_camera_info, 1)
+        self.health = HealthBuffer()
+        self.create_subscription(Temperature, '/health/temperature',
+                                 lambda m: self.health.update(temperature_c=round(m.temperature, 1)), 2)
+        self.create_subscription(String, '/health/thermal_level', lambda m: self.health.update(thermal_level=m.data), 2)
+        self.create_subscription(UInt32, '/health/throttled',
+                                 lambda m: self.health.update(throttled=decode_throttled(m.data)), 2)
         people_path = os.path.expanduser(str(param('faces_db', '~/.local/share/hermes/faces.json')))
         hardware = {'speakers': bool(param('speakers', False))}
 
@@ -53,7 +61,7 @@ class McpNode(Node):
                                     'not serving MCP, and no heartbeat')
             return
         mcp = build_mcp(self.frames, self._announce_look, head=None, look_timeout_s=look_timeout,
-                        faces_buf=self.faces, people_path=people_path, hardware=hardware)
+                        faces_buf=self.faces, people_path=people_path, hardware=hardware, health=self.health)
         self.app, self.activity = build_app(mcp, token, self.host)
         threading.Thread(target=self._serve, name='mcp-http', daemon=True).start()
         self.create_timer(1.0, self._heartbeat)

ros/hermes_mcp/package.xml

@@ -11,6 +11,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>vision_msgs</depend>
+  <exec_depend>hermes_thermal</exec_depend>
 
   <test_depend>python3-pytest</test_depend>
 

ros/hermes_mcp/test/test_bridge.py

@@ -10,7 +10,7 @@ pytest.importorskip('mcp')
 from starlette.testclient import TestClient  # noqa: E402
 
 from hermes_mcp.bridge import (Activity, BearerAuth, FacesBuffer, LatestFrame, build_app, build_mcp,  # noqa: E402
-                               people_summary)
+                               HealthBuffer, people_summary)
 
 TOKEN = 'x' * 32
 
@@ -172,3 +172,13 @@ def test_the_server_can_be_discovered_resources_and_prompts_are_not_empty():
     assert status['detects'] == ['faces'] and 'people' in status['not_detected']
     assert 'embeddings never leave it' in status['recognition']['registry']
     assert 'turn_head' not in status['tools']
+
+
+def test_status_reports_health_when_known_and_says_when_it_is_not():
+    h = HealthBuffer()
+    mcp = build_mcp(LatestFrame(), health=h)
+    unknown = json.loads(asyncio.run(mcp.read_resource('robot://status'))[0].content)
+    assert unknown['health'] == {'available': False}
+    h.update(temperature_c=63.4, thermal_level='ok', throttled={'available': True})
+    known = json.loads(asyncio.run(mcp.read_resource('robot://status'))[0].content)
+    assert known['health']['temperature_c'] == 63.4 and known['health']['thermal_level'] == 'ok'

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

ros/hermes_vision/hermes_vision/face_detector_node.py

@@ -17,6 +17,7 @@ import time
 import cv2
 import numpy as np
 import rclpy
+from rcl_interfaces.msg import SetParametersResult
 from rclpy.node import Node
 from sensor_msgs.msg import CompressedImage
 from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
@@ -53,10 +54,20 @@ class FaceDetectorNode(Node):
                           if self.debug else None)
         self.create_subscription(CompressedImage, topic, self._on_image, 1)
         self._next = 0.0
+        self.add_on_set_parameters_callback(self._on_params)       # the thermal guard slows detection when hot
         n = len(self.recognizer.enrolled) if self.recognizer else None   # a count, not names: logs persist
         self.get_logger().info(f'face detector up: {self.detector.backend}, {1 / self.period:g} Hz, {topic}; '
                                f'recognition: {"off" if n is None else f"{n} person(s) enrolled"}')
 
+    def _on_params(self, params) -> SetParametersResult:
+        for p in params:
+            if p.name == 'rate_hz':
+                try:
+                    self.period = 1.0 / min(30.0, max(0.5, float(p.value)))
+                except (TypeError, ValueError, ZeroDivisionError):
+                    return SetParametersResult(successful=False, reason='rate_hz must be a positive number')
+        return SetParametersResult(successful=True)
+
     def _on_image(self, msg: CompressedImage) -> None:
         now = time.monotonic()
         if now < self._next:

ros/hermes_vision/test/test_node.py

@@ -13,3 +13,19 @@ def test_the_node_constructs_with_default_parameters():
         node.destroy_node()
     finally:
         rclpy.shutdown()
+
+
+def test_detector_rate_can_be_changed_live():
+    from rclpy.parameter import Parameter
+    from hermes_vision.face_detector_node import FaceDetectorNode
+    rclpy.init()
+    try:
+        node = FaceDetectorNode()
+        assert abs(node.period - 0.25) < 1e-9
+        assert node.set_parameters([Parameter('rate_hz', value=1.0)])[0].successful
+        assert abs(node.period - 1.0) < 1e-9
+        assert node.set_parameters([Parameter('rate_hz', value=0.01)])[0].successful
+        assert abs(node.period - 2.0) < 1e-9                    # clamped to 0.5 Hz
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()