#!/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 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 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"), 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)) 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 with self._lock: self._bms = {"soc": max(0, min(100, soc)), "current_a": round(cur / 1000.0, 2)} 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_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}, "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) 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, "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())