530 lines
20 KiB
Python
530 lines
20 KiB
Python
#!/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 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
|
|
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"),
|
|
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))
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# 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)
|
|
batt = {
|
|
"soc": max(0, min(100, soc)),
|
|
"current_a": round(cur_mA / 1000.0, 2),
|
|
"soh": int(getattr(msg, "soh", 0) or 0),
|
|
"cycle": 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)]
|
|
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 = 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[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:
|
|
hot = max(temps)
|
|
if hot >= cfg.motor_temp_max:
|
|
faults.append({"code": "MOTOR_OVERTEMP", "severity": "warning",
|
|
"message": f"motor temp {hot:.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:
|
|
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},
|
|
"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)
|
|
|
|
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,
|
|
"mac": mac,
|
|
"battery": battery, # null = couldn't read (heartbeat)
|
|
"charging": charging,
|
|
"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())
|