#!/usr/bin/env python3 """sanad_api_g1t — G1 fleet TELEMETRY agent. Pushes the Unitree **G1**'s live status to the YS Lootah fleet server: POST {SERVER_URL}/api/v1/fleet/ingest/telemetry (Bearer device token) body: { "sn", "mac", "battery", "charging", "status", "position":{x,y}, "faults":[] } This is the telemetry sibling of the G1 MAP uploader (sanad_api_g1) — the G1 runs both, each its own container/service, both keyed by the SAME `sn` (same robot). DATA SOURCES (Unitree G1, unitree_hg DDS — same family as the R1) ---------------------------------------------------------------- battery / charging : rt/lf/bmsstate (BmsState_) soc 0-100; charging = current>+0.05A faults / liveness : rt/lowstate (LowState_) motor temps + message staleness position {x,y} : rt/lf/odommodestate (SportModeState_, unitree_go idl) — firmware leg-odometry x,y over DDS, no ROS. Odom frame: resets at boot, drifts. (Map-frame via rosbridge/TF optional.) status : G1 loco FSM via GET RPC 7001 (ids 200 walk-ready / 4 stand / 2 squat / 702 lie2stand) — READ-ONLY, optional (G1_READ_FSM=1). Default derives status from BMS + motion. mac : primary NIC hardware address. NO ROS. DDS via unitree_sdk2py. Degrades to heartbeats if unitree_sdk2py is unavailable; --simulate tests the upload path without a robot. Read-only: never commands motion (only the GET_FSM_ID RPC is ever issued). CONFIG — environment (see .env.example) --------------------------------------- SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required) SN this robot's fleet id (SAME as the G1 map agent) default g1_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 G1_READ_FSM 1 = read loco FSM for status default 0 G1_POSITION_SOURCE odom | rosbridge | none default odom ROSBRIDGE_URL ws://127.0.0.1:9090 (position) default ws://127.0.0.1:9090 LOW_SOC / MOTOR_TEMP_MAX fault thresholds default 15 / 85 POLL_INTERVAL seconds between posts default 2 VERIFY_TLS / HTTP_TIMEOUT TLS verify (1) / timeout (10) CLI: --simulate | --once | --dry-run | --interval N | -v """ from __future__ import annotations import argparse import json import logging import os 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_g1t") 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") @dataclass class Config: server_url: str device_token: str sn: 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", "g1_0000"), dds_interface=iface, dds_domain=int(_env("DDS_DOMAIN", "0")), mac_interface=_env("MAC_INTERFACE", iface), read_fsm=_env_bool("G1_READ_FSM", False), position_source=_env("G1_POSITION_SOURCE", "odom").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}"} def read_mac(interface: str) -> str: 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)) class DDSReader: """Subscribes rt/lf/bmsstate + rt/lowstate (+ rt/lf/odommodestate for position). Passive reads; 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_ts = 0.0 self._temps: List[float] = [] self._max_dq = 0.0 self._xy: Optional[Dict[str, float]] = None 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 SportModeState_ = None if self.cfg.position_source == "odom": try: # G1 firmware publishes odom as unitree_go SportModeState_. from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ except Exception: SportModeState_ = 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 SportModeState_ is not None: self._odom_sub = ChannelSubscriber("rt/lf/odommodestate", SportModeState_) self._odom_sub.Init(self._on_odom, 10) if self.cfg.read_fsm: self._init_loco() self.ok = True log.info("DDS up: domain=%d iface=%s (bmsstate + lowstate%s)", self.cfg.dds_domain, self.cfg.dds_interface, " + odom" if SportModeState_ is not None else "") except Exception as e: log.warning("DDS init failed (%s) — heartbeat mode", e) def _init_loco(self) -> None: 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: 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 def _on_bms(self, msg) -> None: try: soc = int(getattr(msg, "soc", 0) or 0) cur_mA = int(getattr(msg, "current", 0) or 0) with self._lock: self._bms = {"soc": max(0, min(100, soc)), "current_a": round(cur_mA / 1000.0, 2)} self._bms_ts = time.monotonic() except Exception: pass def _on_low(self, msg) -> None: try: temps: List[float] = [] max_dq = 0.0 for m in (getattr(msg, "motor_state", None) or []): t = getattr(m, "temperature", None) if t is not None: try: vals = [float(x) for x in t] if hasattr(t, "__iter__") else [float(t)] temps.extend(v for v in vals if -40 <= 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_ts = time.monotonic() self._temps = temps self._max_dq = max_dq except Exception: pass def _on_odom(self, msg) -> None: try: pos = getattr(msg, "position", None) if pos is not None and len(pos) >= 2: with self._lock: self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)} except Exception: pass def snapshot(self) -> Dict[str, Any]: with self._lock: now = time.monotonic() return { "bms": dict(self._bms) if self._bms else None, "low_age": (now - self._low_ts) if self._low_ts else None, "temps": list(self._temps), "max_dq": self._max_dq, "xy": dict(self._xy) if self._xy else None, } 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", data)) if data.strip().startswith("{") else int(data) except Exception as e: log.debug("fsm read failed: %s", e) return None # G1 FSM ids (differ from the R1's): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand. _FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"} 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 except Exception as e: log.warning("websocket-client absent (%s) — rosbridge 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 def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]: faults: List[Dict[str, Any]] = [] bms = snap.get("bms") if bms and bms.get("soc", 100) <= cfg.low_soc: faults.append({"code": "LOW_BATTERY", "severity": "warning", "message": f"battery {bms['soc']}%"}) temps = snap.get("temps") or [] if temps and max(temps) >= cfg.motor_temp_max: faults.append({"code": "MOTOR_OVERTEMP", "severity": "warning", "message": f"motor temp {max(temps):.0f}C"}) if snap.get("low_age") is not None and snap["low_age"] > 3.0: faults.append({"code": "COMMS_STALE", "severity": "critical", "message": f"no rt/lowstate for {snap['low_age']:.0f}s"}) return faults def derive_status(cfg: Config, snap: Dict[str, Any], fsm: Optional[int]) -> str: base = _FSM_STATUS.get(fsm) if fsm is not None else 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}, "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0), "xy": sim.get("position")} fsm = sim.get("fsm") else: snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0, "xy": None} 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) position = snap.get("xy") if position is None and pos is not None: position = pos.get() return { "sn": cfg.sn, "mac": mac, "battery": battery, "charging": charging, "status": status, "position": position, "faults": faults, "ts": int(time.time()), } 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 def _sim_state(i: int) -> Dict[str, Any]: 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": 200 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="G1 fleet telemetry agent") ap.add_argument("--simulate", action="store_true") ap.add_argument("--once", action="store_true") ap.add_argument("--dry-run", action="store_true") 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_g1t telemetry — sn=%s mac=%s server=%s iface=%s domain=%d pos=%s%s", cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain, cfg.position_source, " [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) 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: 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())