#!/usr/bin/env python3 """sanad_api_r1 — R1 fleet TELEMETRY agent. Scope (this build): push the R1's live status to the YS Lootah fleet server. This is the "Main statuses" row of the fleet spec (NOT the map — the R1 map is skipped by request): POST {SERVER_URL}/api/v1/fleet/ingest/telemetry (Bearer device token) body: { "sn", "mac", "battery", "charging", "status", "position":{x,y}, "faults":[] } Sent every ~2 s. Per the spec: if state can't be read, still send a heartbeat so the robot stays "online". DATA SOURCES (Unitree R1 EDU, unitree_hg DDS — same family as the G1) -------------------------------------------------------------------- battery / charging : rt/lf/bmsstate (BmsState_) soc 0-100; charging = current>+0.05A (mirrors SanadR1 motion/arm_controller.get_battery) faults / liveness : rt/lowstate (LowState_) motor temps + message staleness status : R1 loco FSM via GET RPC 7001 (ids 0 ZeroTorque / 1 Damp / 4 Locked-Standing / 811 Gait-Running) — READ-ONLY, optional (R1_READ_FSM=1). Default derives status from BMS + motion. position {x,y} : OPTIONAL. R1 localizes with stereo VSLAM (ROS side); this agent has no ROS, so position is read over rosbridge /odom only when R1_POSITION_SOURCE=rosbridge, else omitted. mac : primary NIC hardware address. SAFETY: never commands motion. Only GET RPCs are ever issued to the R1. NO ROS. DDS via unitree_sdk2py (net=host + the robot interface). If unitree_sdk2py is unavailable it degrades to heartbeats. --simulate feeds synthetic state so the upload path is testable without a robot. CONFIG — environment (see .env.example) --------------------------------------- SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required) SN this robot's fleet id default r1_0000 DDS_INTERFACE robot network iface for DDS default eth0 DDS_DOMAIN DDS domain id default 0 MAC_INTERFACE iface whose MAC to report default = DDS_INTERFACE R1_READ_FSM 1 = read loco FSM for status default 0 R1_POSITION_SOURCE none | rosbridge default none ROSBRIDGE_URL ws://127.0.0.1:9090 (position) default ws://127.0.0.1:9090 LOW_SOC % below which -> LOW_BATTERY fault default 15 MOTOR_TEMP_MAX °C above which -> OVERTEMP fault default 85 POLL_INTERVAL seconds between telemetry posts default 2 VERIFY_TLS / HTTP_TIMEOUT TLS verify (1) / per-req timeout (10) CLI --- python sanad_api_r1.py # real DDS loop (default) python sanad_api_r1.py --simulate # synthetic state (no robot) — for testing python sanad_api_r1.py --once # one read+post, then exit python sanad_api_r1.py --dry-run # build telemetry, print it, never POST """ from __future__ import annotations import argparse import json import logging import math import os import shutil import socket import sys import threading import time import uuid from dataclasses import dataclass from pathlib import Path from typing import Any, Dict, List, Optional import requests log = logging.getLogger("sanad_api_r1") # --------------------------------------------------------------------------- # # env helpers # --------------------------------------------------------------------------- # def _load_dotenv(path: str = ".env") -> None: p = Path(path) if not p.exists(): return for line in p.read_text().splitlines(): line = line.strip() if not line or line.startswith("#") or "=" not in line: continue k, _, v = line.partition("=") os.environ.setdefault(k.strip(), v.strip().strip('"').strip("'")) def _env(name: str, default: str = "") -> str: return os.environ.get(name, default).strip() def _env_bool(name: str, default: bool) -> bool: return _env(name, "1" if default else "0").lower() in ("1", "true", "yes", "on") # --------------------------------------------------------------------------- # # config # --------------------------------------------------------------------------- # @dataclass class Config: server_url: str device_token: str sn: str name: str brand: str robot_type: str model: str storage_path: str data_path: str dds_interface: str dds_domain: int mac_interface: str read_fsm: bool position_source: str rosbridge_url: str low_soc: int motor_temp_max: float poll_interval: float endpoint: str verify_tls: bool http_timeout: float @classmethod def from_env(cls) -> "Config": server = _env("SERVER_URL").rstrip("/") token = _env("DEVICE_TOKEN") missing = [n for n, v in (("SERVER_URL", server), ("DEVICE_TOKEN", token)) if not v] if missing: raise SystemExit(f"[config] missing required env: {', '.join(missing)}") iface = _env("DDS_INTERFACE", "eth0") return cls( server_url=server, device_token=token, sn=_env("SN", "r1_0000"), name=_env("ROBOT_NAME", "") or _env("SN", "r1_0000"), brand=_env("ROBOT_BRAND", "unitree"), robot_type=_env("ROBOT_TYPE", "humanoid"), model=_env("ROBOT_MODEL", "r1"), storage_path=_env("STORAGE_PATH", ""), data_path=_env("STORAGE_DATA_PATH", ""), dds_interface=iface, dds_domain=int(_env("DDS_DOMAIN", "0")), mac_interface=_env("MAC_INTERFACE", iface), read_fsm=_env_bool("R1_READ_FSM", False), position_source=_env("R1_POSITION_SOURCE", "none").lower(), rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"), low_soc=int(_env("LOW_SOC", "15")), motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), poll_interval=float(_env("POLL_INTERVAL", "2")), endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), verify_tls=_env_bool("VERIFY_TLS", True), http_timeout=float(_env("HTTP_TIMEOUT", "10")), ) def telemetry_url(self) -> str: return self.server_url + self.endpoint def auth_headers(self) -> Dict[str, str]: return {"Authorization": f"Bearer {self.device_token}"} # --------------------------------------------------------------------------- # # mac address # --------------------------------------------------------------------------- # def read_mac(interface: str) -> str: """Stable hardware MAC. Prefer the named NIC (/sys), fall back to uuid.getnode. NOTE: with docker network_mode: host the container shares the host net namespace, so this is the real robot NIC MAC (not a virtual docker MAC).""" p = Path(f"/sys/class/net/{interface}/address") try: mac = p.read_text().strip() if mac and mac != "00:00:00:00:00:00": return mac.lower() except Exception: pass n = uuid.getnode() return ":".join(f"{(n >> b) & 0xff:02x}" for b in range(40, -1, -8)) # --------------------------------------------------------------------------- # # storage (host disk usage; mount / at /host:ro in docker) # --------------------------------------------------------------------------- # _data_size_cache: Dict[str, Any] = {"ts": 0.0, "kb": None} def read_storage(cfg: Config) -> Optional[Dict[str, Any]]: """Disk usage of the robot's root fs + optional Sanad data-dir size. In docker, bind-mount the host root read-only at /host (the installer does) so this reports the HOST disk, not the container overlay.""" root = cfg.storage_path or ("/host" if os.path.isdir("/host") else "/") try: du = shutil.disk_usage(root) out: Dict[str, Any] = { "total_gb": round(du.total / 1e9, 2), "free_gb": round(du.free / 1e9, 2), "used_percent": round(du.used / du.total * 100, 1), } except Exception: return None # data-dir size is a directory walk — cache it (refresh every 60 s) if cfg.data_path and os.path.isdir(cfg.data_path): now = time.monotonic() if _data_size_cache["kb"] is None or now - _data_size_cache["ts"] > 60: try: total = 0 for r, _, files in os.walk(cfg.data_path): for f in files: try: total += os.path.getsize(os.path.join(r, f)) except OSError: pass _data_size_cache.update(ts=now, kb=round(total / 1024, 1)) except Exception: pass if _data_size_cache["kb"] is not None: out["data_kb"] = _data_size_cache["kb"] return out # --------------------------------------------------------------------------- # # DDS reader (optional — degrades if unitree_sdk2py is absent) # --------------------------------------------------------------------------- # class DDSReader: """Subscribes rt/lf/bmsstate + rt/lowstate and (optionally) reads the loco FSM. All reads are passive; the only RPC ever issued is GET_FSM_ID.""" def __init__(self, cfg: Config): self.cfg = cfg self._lock = threading.Lock() self._bms: Optional[Dict[str, Any]] = None self._bms_ts = 0.0 self._low = None self._low_ts = 0.0 self._temps: List[float] = [] self._max_dq = 0.0 self._loco = None self.ok = False self._start() def _start(self) -> None: try: from unitree_sdk2py.core.channel import ( ChannelFactoryInitialize, ChannelSubscriber) from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ try: from unitree_sdk2py.idl.unitree_hg.msg.dds_ import BmsState_ except Exception: BmsState_ = None except Exception as e: log.warning("unitree_sdk2py unavailable (%s) — telemetry runs in heartbeat mode", e) return try: ChannelFactoryInitialize(self.cfg.dds_domain, self.cfg.dds_interface) self._low_sub = ChannelSubscriber("rt/lowstate", LowState_) self._low_sub.Init(self._on_low, 10) if BmsState_ is not None: self._bms_sub = ChannelSubscriber("rt/lf/bmsstate", BmsState_) self._bms_sub.Init(self._on_bms, 10) else: log.warning("BmsState_ not in this unitree_sdk2py — battery will be null") if self.cfg.read_fsm: self._init_loco() self.ok = True log.info("DDS up: domain=%d iface=%s (rt/lowstate + rt/lf/bmsstate)", self.cfg.dds_domain, self.cfg.dds_interface) except Exception as e: log.warning("DDS init failed (%s) — heartbeat mode", e) def _init_loco(self) -> None: """Loco client for READ-ONLY FSM id (GET RPC 7001). Never sends motion.""" try: from unitree_sdk2py.rpc.client import Client # type: ignore except Exception as e: log.warning("loco RPC client unavailable (%s) — status from BMS/motion only", e) return try: # R1 loco service ("loco"), GET_FSM_ID = 7001 (see R1 r1_loco_client). c = Client("loco", 0) c.Init() c.SetTimeout(3.0) self._loco = c log.info("loco FSM read enabled (GET-only, no motion)") except Exception as e: log.warning("loco client init failed (%s) — status from BMS/motion only", e) self._loco = None # -- callbacks -- def _on_bms(self, msg) -> None: try: soc = int(getattr(msg, "soc", 0) or 0) cur_mA = int(getattr(msg, "current", 0) or 0) # Pack voltage: prefer bmsvoltage[0] (mV); else sum of cell voltages. volt_mv = 0 bv = getattr(msg, "bmsvoltage", None) try: if bv is not None and len(bv) and int(bv[0]): volt_mv = int(bv[0]) except Exception: volt_mv = 0 if not volt_mv: cv = getattr(msg, "cell_vol", None) if cv is not None: try: volt_mv = int(sum(int(x) for x in cv if x)) except Exception: volt_mv = 0 # Max plausible pack temperature (int16 °C). temp_c = None tt = getattr(msg, "temperature", None) if tt is not None: try: vals = [int(x) for x in tt if -40 <= int(x) <= 150] if vals: temp_c = max(vals) except Exception: temp_c = None batt = { "soc": max(0, min(100, soc)), "current_a": round(cur_mA / 1000.0, 2), "voltage_v": round(volt_mv / 1000.0, 1) if volt_mv else None, "temp_c": temp_c, "soh": int(getattr(msg, "soh", 0) or 0), "cycles": int(getattr(msg, "cycle", 0) or 0), } with self._lock: self._bms = batt self._bms_ts = time.monotonic() except Exception: pass def _on_low(self, msg) -> None: try: temps: List[float] = [] max_dq = 0.0 ms = getattr(msg, "motor_state", None) or [] for m in ms: t = getattr(m, "temperature", None) if t is not None: try: # temperature may be a scalar or a small array (surface/winding) vals = [float(x) for x in t] if hasattr(t, "__iter__") else [float(t)] # 0 = slot not reporting (unpopulated motor), not a real temp temps.extend(v for v in vals if 0 < v <= 200) except Exception: pass dq = getattr(m, "dq", None) if dq is not None: try: max_dq = max(max_dq, abs(float(dq))) except Exception: pass with self._lock: self._low = msg self._low_ts = time.monotonic() self._temps = temps self._max_dq = max_dq except Exception: pass # -- reads -- def snapshot(self) -> Dict[str, Any]: with self._lock: now = time.monotonic() return { "bms": dict(self._bms) if self._bms else None, "bms_age": (now - self._bms_ts) if self._bms_ts else None, "low_age": (now - self._low_ts) if self._low_ts else None, "temps": list(self._temps), "max_dq": self._max_dq, } def fsm_id(self) -> Optional[int]: if not self._loco: return None try: code, data = self._loco._Call(7001, "{}") # GET_FSM_ID — read-only if code == 0 and data: return int(json.loads(data).get("data", json.loads(data)) if data.strip().startswith("{") else data) except Exception as e: log.debug("fsm read failed: %s", e) return None _FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "standing", 811: "ready"} # --------------------------------------------------------------------------- # # optional position over rosbridge (/odom) # --------------------------------------------------------------------------- # class RosbridgePosition: def __init__(self, cfg: Config): self.cfg = cfg self._xy: Optional[Dict[str, float]] = None self._lock = threading.Lock() self._stop = False try: import websocket # noqa: F401 (websocket-client) except Exception as e: log.warning("websocket-client absent (%s) — position disabled", e) self._ok = False return self._ok = True threading.Thread(target=self._run, daemon=True).start() def _run(self) -> None: import websocket sub = json.dumps({"op": "subscribe", "topic": "/odom", "type": "nav_msgs/Odometry", "throttle_rate": 500}) while not self._stop: try: ws = websocket.create_connection(self.cfg.rosbridge_url, timeout=5) ws.send(sub) while not self._stop: msg = json.loads(ws.recv()) pos = (((msg.get("msg") or {}).get("pose") or {}).get("pose") or {}).get("position") if pos: with self._lock: self._xy = {"x": round(float(pos["x"]), 3), "y": round(float(pos["y"]), 3)} except Exception as e: log.debug("rosbridge position reconnect: %s", e) time.sleep(3) def get(self) -> Optional[Dict[str, float]]: with self._lock: return dict(self._xy) if self._xy else None # --------------------------------------------------------------------------- # # telemetry assembly # --------------------------------------------------------------------------- # def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[str]: # STRINGS, not objects — the fleet ingest 500s on fault objects. faults: List[str] = [] bms = snap.get("bms") if bms and bms.get("soc", 100) <= cfg.low_soc: faults.append(f"LOW_BATTERY: battery {bms['soc']}% (warning)") temps = snap.get("temps") or [] if temps and max(temps) >= cfg.motor_temp_max: faults.append(f"MOTOR_OVERTEMP: motor temp {max(temps):.0f}C (warning)") if snap.get("low_age") is not None and snap["low_age"] > 3.0: faults.append(f"COMMS_STALE: no rt/lowstate for {snap['low_age']:.0f}s (critical)") return faults def derive_status(cfg: Config, snap: Dict[str, Any], fsm: Optional[int]) -> str: if fsm is not None and fsm in _FSM_STATUS: base = _FSM_STATUS[fsm] else: base = None bms = snap.get("bms") charging = bool(bms and bms.get("current_a", 0.0) > 0.05) alive = snap.get("low_age") is not None and snap["low_age"] <= 3.0 if not alive and bms is None: return "offline" if charging: return "charging" if snap.get("max_dq", 0.0) > 0.15: return "moving" return base or "idle" def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], pos: Optional[RosbridgePosition], sim: Optional[Dict[str, Any]] = None) -> Dict[str, Any]: if sim is not None: snap = {"bms": {"soc": sim["battery"], "current_a": 0.5 if sim["charging"] else -0.3, "voltage_v": 47.5, "temp_c": 36, "soh": 100, "cycles": 45}, "bms_age": 0.1, "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0)} fsm = sim.get("fsm") else: snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0} fsm = reader.fsm_id() if (reader and cfg.read_fsm) else None bms = snap.get("bms") battery = bms["soc"] if bms else None charging = bool(bms and bms.get("current_a", 0.0) > 0.05) status = derive_status(cfg, snap, fsm) faults = derive_faults(cfg, snap) # Battery detail (voltage / current / pack temp / health / cycles). battery_detail = None if bms: battery_detail = {"voltage_v": bms.get("voltage_v"), "current_a": bms.get("current_a"), "temp_c": bms.get("temp_c"), "soh": bms.get("soh"), "cycles": bms.get("cycles")} # Motor temperature stats; null = temps not receiving. temps = snap.get("temps") or [] motor_temp = ({"max": round(max(temps), 1), "avg": round(sum(temps) / len(temps), 1), "min": round(min(temps), 1)} if temps else None) position = None if sim is not None: position = sim.get("position") elif pos is not None: position = pos.get() payload: Dict[str, Any] = { "sn": cfg.sn, "name": cfg.name, # friendly display name (e.g. r1_82) "mac": mac, "brand": cfg.brand, "type": cfg.robot_type, # humanoid | dog "model": cfg.model, # r1 | g1 | go2 "battery": battery, # null = couldn't read (heartbeat) "charging": charging, "battery_detail": battery_detail, "motor_temp": motor_temp, # null = not receiving "storage": read_storage(cfg), "status": status, "position": position, # null when no localization source "faults": faults, "ts": int(time.time()), } return payload def post_telemetry(cfg: Config, payload: Dict[str, Any], session: requests.Session) -> bool: try: r = session.post(cfg.telemetry_url(), json=payload, headers=cfg.auth_headers(), timeout=cfg.http_timeout, verify=cfg.verify_tls) except requests.RequestException as e: log.error("telemetry POST failed (transport): %s", e) return False if not r.ok: log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200]) return False log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d -> HTTP %s", payload["battery"], payload["charging"], payload["status"], payload["position"], len(payload["faults"]), r.status_code) return True # --------------------------------------------------------------------------- # # main # --------------------------------------------------------------------------- # def _sim_state(i: int) -> Dict[str, Any]: """Deterministic-ish synthetic state that varies each tick (for testing).""" charging = (i % 6) in (0, 1) battery = max(5, 90 - (i % 40)) moving = (i % 3) == 2 and not charging return { "battery": battery, "charging": charging, "temp": 45 + (i % 10), "max_dq": 0.4 if moving else 0.0, "fsm": 811 if moving else 4, "position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}, } def main(argv: Optional[List[str]] = None) -> int: ap = argparse.ArgumentParser(description="R1 fleet telemetry agent") ap.add_argument("--simulate", action="store_true", help="synthetic state (no robot)") ap.add_argument("--once", action="store_true", help="one read+post, then exit") ap.add_argument("--dry-run", action="store_true", help="print telemetry, never POST") ap.add_argument("--interval", type=float, default=None) ap.add_argument("-v", "--verbose", action="store_true") args = ap.parse_args(argv) logging.basicConfig( level=logging.DEBUG if args.verbose else logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s") _load_dotenv() cfg = Config.from_env() if args.interval is not None: cfg.poll_interval = args.interval mac = read_mac(cfg.mac_interface) log.info("sanad_api_r1 telemetry — sn=%s mac=%s server=%s iface=%s domain=%d%s", cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain, " [SIMULATE]" if args.simulate else "") reader = None pos = None if not args.simulate: reader = DDSReader(cfg) if cfg.position_source == "rosbridge": pos = RosbridgePosition(cfg) time.sleep(1.0) # let first DDS messages land session = requests.Session() tick = 0 def one() -> None: nonlocal tick sim = _sim_state(tick) if args.simulate else None payload = build_telemetry(cfg, mac, reader, pos, sim=sim) if args.dry_run: log.info("[dry-run] %s", json.dumps(payload)) else: post_telemetry(cfg, payload, session) tick += 1 if args.once or (args.dry_run and args.once): one() return 0 if args.dry_run: for _ in range(3): one() time.sleep(min(cfg.poll_interval, 1.0)) return 0 log.info("loop every %.1fs (Ctrl-C to stop)", cfg.poll_interval) while True: try: one() except Exception as e: log.exception("tick failed: %s", e) try: time.sleep(cfg.poll_interval) except KeyboardInterrupt: log.info("stopped") return 0 if __name__ == "__main__": sys.exit(main())