diff --git a/PIPELINE.md b/PIPELINE.md index 6af5848..ef209f6 100644 --- a/PIPELINE.md +++ b/PIPELINE.md @@ -42,8 +42,8 @@ repo currently implements the **bold** ones; the rest are documented for later. | endpoint | method | agent | status | |---|---|---|---| -| **`/api/v1/fleet/ingest/telemetry`** | POST | R1, Go2 | ✅ implemented | -| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1 | ✅ implemented | +| **`/api/v1/fleet/ingest/telemetry`** | POST | G1 (`g1t`), R1, Go2 | ✅ implemented | +| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1 (`g1`) | ✅ implemented | | `/api/v1/fleet/ingest/{sn}/commands` | GET | — | ⏳ spec'd, not built | | `/api/v1/fleet/ingest/commands/{id}/ack` | POST | — | ⏳ | | `/api/v1/fleet/ingest/{sn}/alert` | POST | — | ⏳ (critical faults; ordinary ones ride in telemetry `faults[]`) | diff --git a/README.md b/README.md index a5ed89e..9c4cc13 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,10 @@ Project/fleet/ │ ├── requirements.txt │ ├── docker-compose.yml (manual local use only) │ └── .env.example + ├── g1t/ ← TELEMETRY (G1, unitree_hg) + │ ├── sanad_api_g1t.py + │ ├── Dockerfile (DDS) + vendor/ + │ └── .env.example ├── r1/ ← TELEMETRY (unitree_hg) │ ├── sanad_api_r1.py │ ├── Dockerfile (DDS: CycloneDDS + unitree_sdk2py) @@ -80,11 +84,17 @@ agent to the robot; you never edit files on the robot. ## 3. The three agents -| agent | robot | endpoint | what it sends | cadence | +| agent (type) | robot | endpoint | what it sends | cadence | |---|---|---|---|---| -| `sanad_api_g1` | Unitree G1 | `POST /api/v1/fleet/ingest/{sn}/map` | RTAB-Map `.db` + places (points) | on change (~30 s scan) | -| `sanad_api_r1` | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | battery, charging, status, position, faults, mac | every ~2 s | -| `sanad_api_go2` | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same as R1 | every ~2 s | +| `sanad_api_g1` (`g1`) | Unitree G1 | `POST /api/v1/fleet/ingest/{sn}/map` | RTAB-Map `.db` + places (points) | on change (~30 s scan) | +| `sanad_api_g1t` (`g1t`) | Unitree G1 | `POST /api/v1/fleet/ingest/telemetry` | battery, charging, status, position, faults, mac | every ~2 s | +| `sanad_api_r1` (`r1`) | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | +| `sanad_api_go2` (`go2`) | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | + +The G1 runs **both** `g1` (map) and `g1t` (telemetry) — two independent +containers/services, both keyed by the **same `sn`** (same physical robot). `g1t` +uses the same `unitree_hg` DDS as R1, and reads `position` from the firmware odom +topic (`rt/lf/odommodestate`) over DDS. All requests carry `Authorization: Bearer `. Full payload schemas and the data pipeline are in [PIPELINE.md](PIPELINE.md). @@ -345,11 +355,11 @@ bridge). ## 12. Fleet inventory -| robot | agent | IP | SSH | arch | DDS iface | SN | +| robot | agent(s) | IP | SSH | arch | DDS iface | SN | |---|---|---|---|---|---|---| -| G1 | map | `10.255.254.58` | `unitree` (key) | arm64 | — | `g1_7892` | -| R1 | telemetry | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `r1_82` | -| Go2 | telemetry | *(TBD)* | `unitree` | arm64 | `eth0` | — | +| G1 | map (`g1`) + telemetry (`g1t`) | `10.255.254.58` | `unitree` (key) | arm64 | `eth0` | `g1_7892` | +| R1 | telemetry (`r1`) | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `r1_82` | +| Go2 | telemetry (`go2`) | *(TBD)* | `unitree` | arm64 | `eth0` | — | Workstation (deploy host + test fleet server): **`10.255.254.83`** (`wlp4s0`). diff --git a/agents/g1t/.dockerignore b/agents/g1t/.dockerignore new file mode 100644 index 0000000..bce1cad --- /dev/null +++ b/agents/g1t/.dockerignore @@ -0,0 +1,4 @@ +.env +__pycache__/ +*.pyc +README.md diff --git a/agents/g1t/.env.example b/agents/g1t/.env.example new file mode 100644 index 0000000..546c5ce --- /dev/null +++ b/agents/g1t/.env.example @@ -0,0 +1,35 @@ +# sanad_api_g1t — copy to .env and fill in. (G1 telemetry; sibling of the G1 map agent.) + +# ── fleet server (REQUIRED — YS Lootah gives you these two) ────────────────── +SERVER_URL=https://fleet.example.com +DEVICE_TOKEN=REPLACE_WITH_DEVICE_TOKEN + +# ── identity ───────────────────────────────────────────────────────────────── +# Use the SAME sn as the G1 map agent — it's the same physical robot. +SN=g1_7892 + +# ── DDS (reading robot state — unitree_hg, same family as R1) ──────────────── +# G1 DDS link is usually eth0. +DDS_INTERFACE=eth0 +DDS_DOMAIN=0 +# MAC_INTERFACE=eth0 + +# ── status / position ──────────────────────────────────────────────────────── +# Read the loco FSM for a richer status (READ-ONLY GET RPC, never moves). +# 0 = derive status from battery + joint motion (safe default). +G1_READ_FSM=0 +# Position source: +# odom (default) — firmware leg-odometry x,y from rt/lf/odommodestate (drifts, resets at boot) +# rosbridge — map-frame /odom over rosbridge (needs the nav bringup + ROSBRIDGE_URL) +# none — omit position +G1_POSITION_SOURCE=odom +ROSBRIDGE_URL=ws://127.0.0.1:9090 + +# ── fault thresholds ───────────────────────────────────────────────────────── +LOW_SOC=15 +MOTOR_TEMP_MAX=85 + +# ── cadence / transport ────────────────────────────────────────────────────── +POLL_INTERVAL=2 +VERIFY_TLS=1 +HTTP_TIMEOUT=10 diff --git a/agents/g1t/.gitignore b/agents/g1t/.gitignore new file mode 100644 index 0000000..cff5543 --- /dev/null +++ b/agents/g1t/.gitignore @@ -0,0 +1,3 @@ +.env +__pycache__/ +*.pyc diff --git a/agents/g1t/Dockerfile b/agents/g1t/Dockerfile new file mode 100644 index 0000000..931fa83 --- /dev/null +++ b/agents/g1t/Dockerfile @@ -0,0 +1,46 @@ +# sanad_api_g1t — G1 fleet telemetry agent (DDS: rt/lf/bmsstate + rt/lowstate + odom). +# +# BUILD ON THE G1 JETSON (native arm64). DDS stack: prebuilt CycloneDDS (apt) + +# the CycloneDDS python binding (pip) + the vendored unitree_sdk2py wheel. Run with +# --network host so the robot's DDS is visible (see docker-compose.yml). +FROM python:3.10-slim-bookworm + +# CycloneDDS C lib + idlc (bookworm ships 0.10.2 — matches the robot), build tools +# for the python binding, iproute2 for iface checks, libgomp1 for the TLS guard. +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake \ + cyclonedds-dev cyclonedds-tools \ + iproute2 libgomp1 ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +# aarch64 "static TLS block" guard (harmless on amd64). +ENV LD_PRELOAD=libgomp.so.1 +# cyclonedds build helper looks for libddsc.so under $CYCLONEDDS_HOME/lib; Debian +# installs into the multiarch dir — symlink whatever arch built into /usr/lib. +ENV CYCLONEDDS_HOME=/usr +RUN set -e; for lib in libddsc.so libcycloneddsidl.so; do \ + f=$(ls /usr/lib/*/"$lib" 2>/dev/null | head -1); \ + [ -n "$f" ] && ln -sf "$f" /usr/lib/"$lib" || true; \ + done + +WORKDIR /app +COPY requirements.txt . +RUN pip install --no-cache-dir -r requirements.txt +# CycloneDDS python binding, compiled against the C lib above. 0.10.2's build +# helper imports wheel.bdist_wheel (removed in wheel>=0.46) → pin the toolchain. +RUN pip install --no-cache-dir "setuptools<80" "wheel<0.46" \ + && pip install --no-cache-dir --no-build-isolation cyclonedds==0.10.2 + +# Unitree SDK (vendored wheel — not on PyPI) + native crc lib (wheel omits it). +# Both arch crc libs are copied; unitree_sdk2py loads the one matching the image. +COPY vendor/unitree_sdk2py-*.whl /tmp/ +RUN pip install --no-cache-dir --no-deps /tmp/unitree_sdk2py-*.whl +COPY vendor/crc_aarch64.so vendor/crc_amd64.so \ + /usr/local/lib/python3.10/site-packages/unitree_sdk2py/utils/lib/ + +COPY sanad_api_g1t.py . + +ENV PYTHONUNBUFFERED=1 \ + DDS_INTERFACE=eth0 \ + DDS_DOMAIN=0 +ENTRYPOINT ["python", "-u", "sanad_api_g1t.py"] diff --git a/agents/g1t/README.md b/agents/g1t/README.md new file mode 100644 index 0000000..212833f --- /dev/null +++ b/agents/g1t/README.md @@ -0,0 +1,40 @@ +# sanad_api_g1t — G1 fleet **telemetry** agent + +Streams the Unitree **G1**'s live status to the YS Lootah fleet server. It's the +**telemetry** sibling of the G1 **map** agent (`../g1`) — the G1 runs both, each +its own container/service, both keyed by the **same `sn`** (same physical robot). + +``` +POST {SERVER_URL}/api/v1/fleet/ingest/telemetry Authorization: Bearer +{ "sn":"g1_7892", "mac":"…", "battery":74, "charging":false, + "status":"idle", "position":{"x":…,"y":…}|null, "faults":[], "ts":… } +``` + +Every ~2 s; a **heartbeat** (`battery:null, status:offline`) when DDS is silent. + +## Data sources (G1, `unitree_hg` DDS — same family as R1) + +| field | source | +|---|---| +| `battery`, `charging` | `rt/lf/bmsstate` (`BmsState_`): `soc` 0–100; charging from `current` sign | +| `faults[]` | `rt/lowstate` (`LowState_`): motor temps + staleness | +| `position` `{x,y}` | **`rt/lf/odommodestate`** (`SportModeState_`) — firmware leg-odometry over DDS, no ROS (odom frame: drifts, resets at boot). Optional map-frame via `rosbridge`. | +| `status` | derived (charging/moving/idle/offline); optional loco **FSM** (`G1_READ_FSM=1`, ids **200** ready / 4 stand / 2 squat / 702 lie2stand — GET-only) | +| `mac` | primary NIC | + +> **Safety:** read-only — never commands motion. + +## Run / test + +```bash +# via the fleet installer (adds a 'g1t' type): +../../fleet_install.sh install g1t --sn g1_7892 +../../fleet_install.sh data g1t + +# bare simulate (no robot): +SERVER_URL=… DEVICE_TOKEN=… SN=g1_7892 python3 sanad_api_g1t.py --simulate -v +``` + +Build on the G1 Jetson (arm64); bundles CycloneDDS + the vendored `unitree_sdk2py` +wheel (`vendor/`). `network_mode: host` for DDS. Coexists with the map agent and +with a running Sanad (DDS allows many readers of the same topics). diff --git a/agents/g1t/docker-compose.yml b/agents/g1t/docker-compose.yml new file mode 100644 index 0000000..470080d --- /dev/null +++ b/agents/g1t/docker-compose.yml @@ -0,0 +1,20 @@ +# sanad_api_g1t — standalone G1 fleet TELEMETRY agent. +# (The fleet installer deploys via docker build/run + systemd; this compose is +# for manual local use.) +# +# cp .env.example .env # SERVER_URL + DEVICE_TOKEN + SN + DDS_INTERFACE +# docker compose up -d --build +# +# Reads the G1's DDS state (rt/lf/bmsstate + rt/lowstate + rt/lf/odommodestate) +# and POSTs telemetry every ~2s. network_mode: host is REQUIRED for DDS. +services: + sanad-api-g1t: + build: + context: . + dockerfile: Dockerfile + image: "${SANAD_API_G1T_IMAGE:-sanad-api-g1t:latest}" + container_name: sanad-api-g1t + network_mode: host + restart: unless-stopped + env_file: .env + # command: ["--simulate"] # synthetic state, no robot diff --git a/agents/g1t/requirements.txt b/agents/g1t/requirements.txt new file mode 100644 index 0000000..b462572 --- /dev/null +++ b/agents/g1t/requirements.txt @@ -0,0 +1,7 @@ +# sanad_api_r1 telemetry agent. +requests>=2.31,<3 +# only needed if R1_POSITION_SOURCE=rosbridge (reads /odom for position): +websocket-client>=1.6,<2 +# DDS stack (cyclonedds + the vendored unitree_sdk2py wheel) is installed by the +# Dockerfile, not from here — see Dockerfile. The agent degrades to heartbeats if +# unitree_sdk2py is unavailable. diff --git a/agents/g1t/sanad_api_g1t.py b/agents/g1t/sanad_api_g1t.py new file mode 100644 index 0000000..140be64 --- /dev/null +++ b/agents/g1t/sanad_api_g1t.py @@ -0,0 +1,467 @@ +#!/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()) diff --git a/agents/g1t/vendor/crc_aarch64.so b/agents/g1t/vendor/crc_aarch64.so new file mode 100644 index 0000000..878df74 Binary files /dev/null and b/agents/g1t/vendor/crc_aarch64.so differ diff --git a/agents/g1t/vendor/crc_amd64.so b/agents/g1t/vendor/crc_amd64.so new file mode 100644 index 0000000..258055d Binary files /dev/null and b/agents/g1t/vendor/crc_amd64.so differ diff --git a/agents/g1t/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl b/agents/g1t/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl new file mode 100644 index 0000000..3231178 Binary files /dev/null and b/agents/g1t/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl differ diff --git a/diagram.svg b/diagram.svg index bda05d5..af87dec 100644 --- a/diagram.svg +++ b/diagram.svg @@ -25,10 +25,10 @@ - G1 — map uploader - source: navigation map files - web_nav3 maps/*.db + places - maps_meta.json + G1 — map + telemetry + map: web_nav3 .db + places (files) + telem: rt/lf/bmsstate · rt/lowstate + position: rt/lf/odommodestate diff --git a/fleet_install.sh b/fleet_install.sh index 7c3881a..60de8a0 100755 --- a/fleet_install.sh +++ b/fleet_install.sh @@ -22,13 +22,15 @@ set -uo pipefail SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" AGENTS="$SCRIPT_DIR/agents" -TYPES="g1 r1 go2" -declare -A KIND=( [g1]="MAP uploader" [r1]="TELEMETRY (unitree_hg)" [go2]="TELEMETRY (unitree_go)" ) +TYPES="g1 g1t r1 go2" +declare -A KIND=( [g1]="MAP uploader" [g1t]="TELEMETRY (G1, unitree_hg)" [r1]="TELEMETRY (unitree_hg)" [go2]="TELEMETRY (unitree_go)" ) die(){ echo "ERROR: $*" >&2; exit 1; } img_of(){ echo "sanad-api-$1"; } # image + container + unit share this base rdir_of(){ echo "sanad_api_$1"; } unit_of(){ echo "sanad-api-$1.service"; } +# G1 map + G1 telemetry are the SAME physical robot → share one sn. +sn_default(){ case "$1" in g1|g1t) echo "g1_${IP##*.}";; *) echo "${1}_${IP##*.}";; esac; } SSH_OPTS=(-o BatchMode=yes -o ConnectTimeout=8 -o StrictHostKeyChecking=accept-new) rmt(){ ssh -n "${SSH_OPTS[@]}" "$USER_@$IP" "$@"; } # -n: never read our stdin @@ -209,10 +211,12 @@ CMD="${POSA[0]:-}"; ROBOT="${POSA[1]:-}"; IP="${POSA[2]:-}" # --------------------------- interactive --------------------------- # choose_type(){ - echo "Which robot?" >&2; local i=1 - for t in $TYPES; do echo " $i) $t — ${KIND[$t]}" >&2; i=$((i+1)); done - read -rp "Choice [1-3]: " c - case "$c" in 1) echo g1;; 2) echo r1;; 3) echo go2;; g1|r1|go2) echo "$c";; *) echo "";; esac + echo "Which robot / agent?" >&2; local i=1; local -a arr=() + for t in $TYPES; do echo " $i) $t — ${KIND[$t]}" >&2; arr[$i]="$t"; i=$((i+1)); done + read -rp "Choice: " c + if [[ "$c" =~ ^[0-9]+$ ]] && [ -n "${arr[$c]:-}" ]; then echo "${arr[$c]}" + elif echo "$TYPES" | grep -qw "$c"; then echo "$c" + else echo ""; fi } interactive(){ @@ -239,7 +243,7 @@ interactive(){ } prompt_install(){ - local def="${ROBOT}_${IP##*.}" + local def; def="$(sn_default "$ROBOT")" read -rp "Robot name / SN [$def]: " s; SN="${s:-$def}" local autos; autos="$(detect_server_ip)" read -rp "Fleet server IP [${autos:-required}]: " si; [ -n "$si" ] && SERVER_IP="$si" || SERVER_IP="${SERVER_IP:-$autos}" @@ -252,7 +256,7 @@ prompt_install(){ [ -n "$ROBOT" ] && [ -n "$IP" ] || die "usage: $0 [opts] (or run with no args for interactive)" echo "$TYPES" | grep -qw "$ROBOT" || die "robot must be one of: $TYPES" [ -d "$AGENTS/$ROBOT" ] || die "agent dir missing: $AGENTS/$ROBOT" -[ -z "$SN" ] && SN="${ROBOT}_${IP##*.}" +[ -z "$SN" ] && SN="$(sn_default "$ROBOT")" case "$CMD" in install) do_install "$ROBOT";;