diff --git a/CONTEXT.md b/CONTEXT.md index 07a957f..11a5d46 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -321,6 +321,31 @@ unrelated photo scored 0.16. **Nobody is enrolled yet**; the owner enrols themse 4. A new Omarchy host with a GPU (it runs the local `ornith` model) is arriving: it needs only `host/omarchy/robot-usb-link.sh` and its SSH key added to the Pi. +## Thermal guard (`ros/hermes_thermal`) + +Built because the Pi has no fan (about 60 C open-air with everything running) and an enclosure will be +hotter. `thermal_guard` reads the CPU temperature (`/sys/class/thermal`) and `vcgencmd get_throttled`, +publishes `/health/temperature`, `/health/thermal_level` (ok | warm | hot | critical) and +`/health/throttled`, and sheds load through the target nodes' own live parameters: the camera's +`FrameDurationLimits`, the face's `fps` and the detector's `rate_hz` (the face and detector gained live +parameter callbacks for this). Levels enter at 72 / 77 / 82 C (the Pi throttles at about 80); heating is +acted on at once, cooling goes one level at a time, only below the threshold minus 3 C and after a 20 s +dwell. Rates scale by 1.0 / 0.7 / 0.45 / 0.2 with floors (camera 2 fps, face 8, detector 1 Hz). The +"normal" rates are parameters of the guard and a bringup test asserts they equal the real ones in +`robot.yaml`, so restoring is exact and survives the guard restarting. `fake_temp_c` forces a +temperature for testing. `robot://status` now carries a `health` block (temperature, level, throttle +flags now and since boot, which doubles as a power-supply check). +**Verified live on the robot** by forcing 73 / 78 / 85 C and back: the camera, face and detector rates +followed (measured camera 16.5 -> 11.5 -> 7.2 -> 3.5 Hz and back to 16.5), cooling stepped down critical +-> hot -> warm -> ok exactly one dwell apart, and the guard stayed the same process throughout. +**A live bug the unit tests missed:** one logging call that warned when heating and informed when +cooling raised `ValueError: Logger severity cannot be changed between calls` in rclpy, killing the +guard on its first cooling step (the launch file respawned it, which only looked like a recovery). Fixed +with separate call sites, the tick now catches and logs any error instead of dying, and a test drives +the real node through a full heat-then-cool run; a mutation check (the old logging in a throwaway +copy) confirmed that test fails on exactly that error. Not yet tested under a real thermal load. +When the servos arrive, watch the under-voltage flag in `robot://status` during a sweep. + ## Arrival day: the neck (servos), planned and deliberately not built ahead of the hardware The servos are expected Thursday or Friday at the earliest. The real unknowns cannot be faked (which diff --git a/README.md b/README.md index 2406650..39c4e08 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,8 @@ only speaks MCP. | Face on the display, always on at power-up, with a sleeping pose | Servos: `turn_head`, `head_position`, the pan/tilt URDF | | Camera, face detection, opt-in recognition of people you enrol | Speakers: playback, echo cancel, Hermes talking out loud | | Eyes follow faces; mouth draws your live voice | Fan (the Pi runs at about 62 °C open-air without one) | -| MCP bridge: `hermes mcp test robot` passes, a real Hermes turn calls `look()` | USB-C splitter (the Pi is powered from your computer's port for now) | +| MCP bridge: `hermes mcp test robot` passes, real Hermes turns call `look()` and `faces()` | USB-C splitter (the Pi is powered from your computer's port for now) | +| Thermal guard: slows the camera, face and detector before the Pi throttles | | | USB network link, firewall, boots and works with Wi-Fi off | Printed enclosure (`enclosure/`) | ## How it fits together @@ -127,6 +128,11 @@ give MCP images to the main model: it saves the photo to its image cache and the | looking | squint, then a flash | `look()` | | speaking | mouth draws the outgoing audio | the speaker | +- **Heat:** `hermes_thermal` watches the CPU temperature and, at 72, 77 and 82 °C (the Pi starts + throttling at about 80), scales the camera, face and detector rates down, then restores them as it cools + (one step at a time, so it does not flap). The temperature and the Pi's throttle and under-voltage + flags are in `robot://status` and on `/health/*`. To test it without heating the Pi, on the Pi: + `ros2 param set /hermes_thermal fake_temp_c 85.0` (and `-1` to stop). - **Logs and control:** `journalctl -u robot -f` (the face logs every state change), `sudo systemctl restart robot`. - **Enrol yourself** so the eyes prefer you (numbers only are stored, never photos, in @@ -195,7 +201,7 @@ about 0.5 px is good. `sudo systemctl restart robot` to load it. | Path | What | |---|---| -| `ros/` | The ROS 2 workspace: `hermes_face`, `hermes_vision`, `hermes_audio`, `hermes_mcp`, `hermes_bringup`. | +| `ros/` | The ROS 2 workspace: `hermes_face`, `hermes_vision`, `hermes_audio`, `hermes_mcp`, `hermes_thermal`, `hermes_bringup`. | | `pi/` | Everything installed on the Pi: `install.sh` and its parts, services, firewall, overlays. | | `host/` | Your computer's side: the USB link script, Hermes and PipeWire config, a platform check. | | `enclosure/` | OpenSCAD model, STLs and the print guide. Built from estimates: measure before printing. | diff --git a/ros/hermes_bringup/config/robot.yaml b/ros/hermes_bringup/config/robot.yaml index 3a86970..12ca4ae 100644 --- a/ros/hermes_bringup/config/robot.yaml +++ b/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 diff --git a/ros/hermes_bringup/launch/robot.launch.py b/ros/hermes_bringup/launch/robot.launch.py index 20bd321..5b2bf0f 100644 --- a/ros/hermes_bringup/launch/robot.launch.py +++ b/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(): diff --git a/ros/hermes_bringup/package.xml b/ros/hermes_bringup/package.xml index 135a870..50c7fbf 100644 --- a/ros/hermes_bringup/package.xml +++ b/ros/hermes_bringup/package.xml @@ -15,6 +15,7 @@ hermes_vision hermes_audio hermes_mcp + hermes_thermal python3-pytest diff --git a/ros/hermes_bringup/test/test_launch.py b/ros/hermes_bringup/test/test_launch.py index bd2f5ca..5a044c3 100644 --- a/ros/hermes_bringup/test/test_launch.py +++ b/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 diff --git a/ros/hermes_face/hermes_face/face_node.py b/ros/hermes_face/hermes_face/face_node.py index 5512087..94722eb 100644 --- a/ros/hermes_face/hermes_face/face_node.py +++ b/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)) diff --git a/ros/hermes_face/test/test_live_params.py b/ros/hermes_face/test/test_live_params.py new file mode 100644 index 0000000..a3723e4 --- /dev/null +++ b/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() diff --git a/ros/hermes_mcp/hermes_mcp/bridge.py b/ros/hermes_mcp/hermes_mcp/bridge.py index 18e5785..e17b5c8 100644 --- a/ros/hermes_mcp/hermes_mcp/bridge.py +++ b/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 [])), diff --git a/ros/hermes_mcp/hermes_mcp/mcp_node.py b/ros/hermes_mcp/hermes_mcp/mcp_node.py index d398253..e7c0c6e 100644 --- a/ros/hermes_mcp/hermes_mcp/mcp_node.py +++ b/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) diff --git a/ros/hermes_mcp/package.xml b/ros/hermes_mcp/package.xml index 74ec1a0..ac1116b 100644 --- a/ros/hermes_mcp/package.xml +++ b/ros/hermes_mcp/package.xml @@ -11,6 +11,7 @@ sensor_msgs std_msgs vision_msgs + hermes_thermal python3-pytest diff --git a/ros/hermes_mcp/test/test_bridge.py b/ros/hermes_mcp/test/test_bridge.py index 4486cfb..c578a8d 100644 --- a/ros/hermes_mcp/test/test_bridge.py +++ b/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' diff --git a/ros/hermes_thermal/hermes_thermal/__init__.py b/ros/hermes_thermal/hermes_thermal/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_thermal/hermes_thermal/policy.py b/ros/hermes_thermal/hermes_thermal/policy.py new file mode 100644 index 0000000..82b042a --- /dev/null +++ b/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 diff --git a/ros/hermes_thermal/hermes_thermal/sensors.py b/ros/hermes_thermal/hermes_thermal/sensors.py new file mode 100644 index 0000000..ccd2c54 --- /dev/null +++ b/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()}} diff --git a/ros/hermes_thermal/hermes_thermal/thermal_node.py b/ros/hermes_thermal/hermes_thermal/thermal_node.py new file mode 100644 index 0000000..0d21046 --- /dev/null +++ b/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() diff --git a/ros/hermes_thermal/package.xml b/ros/hermes_thermal/package.xml new file mode 100644 index 0000000..dc3f1a8 --- /dev/null +++ b/ros/hermes_thermal/package.xml @@ -0,0 +1,20 @@ + + + + hermes_thermal + 0.1.0 + Watches the Pi's temperature and sheds load before it throttles. + K. Isom + UNLICENSED + + rclpy + rcl_interfaces + sensor_msgs + std_msgs + + python3-pytest + + + ament_python + + diff --git a/ros/hermes_thermal/resource/hermes_thermal b/ros/hermes_thermal/resource/hermes_thermal new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_thermal/setup.cfg b/ros/hermes_thermal/setup.cfg new file mode 100644 index 0000000..6fd988e --- /dev/null +++ b/ros/hermes_thermal/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/hermes_thermal +[install] +install_scripts=$base/lib/hermes_thermal diff --git a/ros/hermes_thermal/setup.py b/ros/hermes_thermal/setup.py new file mode 100644 index 0000000..3a43222 --- /dev/null +++ b/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', + ], + }, +) diff --git a/ros/hermes_thermal/test/test_node.py b/ros/hermes_thermal/test/test_node.py new file mode 100644 index 0000000..cc8a4b3 --- /dev/null +++ b/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() diff --git a/ros/hermes_thermal/test/test_policy.py b/ros/hermes_thermal/test/test_policy.py new file mode 100644 index 0000000..c4817c7 --- /dev/null +++ b/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 diff --git a/ros/hermes_thermal/test/test_sensors.py b/ros/hermes_thermal/test/test_sensors.py new file mode 100644 index 0000000..4f09461 --- /dev/null +++ b/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 diff --git a/ros/hermes_vision/hermes_vision/face_detector_node.py b/ros/hermes_vision/hermes_vision/face_detector_node.py index 219e570..67b30ce 100644 --- a/ros/hermes_vision/hermes_vision/face_detector_node.py +++ b/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: diff --git a/ros/hermes_vision/test/test_node.py b/ros/hermes_vision/test/test_node.py index e220114..fc3902d 100644 --- a/ros/hermes_vision/test/test_node.py +++ b/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()