468 lines
18 KiB
Python
468 lines
18 KiB
Python
#!/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())
|