@rainbow-robotics/muscat-plugin-kit 1.10.1-dev.1 → 1.10.1

package/cli/src/host_agent.py

@@ -1,223 +1,223 @@
-"""Host-side Zenoh byte-proxy for Docker Desktop containers (Mac/WSL remote dev).
-
-Joins the robot's Zenoh mesh with eclipse-zenoh (no rainbow SDK) and relays
-{query, put, subscribe} bytes over TCP to the dev container.
-
-Env: MUSCAT_ROBOT_HOST, MUSCAT_ROBOT_TOKEN, MUSCAT_ZENOH_ADVERTISE_HOST,
-MUSCAT_ZENOH_PORT (robot-facing), MUSCAT_PROXY_PORT (container-facing).
-"""
-
-from __future__ import annotations
-
-import contextlib
-import json
-import os
-import signal
-import socket
-import struct
-import sys
-import threading
-import urllib.error
-import urllib.request
-
-import zenoh
-
-PEER_TTL = 30
-QUERY_TARGETS = {0: zenoh.QueryTarget.BEST_MATCHING, 1: zenoh.QueryTarget.ALL, 2: zenoh.QueryTarget.ALL_COMPLETE}
-
-
-def _send_frame(conn: socket.socket, lock: threading.Lock, obj: dict) -> None:
-    body = json.dumps(obj).encode()
-    with lock:
-        conn.sendall(struct.pack(">I", len(body)) + body)
-
-
-def _recv_frame(conn: socket.socket) -> dict | None:
-    hdr = b""
-    while len(hdr) < 4:
-        chunk = conn.recv(4 - len(hdr))
-        if not chunk:
-            return None
-        hdr += chunk
-    n = struct.unpack(">I", hdr)[0]
-    buf = b""
-    while len(buf) < n:
-        chunk = conn.recv(n - len(buf))
-        if not chunk:
-            return None
-        buf += chunk
-    return json.loads(buf.decode())
-
-
-def _att(sample) -> str:
-    return sample.attachment.to_string() if sample.attachment is not None else ""
-
-
-def _query(session: zenoh.Session, req: dict) -> list[dict]:
-    payload = bytes.fromhex(req["payload_hex"]) if req.get("payload_hex") else None
-    replies = session.get(
-        req["keyexpr"],
-        payload=payload,
-        timeout=req.get("timeout_ms", 2000) / 1000,
-        target=QUERY_TARGETS.get(req.get("target", 0), zenoh.QueryTarget.BEST_MATCHING),
-    )
-    out = []
-    for reply in replies:
-        if reply.ok is not None:
-            s = reply.ok
-            out.append({"key": str(s.key_expr), "payload_hex": s.payload.to_bytes().hex(),
-                        "att": _att(s), "encoding": str(s.encoding), "is_error": False})
-        else:
-            e = reply.err
-            out.append({"key": req["keyexpr"], "payload_hex": e.payload.to_bytes().hex(),
-                        "att": "", "encoding": str(e.encoding), "is_error": True})
-    return out
-
-
-def _handle_conn(conn: socket.socket, session: zenoh.Session) -> None:
-    send_lock = threading.Lock()
-    subs: dict[str, zenoh.Subscriber] = {}
-    try:
-        while True:
-            req = _recv_frame(conn)
-            if req is None:
-                return
-            op = req.get("type")
-            rid = req.get("id")
-
-            try:
-                if op == "query":
-                    out = {"type": "query_resp", "id": rid, "ok": True, "replies": _query(session, req)}
-
-                elif op == "put":
-                    payload = bytes.fromhex(req["payload_hex"]) if req.get("payload_hex") else b""
-                    session.put(req["keyexpr"], payload)
-                    out = {"type": "put_resp", "id": rid, "ok": True}
-
-                elif op == "subscribe":
-                    keyexpr = req["keyexpr"]
-
-                    def on_sample(s, _ke: str = keyexpr) -> None:
-                        _send_frame(conn, send_lock, {
-                            "type": "sample", "keyexpr": _ke, "key": str(s.key_expr),
-                            "payload_hex": s.payload.to_bytes().hex(), "att": _att(s),
-                        })
-
-                    old = subs.pop(keyexpr, None)
-                    if old is not None:
-                        old.undeclare()
-                    subs[keyexpr] = session.declare_subscriber(keyexpr, on_sample)
-                    out = {"type": "subscribe_resp", "id": rid, "ok": True}
-
-                elif op == "unsubscribe":
-                    sub = subs.pop(req["keyexpr"], None)
-                    if sub is not None:
-                        with contextlib.suppress(Exception):
-                            sub.undeclare()
-                    out = {"type": "unsubscribe_resp", "id": rid, "ok": True}
-
-                else:
-                    out = {"type": "error", "error": f"unknown op {op!r}"}
-            except Exception as exc:
-                out = {"type": f"{op}_resp", "id": rid, "ok": False, "error": f"{type(exc).__name__}: {exc}"}
-            _send_frame(conn, send_lock, out)
-    except (ConnectionResetError, BrokenPipeError, OSError):
-        pass
-    finally:
-        for sub in subs.values():
-            with contextlib.suppress(Exception):
-                sub.undeclare()
-        conn.close()
-
-
-class Peer:
-    """Registers this host with the robot's common: POST /zenoh/peer, heartbeat, DELETE."""
-
-    def __init__(self, host: str, token: str, ip: str, port: int) -> None:
-        self.base = f"http://{host}:8000"
-        self.headers = {"Content-Type": "application/json"}
-        if token:
-            self.headers["Authorization"] = f"Bearer {token}"
-        self.body = {"ip": ip, "port": port, "ttl": PEER_TTL}
-        self.peer_id: str | None = None
-        self.stop = threading.Event()
-
-    def _http(self, method: str, path: str, body: dict | None = None) -> dict:
-        data = json.dumps(body).encode() if body is not None else None
-        req = urllib.request.Request(self.base + path, data=data, headers=self.headers, method=method)
-        with urllib.request.urlopen(req, timeout=10) as resp:
-            raw = resp.read()
-            return json.loads(raw) if raw else {}
-
-    def register(self) -> None:
-        self.peer_id = self._http("POST", "/zenoh/peer", self.body)["peer_id"]
-        print(f"[host-agent] registered with {self.base} as zenoh peer {self.peer_id}", flush=True)
-
-    def maintain(self) -> None:
-        while True:
-            wait = PEER_TTL / 2 if self.peer_id else 3
-            if self.stop.wait(wait):
-                return
-            try:
-                if self.peer_id is None:
-                    self.register()
-                elif not self._http("POST", f"/zenoh/peer/{self.peer_id}/heartbeat", {"ttl": PEER_TTL}).get("ok"):
-                    print(f"[host-agent] heartbeat rejected for {self.peer_id}, re-registering", flush=True)
-                    self.peer_id = None
-            except Exception as exc:
-                print(f"[host-agent] peer maintenance failed ({exc}); retrying", flush=True)
-                if self.peer_id is not None and isinstance(exc, urllib.error.HTTPError) and exc.code == 404:
-                    self.peer_id = None
-
-    def close(self) -> None:
-        self.stop.set()
-        if self.peer_id:
-            with contextlib.suppress(Exception):
-                self._http("DELETE", f"/zenoh/peer/{self.peer_id}")
-
-
-def main() -> None:
-    # dev stops us with SIGTERM; exit via `finally` to deregister.
-    signal.signal(signal.SIGTERM, lambda *_: sys.exit(0))
-    host = os.environ["MUSCAT_ROBOT_HOST"]
-    token = os.environ.get("MUSCAT_ROBOT_TOKEN", "")
-    advertise = os.environ["MUSCAT_ZENOH_ADVERTISE_HOST"]
-    zenoh_port = int(os.environ["MUSCAT_ZENOH_PORT"])
-    proxy_port = int(os.environ["MUSCAT_PROXY_PORT"])
-
-    # Listen only; the robot dials in after registration. Router mode so a C++ SDK dev container
-    # (native Zenoh session, no byte shim) can connect here and be routed to the robot.
-    conf = zenoh.Config()
-    conf.insert_json5("mode", '"peer"')
-    conf.insert_json5("listen/endpoints", json.dumps([f"tcp/0.0.0.0:{zenoh_port}"]))
-    conf.insert_json5("scouting/multicast/enabled", "false")
-    session = zenoh.open(conf)
-
-    peer = Peer(host, token, advertise, zenoh_port)
-    try:
-        peer.register()
-    except Exception as exc:
-        print(f"[host-agent] registration with {peer.base} failed, retrying in background: {exc}", flush=True)
-    threading.Thread(target=peer.maintain, daemon=True, name="zenoh-peer").start()
-    print(f"[host-agent] zenoh peer listening :{zenoh_port} (advertised as {advertise})", flush=True)
-
-    srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
-    srv.bind(("0.0.0.0", proxy_port))
-    srv.listen(32)
-    print(f"[host-agent] byte-proxy listening :{proxy_port} (container connects via host.docker.internal)", flush=True)
-
-    try:
-        while True:
-            conn, _addr = srv.accept()
-            threading.Thread(target=_handle_conn, args=(conn, session), daemon=True).start()
-    except KeyboardInterrupt:
-        pass
-    finally:
-        peer.close()
-        with contextlib.suppress(Exception):
-            session.close()
-
-
-if __name__ == "__main__":
-    main()
+"""Host-side Zenoh byte-proxy for Docker Desktop containers (Mac/WSL remote dev).
+
+Joins the robot's Zenoh mesh with eclipse-zenoh (no rainbow SDK) and relays
+{query, put, subscribe} bytes over TCP to the dev container.
+
+Env: MUSCAT_ROBOT_HOST, MUSCAT_ROBOT_TOKEN, MUSCAT_ZENOH_ADVERTISE_HOST,
+MUSCAT_ZENOH_PORT (robot-facing), MUSCAT_PROXY_PORT (container-facing).
+"""
+
+from __future__ import annotations
+
+import contextlib
+import json
+import os
+import signal
+import socket
+import struct
+import sys
+import threading
+import urllib.error
+import urllib.request
+
+import zenoh
+
+PEER_TTL = 30
+QUERY_TARGETS = {0: zenoh.QueryTarget.BEST_MATCHING, 1: zenoh.QueryTarget.ALL, 2: zenoh.QueryTarget.ALL_COMPLETE}
+
+
+def _send_frame(conn: socket.socket, lock: threading.Lock, obj: dict) -> None:
+    body = json.dumps(obj).encode()
+    with lock:
+        conn.sendall(struct.pack(">I", len(body)) + body)
+
+
+def _recv_frame(conn: socket.socket) -> dict | None:
+    hdr = b""
+    while len(hdr) < 4:
+        chunk = conn.recv(4 - len(hdr))
+        if not chunk:
+            return None
+        hdr += chunk
+    n = struct.unpack(">I", hdr)[0]
+    buf = b""
+    while len(buf) < n:
+        chunk = conn.recv(n - len(buf))
+        if not chunk:
+            return None
+        buf += chunk
+    return json.loads(buf.decode())
+
+
+def _att(sample) -> str:
+    return sample.attachment.to_string() if sample.attachment is not None else ""
+
+
+def _query(session: zenoh.Session, req: dict) -> list[dict]:
+    payload = bytes.fromhex(req["payload_hex"]) if req.get("payload_hex") else None
+    replies = session.get(
+        req["keyexpr"],
+        payload=payload,
+        timeout=req.get("timeout_ms", 2000) / 1000,
+        target=QUERY_TARGETS.get(req.get("target", 0), zenoh.QueryTarget.BEST_MATCHING),
+    )
+    out = []
+    for reply in replies:
+        if reply.ok is not None:
+            s = reply.ok
+            out.append({"key": str(s.key_expr), "payload_hex": s.payload.to_bytes().hex(),
+                        "att": _att(s), "encoding": str(s.encoding), "is_error": False})
+        else:
+            e = reply.err
+            out.append({"key": req["keyexpr"], "payload_hex": e.payload.to_bytes().hex(),
+                        "att": "", "encoding": str(e.encoding), "is_error": True})
+    return out
+
+
+def _handle_conn(conn: socket.socket, session: zenoh.Session) -> None:
+    send_lock = threading.Lock()
+    subs: dict[str, zenoh.Subscriber] = {}
+    try:
+        while True:
+            req = _recv_frame(conn)
+            if req is None:
+                return
+            op = req.get("type")
+            rid = req.get("id")
+
+            try:
+                if op == "query":
+                    out = {"type": "query_resp", "id": rid, "ok": True, "replies": _query(session, req)}
+
+                elif op == "put":
+                    payload = bytes.fromhex(req["payload_hex"]) if req.get("payload_hex") else b""
+                    session.put(req["keyexpr"], payload)
+                    out = {"type": "put_resp", "id": rid, "ok": True}
+
+                elif op == "subscribe":
+                    keyexpr = req["keyexpr"]
+
+                    def on_sample(s, _ke: str = keyexpr) -> None:
+                        _send_frame(conn, send_lock, {
+                            "type": "sample", "keyexpr": _ke, "key": str(s.key_expr),
+                            "payload_hex": s.payload.to_bytes().hex(), "att": _att(s),
+                        })
+
+                    old = subs.pop(keyexpr, None)
+                    if old is not None:
+                        old.undeclare()
+                    subs[keyexpr] = session.declare_subscriber(keyexpr, on_sample)
+                    out = {"type": "subscribe_resp", "id": rid, "ok": True}
+
+                elif op == "unsubscribe":
+                    sub = subs.pop(req["keyexpr"], None)
+                    if sub is not None:
+                        with contextlib.suppress(Exception):
+                            sub.undeclare()
+                    out = {"type": "unsubscribe_resp", "id": rid, "ok": True}
+
+                else:
+                    out = {"type": "error", "error": f"unknown op {op!r}"}
+            except Exception as exc:
+                out = {"type": f"{op}_resp", "id": rid, "ok": False, "error": f"{type(exc).__name__}: {exc}"}
+            _send_frame(conn, send_lock, out)
+    except (ConnectionResetError, BrokenPipeError, OSError):
+        pass
+    finally:
+        for sub in subs.values():
+            with contextlib.suppress(Exception):
+                sub.undeclare()
+        conn.close()
+
+
+class Peer:
+    """Registers this host with the robot's common: POST /zenoh/peer, heartbeat, DELETE."""
+
+    def __init__(self, host: str, token: str, ip: str, port: int) -> None:
+        self.base = f"http://{host}:8000"
+        self.headers = {"Content-Type": "application/json"}
+        if token:
+            self.headers["Authorization"] = f"Bearer {token}"
+        self.body = {"ip": ip, "port": port, "ttl": PEER_TTL}
+        self.peer_id: str | None = None
+        self.stop = threading.Event()
+
+    def _http(self, method: str, path: str, body: dict | None = None) -> dict:
+        data = json.dumps(body).encode() if body is not None else None
+        req = urllib.request.Request(self.base + path, data=data, headers=self.headers, method=method)
+        with urllib.request.urlopen(req, timeout=10) as resp:
+            raw = resp.read()
+            return json.loads(raw) if raw else {}
+
+    def register(self) -> None:
+        self.peer_id = self._http("POST", "/zenoh/peer", self.body)["peer_id"]
+        print(f"[host-agent] registered with {self.base} as zenoh peer {self.peer_id}", flush=True)
+
+    def maintain(self) -> None:
+        while True:
+            wait = PEER_TTL / 2 if self.peer_id else 3
+            if self.stop.wait(wait):
+                return
+            try:
+                if self.peer_id is None:
+                    self.register()
+                elif not self._http("POST", f"/zenoh/peer/{self.peer_id}/heartbeat", {"ttl": PEER_TTL}).get("ok"):
+                    print(f"[host-agent] heartbeat rejected for {self.peer_id}, re-registering", flush=True)
+                    self.peer_id = None
+            except Exception as exc:
+                print(f"[host-agent] peer maintenance failed ({exc}); retrying", flush=True)
+                if self.peer_id is not None and isinstance(exc, urllib.error.HTTPError) and exc.code == 404:
+                    self.peer_id = None
+
+    def close(self) -> None:
+        self.stop.set()
+        if self.peer_id:
+            with contextlib.suppress(Exception):
+                self._http("DELETE", f"/zenoh/peer/{self.peer_id}")
+
+
+def main() -> None:
+    # dev stops us with SIGTERM; exit via `finally` to deregister.
+    signal.signal(signal.SIGTERM, lambda *_: sys.exit(0))
+    host = os.environ["MUSCAT_ROBOT_HOST"]
+    token = os.environ.get("MUSCAT_ROBOT_TOKEN", "")
+    advertise = os.environ["MUSCAT_ZENOH_ADVERTISE_HOST"]
+    zenoh_port = int(os.environ["MUSCAT_ZENOH_PORT"])
+    proxy_port = int(os.environ["MUSCAT_PROXY_PORT"])
+
+    # Listen only; the robot dials in after registration. Router mode so a C++ SDK dev container
+    # (native Zenoh session, no byte shim) can connect here and be routed to the robot.
+    conf = zenoh.Config()
+    conf.insert_json5("mode", '"peer"')
+    conf.insert_json5("listen/endpoints", json.dumps([f"tcp/0.0.0.0:{zenoh_port}"]))
+    conf.insert_json5("scouting/multicast/enabled", "false")
+    session = zenoh.open(conf)
+
+    peer = Peer(host, token, advertise, zenoh_port)
+    try:
+        peer.register()
+    except Exception as exc:
+        print(f"[host-agent] registration with {peer.base} failed, retrying in background: {exc}", flush=True)
+    threading.Thread(target=peer.maintain, daemon=True, name="zenoh-peer").start()
+    print(f"[host-agent] zenoh peer listening :{zenoh_port} (advertised as {advertise})", flush=True)
+
+    srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    srv.bind(("0.0.0.0", proxy_port))
+    srv.listen(32)
+    print(f"[host-agent] byte-proxy listening :{proxy_port} (container connects via host.docker.internal)", flush=True)
+
+    try:
+        while True:
+            conn, _addr = srv.accept()
+            threading.Thread(target=_handle_conn, args=(conn, session), daemon=True).start()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        peer.close()
+        with contextlib.suppress(Exception):
+            session.close()
+
+
+if __name__ == "__main__":
+    main()