hermes-bot/ros/hermes_mcp/hermes_mcp/mcp_node.py

"""ROS node: the bridge between the outside world and the ROS graph, and the only way in or out.

Serves MCP (streamable HTTP, bearer token) to Hermes on the USB link and talks to the graph locally.
Subscribes:  image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
             /vision/faces (vision_msgs/Detection2DArray) and /camera/camera_info, for faces() and look()
Publishes:   /hermes/state (std_msgs/String):
               `idle` once a second while an authenticated MCP client is connected: the agent heartbeat
                 the face uses to wake and, when it stops, to fall asleep ("no agent connected");
               `looking` when look() is called, so the face squints and flashes.
The token comes from the ROBOT_MCP_TOKEN environment variable (an env file readable only by the
service user), never from a parameter.
"""
import os
import threading
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 vision_msgs.msg import Detection2DArray

from .bridge import FacesBuffer, LatestFrame, build_app, build_mcp


class McpNode(Node):
    def __init__(self):
        super().__init__('hermes_mcp')
        param = lambda name, default: self.declare_parameter(name, default).value  # noqa: E731
        topic = param('image_topic', '/camera/image_raw/compressed')
        self.port = int(param('port', 8765))
        self.host = str(param('bind', '10.55.0.2'))          # the USB link; the firewall allows only 10.55.0.1
        self.grace_s = float(param('connected_grace_s', 30.0))
        look_timeout = float(param('look_timeout_s', 3.0))
        if bool(param('head', False)):
            self.get_logger().error('head:=true but hermes_head is not built yet: turn_head stays off')
        self.state_pub = self.create_publisher(String, '/hermes/state', 10)
        self.frames = LatestFrame()
        self.create_subscription(CompressedImage, topic, lambda m: self.frames.put(bytes(m.data)), 1)
        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)
        people_path = os.path.expanduser(str(param('faces_db', '~/.local/share/hermes/faces.json')))
        hardware = {'speakers': bool(param('speakers', False))}

        token = os.environ.get('ROBOT_MCP_TOKEN', '').strip()
        self.activity = None
        self._server = None
        self._stopping = False
        if len(token) < 16:
            self.get_logger().error('ROBOT_MCP_TOKEN is missing or too short (openssl rand -hex 32): '
                                    '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)
        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)

    def _on_camera_info(self, msg: CameraInfo) -> None:
        if msg.width and msg.height:
            self.faces.set_size(msg.width, msg.height)

    def _on_faces(self, msg: Detection2DArray) -> None:
        faces = []
        for d in msg.detections:
            if not d.results:
                continue
            c = d.bbox.center.position
            faces.append({
                'track_id': int(d.id) if d.id.isdigit() else hash(d.id) & 0xFFFF,
                'bbox': {'x': c.x - d.bbox.size_x / 2, 'y': c.y - d.bbox.size_y / 2, 'w': d.bbox.size_x, 'h': d.bbox.size_y},
                'confidence': d.results[0].hypothesis.score,
                'name': d.results[1].hypothesis.class_id if len(d.results) > 1 else '',
                'similarity': d.results[1].hypothesis.score if len(d.results) > 1 else 0.0,
            })
        self.faces.put(faces)

    def _announce_look(self) -> None:
        self.state_pub.publish(String(data='looking'))

    def _heartbeat(self) -> None:
        if self.activity.connected(time.monotonic(), self.grace_s):
            self.state_pub.publish(String(data='idle'))

    def _serve(self) -> None:
        """Serve until stopped. The USB address may not exist yet at boot, so a failed bind retries."""
        while not self._stopping:
            self._server = uvicorn.Server(uvicorn.Config(self.app, host=self.host, port=self.port,
                                                         log_level='warning'))
            try:
                self.get_logger().info(f'serving MCP on http://{self.host}:{self.port}/mcp')
                self._server.run()
            except (SystemExit, OSError) as exc:
                self.get_logger().warn(f'MCP server stopped ({exc!r}); retrying in 3 s', throttle_duration_sec=30.0)
            time.sleep(3.0)

    def destroy_node(self):
        self._stopping = True
        if self._server is not None:
            self._server.should_exit = True
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = McpNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()