#!/usr/bin/env python3 """sanad_api_go2 — Go2 fleet TELEMETRY agent. Pushes the Unitree **Go2**'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":[] } Same shape/cadence as the R1 agent. The DIFFERENCE is the DDS family: * Go2 uses **unitree_go** (not unitree_hg). * Go2 battery lives INSIDE LowState_ as a nested ``bms_state`` (soc/current) — there is NO separate rt/lf/bmsstate topic like the G1/R1. So this agent reads battery straight from rt/lowstate.bms_state. * position (optional) from SportModeState (rt/lf/sportmodestate) or rosbridge /odom. ⚠️ UNVERIFIED ON HARDWARE: written from the unitree_go SDK layout but not yet run on a real Go2. Confirm the bms current sign (charging) and sportmodestate fields on the robot. Degrades to heartbeats if unitree_sdk2py is unavailable; --simulate tests the upload path without a robot. SAFETY: never commands motion (read-only). CONFIG — environment (see .env.example) --------------------------------------- SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required) SN fleet id default go2_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 GO2_POSITION_SOURCE none | sportmode | rosbridge default none 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 shutil 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_go2") 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 name: str brand: str robot_type: str model: str storage_path: str data_path: str dds_interface: str dds_domain: int mac_interface: str 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", "go2_0000"), name=_env("ROBOT_NAME", "") or _env("SN", "go2_0000"), brand=_env("ROBOT_BRAND", "unitree"), robot_type=_env("ROBOT_TYPE", "dog"), model=_env("ROBOT_MODEL", "go2"), 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), position_source=_env("GO2_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}"} 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)) _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 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 class DDSReader: """Subscribes rt/lowstate (unitree_go LowState_) and (optionally) sportmodestate. Battery comes from the nested LowState_.bms_state. Passive reads only.""" def __init__(self, cfg: Config): self.cfg = cfg self._lock = threading.Lock() self._bms: Optional[Dict[str, Any]] = None self._low_ts = 0.0 self._sport_ts = 0.0 self._temps: List[float] = [] self._max_dq = 0.0 self._xy: Optional[Dict[str, float]] = None self.ok = False self._start() def _start(self) -> None: try: from unitree_sdk2py.core.channel import ( ChannelFactoryInitialize, ChannelSubscriber) from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ SportModeState_ = None if self.cfg.position_source == "sportmode": try: 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 SportModeState_ is not None: self._sport_sub = ChannelSubscriber("rt/lf/sportmodestate", SportModeState_) self._sport_sub.Init(self._on_sport, 10) self.ok = True log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state)", self.cfg.dds_domain, self.cfg.dds_interface) except Exception as e: log.warning("DDS init failed (%s) — heartbeat mode", e) def _on_low(self, msg) -> None: try: bms = getattr(msg, "bms_state", None) or getattr(msg, "bms", None) if bms is not None: soc = int(getattr(bms, "soc", 0) or 0) cur = int(getattr(bms, "current", 0) or 0) # mA # Go2: pack voltage is LowState_.power_v (V); pack temp from the # BMS NTC sensors (bq_ntc / mcu_ntc, °C). All defensive getattrs. volt_v = None try: pv = getattr(msg, "power_v", None) if pv: volt_v = round(float(pv), 1) except Exception: volt_v = None temp_c = None try: ntc_vals = [] for attr in ("bq_ntc", "mcu_ntc"): nt = getattr(bms, attr, None) if nt is not None: vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)] ntc_vals.extend(v for v in vals if -40 <= v <= 150) if ntc_vals: temp_c = max(ntc_vals) except Exception: temp_c = None with self._lock: self._bms = { "soc": max(0, min(100, soc)), "current_a": round(cur / 1000.0, 2), "voltage_v": volt_v, "temp_c": temp_c, "soh": int(getattr(bms, "soh", 0) or 0), "cycles": int(getattr(bms, "cycle", 0) or 0), } 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)] # 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_ts = time.monotonic() self._temps = temps self._max_dq = max_dq except Exception: pass def _on_sport(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)} self._sport_ts = time.monotonic() 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, } 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) — 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]) -> str: 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 "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": 28.6, "temp_c": 36, "soh": 100, "cycles": 45}, "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0), "xy": sim.get("position")} else: snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0, "xy": 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) 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 = snap.get("xy") if position is None and pos is not None: position = pos.get() return { "sn": cfg.sn, "name": cfg.name, # friendly display name (e.g. go2_XX) "mac": mac, "brand": cfg.brand, "type": cfg.robot_type, # humanoid | dog "model": cfg.model, # r1 | g1 | go2 "battery": battery, "charging": charging, "battery_detail": battery_detail, "motor_temp": motor_temp, # null = not receiving "storage": read_storage(cfg), "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, "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="Go2 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_go2 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) 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())