commit db757d0b55a7403e2ff1a76f843d6c6fbce98b1c Author: kassam Date: Fri Jul 10 11:24:55 2026 +0400 Update 2026-07-10 11:24:53 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c02264b --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +*.pyc +Logs/ +*.log diff --git a/README.md b/README.md new file mode 100644 index 0000000..d813ce9 --- /dev/null +++ b/README.md @@ -0,0 +1,94 @@ +# Sanad Fleet Agents + +Per-robot agents that report to the YS Lootah fleet server, each shipped as +**full Docker** with a user-level **systemd auto-start service**. Deploy, manage, +and remove them over SSH with one interactive script. + +``` +Project/fleet/ + fleet_install.sh # interactive + scriptable deploy/manage tool + fleet_test_server.py # your workstation standing in as the fleet server (tests) + agents/ + g1/ sanad_api_g1.py — MAP uploader (POST …/{sn}/map) + r1/ sanad_api_r1.py — TELEMETRY unitree_hg (POST …/telemetry) + go2/ sanad_api_go2.py — TELEMETRY unitree_go (POST …/telemetry) [unverified on hw] +``` + +All three are self-contained (own `Dockerfile`, `.env.example`). Deploy uses +plain `docker build`/`docker run` — **no docker-compose needed on the robot** +(R1/Go2 don't have it). Source is pushed from this canonical workstation copy via +`rsync`; images build natively on each arm64 robot; a **user systemd unit** +(linger-enabled, **no sudo**) owns start/stop/auto-start-on-boot. + +## Install / manage + +Interactive (asks robot type → IP → detects installed-or-not → right actions): + +```bash +./fleet_install.sh +``` + +Scriptable: + +```bash +./fleet_install.sh install [--sn NAME] [--server-ip IP] [--port N] [--token TOK] +./fleet_install.sh data # show what it's currently sending +./fleet_install.sh status +./fleet_install.sh logs +./fleet_install.sh test # end-to-end vs workstation server +./fleet_install.sh uninstall # service + container + image + dir +``` + +`install`/`uninstall` are detected automatically in interactive mode: if the +agent is already on the robot it offers **data / status / logs / reinstall / +uninstall**; otherwise it prompts for **robot name (SN)**, server IP, port, token +and installs. + +### Auto-start service + +Each install writes `~/.config/systemd/user/sanad-api-.service`, enables +linger (`loginctl enable-linger`, no sudo), and `systemctl --user enable --now`s +it. It starts on boot and restarts on crash. Manage with: + +```bash +ssh unitree@ 'systemctl --user status sanad-api-r1' +ssh unitree@ 'systemctl --user restart sanad-api-r1' +``` + +## Fleet (this deployment) + +| robot | agent | ip | ssh user | arch | DDS iface | +|---|---|---|---|---|---| +| G1 | map | `10.255.254.58` | `unitree` (key) | arm64 | — | +| R1 | telemetry | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | +| Go2 | telemetry | *(when available)* | `unitree` | arm64 | `eth0` | + +Workstation fleet server (for tests): **10.255.254.83** via `fleet_test_server.py`. + +## What each agent sends, and when + +**G1 map** → `POST /api/v1/fleet/ingest/{sn}/map` — on change (scan ~30 s; +size+mtime→sha256). Sends the RTAB-Map `.db` + places as `multipart` (file `db` + +`meta` JSON `{sn,name,format:"rtabmap_db",size_bytes,sha256,points[…]}`) or +`base64json`. Reads web_nav3 `maps//*.db` + `web/data//places/*.json`. + +**R1 / Go2 telemetry** → `POST /api/v1/fleet/ingest/telemetry` — every ~2 s; +heartbeat (`battery:null,status:offline`) when DDS is silent so it stays online: +```json +{ "sn":"r1_82", "mac":"…", "battery":74, "charging":false, + "status":"idle", "position":{"x":…,"y":…}|null, "faults":[], "ts":… } +``` +- R1 (`unitree_hg`): battery `rt/lf/bmsstate`, faults/liveness `rt/lowstate`. +- Go2 (`unitree_go`): battery from **`rt/lowstate.bms_state`** (nested), no separate BMS topic. +- Both: `status` derived (charging/moving/idle/offline); position optional; read-only (never moves). + +Auth on every request: `Authorization: Bearer `. + +## Notes / follow-ups + +- **Point at the real fleet server:** re-run `install … --server-ip --token --sn `. +- **G1 real maps** live *inside* the `p4_Foxy_sanad` nav container (not host-mounted). + Add a `maps/` bind-mount to Package_4's nav service, or share a volume, so the + uploader sees real maps (it reports 0 until then; `test` seeds a fixture). +- **Go2** agent is written from the `unitree_go` SDK layout but **not yet run on a + Go2** — confirm the `bms_state` current sign (charging) and sportmodestate fields. diff --git a/agents/g1/.dockerignore b/agents/g1/.dockerignore new file mode 100644 index 0000000..ed287fb --- /dev/null +++ b/agents/g1/.dockerignore @@ -0,0 +1,5 @@ +.env +data/ +__pycache__/ +*.pyc +README.md diff --git a/agents/g1/.env.example b/agents/g1/.env.example new file mode 100644 index 0000000..306ae7f --- /dev/null +++ b/agents/g1/.env.example @@ -0,0 +1,43 @@ +# sanad_api_g1 — copy to .env and fill in. docker compose reads it automatically. + +# ── fleet server (REQUIRED — YS Lootah gives you these two) ────────────────── +SERVER_URL=https://fleet.example.com +DEVICE_TOKEN=REPLACE_WITH_DEVICE_TOKEN + +# ── identity ───────────────────────────────────────────────────────────────── +# This robot's fleet id — used as {sn} in POST /api/v1/fleet/ingest/{sn}/map. +SN=g1_7892 +# web_nav3 robot name: the maps subdir (//*.db) + X-Robot-Name. +# Must match web_nav3's robot_config.yaml robot_name (default: sanad). +ROBOT=sanad + +# ── where the map files live on the HOST (bind-mounted read-only) ──────────── +# Point these at the robot's web_nav3 install. Defaults assume a Package_4 +# robot (nav container -> /home/unitree/marcus_nav2_test). For the workstation +# dev copy use .../Project/G1/Nav2_Projects/web_nav3/{maps,web/data}. +MAPS_HOST_DIR=/home/unitree/marcus_nav2_test/maps +DATA_HOST_DIR=/home/unitree/marcus_nav2_test/web/data +STATE_HOST_DIR=./data/state + +# Optional legacy per-robot places file (older sanad setups). Leave blank if +# each map already has its own places under DATA_DIR//places/. +# LEGACY_PLACES=/data/legacy/places.json + +# ── behaviour ──────────────────────────────────────────────────────────────── +# Which maps to upload: all | active | newest. active/newest need WEB_NAV3_URL. +MAP_SELECT=all +# Optional — only used to learn which map is ACTIVE (MAP_SELECT=active). +WEB_NAV3_URL=http://127.0.0.1:8765 + +# Wire format the server accepts on the map endpoint: +# multipart -> multipart/form-data: file field `db` + form field `meta` (JSON) +# base64json -> JSON body with the .db as `db_base64` + the same meta fields +MAP_UPLOAD_MODE=multipart +# Endpoint path template ({sn} substituted). Change only if the server differs. +MAP_ENDPOINT=/api/v1/fleet/ingest/{sn}/map + +# Scan cadence for the loop (seconds). Maps rarely change, so 30s is plenty. +POLL_INTERVAL=30 +# Verify the fleet server's TLS cert (1 recommended; 0 only for self-signed dev). +VERIFY_TLS=1 +HTTP_TIMEOUT=30 diff --git a/agents/g1/.gitignore b/agents/g1/.gitignore new file mode 100644 index 0000000..b6bde21 --- /dev/null +++ b/agents/g1/.gitignore @@ -0,0 +1,4 @@ +.env +data/ +__pycache__/ +*.pyc diff --git a/agents/g1/Dockerfile b/agents/g1/Dockerfile new file mode 100644 index 0000000..2162f1b --- /dev/null +++ b/agents/g1/Dockerfile @@ -0,0 +1,19 @@ +# sanad_api_g1 — G1 fleet MAP uploader. Self-contained, no ROS. +FROM python:3.10-slim-bookworm + +# ca-certificates so outbound HTTPS to the fleet server validates. +RUN apt-get update \ + && apt-get install -y --no-install-recommends ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /app +COPY requirements.txt . +RUN pip install --no-cache-dir -r requirements.txt +COPY sanad_api_g1.py . + +# Writable state (upload fingerprints) — override with a volume in compose. +ENV STATE_DIR=/data/state +RUN mkdir -p /data/state + +# Run as the loop by default; override the CMD for --once / --list / --dry-run. +ENTRYPOINT ["python", "-u", "sanad_api_g1.py"] diff --git a/agents/g1/README.md b/agents/g1/README.md new file mode 100644 index 0000000..46539c6 --- /dev/null +++ b/agents/g1/README.md @@ -0,0 +1,130 @@ +# sanad_api_g1 — G1 fleet **map** uploader + +A tiny, self-contained agent that pushes the G1's navigation **map** to the +YS Lootah fleet server. One robot type = one folder = its own Docker image, so +it drops onto any new G1 unchanged. (Sibling folders `sanad_api_r1`, +`sanad_api_go2` will do the same for those robots.) + +This build covers **only the map** — the "Maps sync" row of the fleet spec: + +``` +POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map Authorization: Bearer +``` + +Telemetry (battery/status/position/faults), commands, alerts and logs are +separate agents and are **not** in this script. + +--- + +## What it uploads — and the one thing the server must do + +The G1's map is built by the **web_nav3** (Nav2 + RTAB-Map) stack and stored on +disk as a **RTAB-Map SQLite `.db`** (`maps//.db`), with each map's +named places in `web/data//places/.json`. **There is no rendered +PNG on disk** — the dashboard draws the occupancy grid live over rosbridge. + +By design this agent uploads the **raw `.db`** (the real map artifact) plus its +places, instead of rendering an image. So: + +> ⚠️ **The fleet server's map endpoint must accept a `rtabmap_db` artifact.** +> The spec's documented body (`image_base64` / `resolution` / `origin`) is for a +> *rendered* map. If you need that instead, render the live `/map` OccupancyGrid +> over rosbridge — that's the telemetry-agent path, not this one. + +Two wire formats are supported (`MAP_UPLOAD_MODE`), pick what your server takes: + +| mode | request | +|---|---| +| `multipart` (default) | `multipart/form-data`: file field **`db`** (the `.db`) + form field **`meta`** (JSON) | +| `base64json` | JSON body: all meta fields + the `.db` as **`db_base64`** | + +`meta` / JSON body shape: + +```json +{ "sn": "g1_7892", "name": "floor-1", "file": "floor-1.db", "format": "rtabmap_db", + "size_bytes": 6994944, "sha256": "…", "mtime": 1731000000, "description": "ground floor", + "points": [ { "name": "dock", "type": "waypoint", "x": 1.2, "y": 3.4, "yaw": 0.0 } ] } +``` + +Each map is fingerprinted (size+mtime, then sha256); a map is (re)uploaded only +**on change**. State lives in `STATE_DIR/uploaded.json` so restarts don't re-push. + +**No ROS, no DDS** — just file reads + outbound HTTPS. web_nav3's HTTP API is +used only (optionally) to learn which map is *active* (`MAP_SELECT=active`). + +--- + +## Install (full Docker) + +One command — builds the image, creates `.env`, starts the container (auto-restarts on boot): + +```bash +./install.sh # creates .env if missing, then build + up -d +# (edit .env when prompted: SERVER_URL, DEVICE_TOKEN, SN, MAPS_HOST_DIR, DATA_HOST_DIR) +./install.sh --logs # follow logs +./install.sh --status # container state + list maps it sees +./install.sh --down # stop + remove +``` + +Or the raw compose flow: + +```bash +cp .env.example .env # set SERVER_URL + DEVICE_TOKEN, and the map paths +docker compose up -d --build +docker compose logs -f +``` + +Handy one-shots (no loop): + +```bash +docker compose run --rm sanad-api-g1 --list # what maps do I see? +docker compose run --rm sanad-api-g1 --dry-run # build payloads, never POST +docker compose run --rm sanad-api-g1 --once # one upload pass, then exit +``` + +Runs bare too (for a quick check on the workstation): + +```bash +pip install -r requirements.txt +SERVER_URL=… DEVICE_TOKEN=… SN=g1_7892 ROBOT=sanad \ + MAPS_DIR=…/web_nav3/maps DATA_DIR=…/web_nav3/web/data \ + python3 sanad_api_g1.py --list +``` + +### Key env (full list in `.env.example`) + +| var | meaning | +|---|---| +| `SERVER_URL`, `DEVICE_TOKEN` | given by YS Lootah — **required** | +| `SN` | fleet id / URL key — default `g1_7892` | +| `ROBOT` | web_nav3 robot name = maps subdir + `X-Robot-Name` — default `sanad` | +| `MAPS_HOST_DIR` / `DATA_HOST_DIR` | host paths bind-mounted read-only (see below) | +| `MAP_SELECT` | `all` (default) · `active` · `newest` | +| `MAP_UPLOAD_MODE` | `multipart` (default) · `base64json` | +| `POLL_INTERVAL` | scan cadence, seconds (default 30) | + +--- + +## ⚠️ Mounting the maps directory + +The uploader needs **read access to wherever web_nav3 actually keeps the `.db` +files.** That location is deployment-specific: + +- **Workstation dev copy:** `Project/G1/Nav2_Projects/web_nav3/{maps,web/data}` +- **Package_4 robot:** the `sanad-nav` container stores named maps *inside itself* + at `/home/unitree/marcus_nav2_test/maps` — that path is **not** bind-mounted to + the host by default. To let this uploader read them, either add a host + bind-mount for `maps/` to the `sanad-nav` service, or put both containers on a + **shared named volume** for the maps dir, then point `MAPS_HOST_DIR` at it. + +Set `MAPS_HOST_DIR` / `DATA_HOST_DIR` in `.env` accordingly. + +--- + +## Porting to R1 / Go2 + +Copy this folder to `sanad_api_r1` / `sanad_api_go2`, rename the script/service, +and change `SN` (`r1_…`, `go2_…`). If those robots store their map the same way +(web_nav3 `.db` + places) nothing else changes. If a robot has no web_nav3 `.db` +(e.g. a different SLAM), adjust `discover_maps()` / `load_points()` for that +robot's on-disk map format. diff --git a/agents/g1/docker-compose.yml b/agents/g1/docker-compose.yml new file mode 100644 index 0000000..6b1d3d3 --- /dev/null +++ b/agents/g1/docker-compose.yml @@ -0,0 +1,35 @@ +# sanad_api_g1 — standalone G1 fleet MAP uploader. +# +# cp .env.example .env # fill SERVER_URL + DEVICE_TOKEN, set the map paths +# docker compose up -d --build +# +# It reads the web_nav3 map files (RTAB-Map .db + per-map places) from the host +# via read-only mounts and POSTs each changed map to the fleet server. No ROS, +# no DDS — only file reads + outbound HTTPS (and an optional localhost call to +# web_nav3:8765 to learn the ACTIVE map, which is why network_mode: host). +services: + sanad-api-g1: + build: + context: . + dockerfile: Dockerfile + image: "${SANAD_API_G1_IMAGE:-sanad-api-g1:latest}" + container_name: sanad-api-g1 + network_mode: host # reach 127.0.0.1:8765 (web_nav3) + outbound HTTPS + restart: unless-stopped + env_file: .env + environment: + # In-container paths the app reads (host paths are the bind mounts below). + MAPS_DIR: /data/maps + DATA_DIR: /data/web_data + STATE_DIR: /data/state + volumes: + # web_nav3 maps dir (holds /*.db + maps_meta.json). READ-ONLY. + # On a Package_4 robot the nav container keeps these under + # /home/unitree/marcus_nav2_test/maps — point MAPS_HOST_DIR at that (or at + # a shared named volume). On the workstation dev copy it is + # /Project/G1/Nav2_Projects/web_nav3/maps. + - "${MAPS_HOST_DIR:-/home/unitree/marcus_nav2_test/maps}:/data/maps:ro" + # web_nav3 web/data dir (per-map places live under /places/*.json). + - "${DATA_HOST_DIR:-/home/unitree/marcus_nav2_test/web/data}:/data/web_data:ro" + # Persistent upload state (so a restart doesn't re-push unchanged maps). + - "${STATE_HOST_DIR:-./data/state}:/data/state" diff --git a/agents/g1/requirements.txt b/agents/g1/requirements.txt new file mode 100644 index 0000000..b143394 --- /dev/null +++ b/agents/g1/requirements.txt @@ -0,0 +1,2 @@ +# sanad_api_g1 map uploader — deliberately tiny (no ROS, no DDS). +requests>=2.31,<3 diff --git a/agents/g1/sanad_api_g1.py b/agents/g1/sanad_api_g1.py new file mode 100644 index 0000000..19378e6 --- /dev/null +++ b/agents/g1/sanad_api_g1.py @@ -0,0 +1,485 @@ +#!/usr/bin/env python3 +"""sanad_api_g1 — G1 fleet MAP uploader. + +Scope (this build): upload the G1's navigation MAP to the YS Lootah fleet +server. Nothing else (no telemetry / commands / logs — those are separate +agents). It is the "Maps sync" row of the fleet spec: + + POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map (Bearer device token) + +WHAT IT UPLOADS +--------------- +The G1's map is produced by the web_nav3 (Nav2 + RTAB-Map) stack and stored on +disk as a RTAB-Map SQLite ``.db`` file (per web_nav3/web/backend.py: +``MAPS_ROOT//.db`` + a ``maps_meta.json`` sidecar), with each map's +named places kept in ``web/data//places/.json``. + +There is NO rendered PNG on disk — the dashboard draws the occupancy grid live +over rosbridge. By design choice this agent uploads the RAW ``.db`` (the actual +map artifact) plus its places, rather than rendering an image. That means the +FLEET SERVER must accept a ``rtabmap_db`` artifact on the map endpoint (see +MAP_UPLOAD_MODE for the two wire formats). The documented spec body +(image_base64 / resolution / origin) is for a rendered map; switch to the +telemetry-agent's live-rosbridge renderer if you need that instead. + +NO ROS, NO DDS. Pure files + HTTPS, so it drops into any robot as its own +container. It reads the maps/places straight from mounted volumes; the web_nav3 +HTTP API is only used (optionally) to learn which map is "active". + +CHANGE DETECTION +---------------- +Each map's ``.db`` is fingerprinted (size + mtime fast-path, then sha256). A map +is (re)uploaded only when its fingerprint changes — matching the spec's "send +the nav map on change". State persists in ``STATE_DIR/uploaded.json`` so a +restart doesn't re-push unchanged maps. + +CONFIG — all via environment (see .env.example) +----------------------------------------------- + SERVER_URL base URL of the fleet server (required) e.g. https://fleet.example.com + DEVICE_TOKEN per-robot bearer token (required) + SN this robot's fleet id (path key) default g1_7892 + ROBOT web_nav3 robot name (maps subdir + header) default sanad + MAPS_DIR mounted web_nav3 maps/ dir (contains /*.db and/or *.db) + DATA_DIR mounted web_nav3 web/data dir (per-map places live under /places/) + LEGACY_PLACES optional path to a legacy places.json (per-robot, no map scoping) + WEB_NAV3_URL optional http://127.0.0.1:8765 — used only to read the ACTIVE map + MAP_SELECT all | active | newest default all + MAP_UPLOAD_MODE multipart | base64json default multipart + MAP_ENDPOINT path template, {sn} substituted default /api/v1/fleet/ingest/{sn}/map + POLL_INTERVAL seconds between scans (loop mode) default 30 + STATE_DIR writable dir for upload state default /data/state + VERIFY_TLS 1|0 verify server TLS default 1 + HTTP_TIMEOUT per-request timeout seconds default 30 + +CLI +--- + python sanad_api_g1.py # run the loop (default) + python sanad_api_g1.py --once # one scan+upload pass, then exit + python sanad_api_g1.py --list # list discovered maps (no upload) + python sanad_api_g1.py --dry-run # build payloads + report, never POST + python sanad_api_g1.py --force # ignore state; upload even if unchanged +""" +from __future__ import annotations + +import argparse +import base64 +import hashlib +import json +import logging +import math +import os +import sys +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any, Dict, List, Optional + +import requests + +log = logging.getLogger("sanad_api_g1") + + +# --------------------------------------------------------------------------- # +# tiny .env loader (so it also runs bare, outside docker) — no dependency +# --------------------------------------------------------------------------- # +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("=") + k, v = k.strip(), v.strip().strip('"').strip("'") + os.environ.setdefault(k, v) + + +def _env(name: str, default: str = "") -> str: + return os.environ.get(name, default).strip() + + +def _env_bool(name: str, default: bool) -> bool: + v = _env(name, "1" if default else "0").lower() + return v in ("1", "true", "yes", "on") + + +# --------------------------------------------------------------------------- # +# config +# --------------------------------------------------------------------------- # +@dataclass +class Config: + server_url: str + device_token: str + sn: str + robot: str + maps_dir: Path + data_dir: Optional[Path] + legacy_places: Optional[Path] + web_nav3_url: str + map_select: str + upload_mode: str + endpoint_tmpl: str + poll_interval: float + state_dir: Path + 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)}") + data_dir = _env("DATA_DIR") + legacy = _env("LEGACY_PLACES") + return cls( + server_url=server, + device_token=token, + sn=_env("SN", "g1_7892"), + robot=_env("ROBOT", "sanad"), + maps_dir=Path(_env("MAPS_DIR", "/data/maps")), + data_dir=Path(data_dir) if data_dir else None, + legacy_places=Path(legacy) if legacy else None, + web_nav3_url=_env("WEB_NAV3_URL", "").rstrip("/"), + map_select=_env("MAP_SELECT", "all").lower(), + upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(), + endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"), + poll_interval=float(_env("POLL_INTERVAL", "30")), + state_dir=Path(_env("STATE_DIR", "/data/state")), + verify_tls=_env_bool("VERIFY_TLS", True), + http_timeout=float(_env("HTTP_TIMEOUT", "30")), + ) + + def map_url(self) -> str: + return self.server_url + self.endpoint_tmpl.format(sn=self.sn) + + def auth_headers(self) -> Dict[str, str]: + return {"Authorization": f"Bearer {self.device_token}"} + + +# --------------------------------------------------------------------------- # +# map artifact +# --------------------------------------------------------------------------- # +@dataclass +class MapArtifact: + path: Path # absolute path to the .db + name: str # file name, e.g. floor-1.db + stem: str # name without .db, e.g. floor-1 + size: int + mtime: int + description: str = "" + sha256: str = "" + points: List[Dict[str, Any]] = field(default_factory=list) + + def fingerprint(self) -> str: + # cheap identity for the change check before we hash the whole file + return f"{self.size}:{self.mtime}" + + +def _sha256(path: Path) -> str: + h = hashlib.sha256() + with path.open("rb") as f: + for chunk in iter(lambda: f.read(1024 * 1024), b""): + h.update(chunk) + return h.hexdigest() + + +# --------------------------------------------------------------------------- # +# discovery — read maps + places straight from the mounted web_nav3 files +# --------------------------------------------------------------------------- # +def _map_key(stem: str) -> str: + """Mirror backend._map_key: safe filename stem for the per-map places file.""" + stem = Path(stem).name + if stem.endswith(".db"): + stem = stem[:-3] + return "".join(c for c in stem if c.isalnum() or c in "_-.") + + +def _read_json(path: Path, default: Any) -> Any: + try: + return json.loads(path.read_text() or "") + except Exception: + return default + + +def _yaw_from_pose(pose: Dict[str, Any]) -> float: + """Yaw (rad) from a places-pose dict. Supports full quaternion, planar + (qz,qw), or an explicit yaw field.""" + if "qw" in pose or "qz" in pose: + qx = float(pose.get("qx", 0.0)); qy = float(pose.get("qy", 0.0)) + qz = float(pose.get("qz", 0.0)); qw = float(pose.get("qw", 1.0)) + return math.atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz)) + return float(pose.get("yaw", 0.0)) + + +def _places_files_for(cfg: Config, stem: str) -> List[Path]: + """Candidate on-disk places files for a map, most-specific first.""" + out: List[Path] = [] + key = _map_key(stem) + if cfg.data_dir: + out.append(cfg.data_dir / cfg.robot / "places" / f"{key}.json") + if cfg.legacy_places: + out.append(cfg.legacy_places) + return out + + +def load_points(cfg: Config, stem: str) -> List[Dict[str, Any]]: + """Return the map's saved places as fleet points: {name, type, x, y, yaw}. + + Places store shape (web_nav3): {"": {x,y,z,qx,qy,qz,qw}}. + """ + for pf in _places_files_for(cfg, stem): + data = _read_json(pf, None) if pf.exists() else None + if isinstance(data, dict) and data: + pts: List[Dict[str, Any]] = [] + for name, pose in data.items(): + if not isinstance(pose, dict): + continue + try: + pts.append({ + "name": name, + "type": str(pose.get("type", "waypoint")), + "x": float(pose["x"]), + "y": float(pose["y"]), + "yaw": round(_yaw_from_pose(pose), 4), + }) + except (KeyError, TypeError, ValueError): + continue + log.debug("points for %s: %d (from %s)", stem, len(pts), pf) + return pts + return [] + + +def discover_maps(cfg: Config) -> List[MapArtifact]: + """Find every .db under MAPS_DIR// and MAPS_DIR/ (legacy root).""" + roots = [cfg.maps_dir / cfg.robot, cfg.maps_dir] + meta: Dict[str, Any] = {} + meta_file = cfg.maps_dir / cfg.robot / "maps_meta.json" + if meta_file.exists(): + meta = _read_json(meta_file, {}) or {} + + seen: set[str] = set() + out: List[MapArtifact] = [] + for root in roots: + if not root.exists(): + continue + for p in sorted(root.glob("*.db")): + rp = str(p.resolve()) + if rp in seen: + continue + seen.add(rp) + st = p.stat() + out.append(MapArtifact( + path=p, + name=p.name, + stem=p.stem, + size=st.st_size, + mtime=int(st.st_mtime), + description=(meta.get(p.name) or {}).get("description", ""), + )) + out.sort(key=lambda m: m.mtime, reverse=True) + return out + + +def _active_map_name(cfg: Config) -> Optional[str]: + """Ask web_nav3 which map is currently loaded (optional; None if not set/up).""" + if not cfg.web_nav3_url: + return None + try: + r = requests.get(cfg.web_nav3_url + "/api/status", + headers={"X-Robot-Name": cfg.robot}, + timeout=min(cfg.http_timeout, 5)) + r.raise_for_status() + am = (r.json() or {}).get("active_map") + return _map_key(am) if am else None + except requests.RequestException as e: + log.debug("active-map query failed: %s", e) + return None + + +def select_maps(cfg: Config, maps: List[MapArtifact]) -> List[MapArtifact]: + if not maps: + return [] + if cfg.map_select == "newest": + return maps[:1] + if cfg.map_select == "active": + active = _active_map_name(cfg) + if active: + picked = [m for m in maps if _map_key(m.stem) == active] + if picked: + return picked + log.warning("active map %r not found on disk; falling back to newest", active) + return maps[:1] + return maps # "all" + + +# --------------------------------------------------------------------------- # +# state (which fingerprints already uploaded) +# --------------------------------------------------------------------------- # +def _state_file(cfg: Config) -> Path: + return cfg.state_dir / "uploaded.json" + + +def load_state(cfg: Config) -> Dict[str, str]: + return _read_json(_state_file(cfg), {}) if _state_file(cfg).exists() else {} + + +def save_state(cfg: Config, state: Dict[str, str]) -> None: + try: + cfg.state_dir.mkdir(parents=True, exist_ok=True) + _state_file(cfg).write_text(json.dumps(state, indent=2)) + except Exception as e: + log.warning("could not persist state: %s", e) + + +# --------------------------------------------------------------------------- # +# upload +# --------------------------------------------------------------------------- # +def build_meta(cfg: Config, m: MapArtifact) -> Dict[str, Any]: + return { + "sn": cfg.sn, + "name": m.stem, + "file": m.name, + "format": "rtabmap_db", + "size_bytes": m.size, + "sha256": m.sha256, + "mtime": m.mtime, + "description": m.description, + "points": m.points, + } + + +def upload_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool: + url = cfg.map_url() + meta = build_meta(cfg, m) + try: + if cfg.upload_mode == "base64json": + body = dict(meta) + body["db_base64"] = base64.b64encode(m.path.read_bytes()).decode("ascii") + resp = session.post(url, json=body, headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + else: # multipart (default) + with m.path.open("rb") as fh: + files = {"db": (m.name, fh, "application/octet-stream")} + data = {"meta": json.dumps(meta)} + resp = session.post(url, files=files, data=data, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + except requests.RequestException as e: + log.error("upload %s FAILED (transport): %s", m.name, e) + return False + + if not resp.ok: + detail = resp.text[:300] + log.error("upload %s FAILED: HTTP %s %s", m.name, resp.status_code, detail) + return False + log.info("uploaded %s (%.2f MB, %d points) -> HTTP %s", + m.name, m.size / 1024 / 1024, len(m.points), resp.status_code) + return True + + +# --------------------------------------------------------------------------- # +# one pass +# --------------------------------------------------------------------------- # +def run_once(cfg: Config, *, force: bool, dry_run: bool, + session: requests.Session) -> int: + maps = select_maps(cfg, discover_maps(cfg)) + if not maps: + log.info("no .db maps found under %s (robot=%s)", cfg.maps_dir, cfg.robot) + return 0 + + state = load_state(cfg) + uploaded = 0 + for m in maps: + prev = state.get(str(m.path.resolve())) + if not force and prev == m.fingerprint(): + log.debug("unchanged, skip: %s", m.name) + continue + # confirm change with a real content hash (mtime can shift without edits) + m.sha256 = _sha256(m.path) + m.points = load_points(cfg, m.stem) + + if dry_run: + log.info("[dry-run] would upload %s (%.2f MB, sha=%s…, %d points)", + m.name, m.size / 1024 / 1024, m.sha256[:12], len(m.points)) + uploaded += 1 + continue + + if upload_map(cfg, m, session): + state[str(m.path.resolve())] = m.fingerprint() + save_state(cfg, state) + uploaded += 1 + + if uploaded == 0: + log.info("nothing to upload (%d map(s) already current)", len(maps)) + return uploaded + + +def cmd_list(cfg: Config) -> None: + maps = discover_maps(cfg) + active = _active_map_name(cfg) + if not maps: + print(f"(no .db maps under {cfg.maps_dir} for robot '{cfg.robot}')") + return + print(f"{len(maps)} map(s) under {cfg.maps_dir} (robot={cfg.robot}):") + for m in maps: + pts = load_points(cfg, m.stem) + flag = " <-- active" if active and _map_key(m.stem) == active else "" + print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points" + f" {m.description}{flag}") + + +# --------------------------------------------------------------------------- # +# main +# --------------------------------------------------------------------------- # +def main(argv: Optional[List[str]] = None) -> int: + ap = argparse.ArgumentParser(description="G1 fleet map uploader") + ap.add_argument("--once", action="store_true", help="one scan+upload pass, then exit") + ap.add_argument("--list", action="store_true", help="list discovered maps and exit") + ap.add_argument("--dry-run", action="store_true", help="build payloads but never POST") + ap.add_argument("--force", action="store_true", help="upload even if unchanged") + ap.add_argument("--interval", type=float, default=None, help="override POLL_INTERVAL seconds") + 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 + + log.info("sanad_api_g1 map uploader — sn=%s robot=%s server=%s mode=%s select=%s", + cfg.sn, cfg.robot, cfg.server_url, cfg.upload_mode, cfg.map_select) + log.info("maps_dir=%s data_dir=%s web_nav3=%s", + cfg.maps_dir, cfg.data_dir, cfg.web_nav3_url or "(disabled)") + + if args.list: + cmd_list(cfg) + return 0 + + session = requests.Session() + if args.once or args.dry_run: + run_once(cfg, force=args.force, dry_run=args.dry_run, session=session) + return 0 + + log.info("loop every %.0fs (Ctrl-C to stop)", cfg.poll_interval) + while True: + try: + run_once(cfg, force=args.force, dry_run=False, session=session) + except Exception as e: # never let the loop die + log.exception("pass 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/go2/.dockerignore b/agents/go2/.dockerignore new file mode 100644 index 0000000..bce1cad --- /dev/null +++ b/agents/go2/.dockerignore @@ -0,0 +1,4 @@ +.env +__pycache__/ +*.pyc +README.md diff --git a/agents/go2/.env.example b/agents/go2/.env.example new file mode 100644 index 0000000..fe186a7 --- /dev/null +++ b/agents/go2/.env.example @@ -0,0 +1,29 @@ +# sanad_api_go2 — copy to .env and fill in. + +# ── fleet server (REQUIRED — YS Lootah gives you these two) ────────────────── +SERVER_URL=https://fleet.example.com +DEVICE_TOKEN=REPLACE_WITH_DEVICE_TOKEN + +# ── identity ───────────────────────────────────────────────────────────────── +# This robot's fleet id, sent in the telemetry body as "sn". Set the real Go2 id. +SN=go2_0000 + +# ── DDS (reading robot state — unitree_go family) ──────────────────────────── +# Interface that sees the Go2's DDS traffic (Jetson/onboard usually eth0). +DDS_INTERFACE=eth0 +DDS_DOMAIN=0 +# MAC_INTERFACE=eth0 + +# ── position options ───────────────────────────────────────────────────────── +# none (default) | sportmode (rt/lf/sportmodestate) | rosbridge (/odom). +GO2_POSITION_SOURCE=none +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/go2/.gitignore b/agents/go2/.gitignore new file mode 100644 index 0000000..cff5543 --- /dev/null +++ b/agents/go2/.gitignore @@ -0,0 +1,3 @@ +.env +__pycache__/ +*.pyc diff --git a/agents/go2/Dockerfile b/agents/go2/Dockerfile new file mode 100644 index 0000000..49c647d --- /dev/null +++ b/agents/go2/Dockerfile @@ -0,0 +1,47 @@ +# sanad_api_go2 — Go2 fleet telemetry agent (reads DDS: rt/lowstate, unitree_go). +# +# BUILD ON THE GO2 COMPUTE (native arm64). The DDS stack: prebuilt CycloneDDS +# (apt) + the CycloneDDS python binding (pip) + the vendored unitree_sdk2py wheel +# (includes both unitree_go and unitree_hg idl). 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_go2.py . + +ENV PYTHONUNBUFFERED=1 \ + DDS_INTERFACE=eth0 \ + DDS_DOMAIN=0 +ENTRYPOINT ["python", "-u", "sanad_api_go2.py"] diff --git a/agents/go2/README.md b/agents/go2/README.md new file mode 100644 index 0000000..b8798c6 --- /dev/null +++ b/agents/go2/README.md @@ -0,0 +1,42 @@ +# sanad_api_go2 — Go2 fleet **telemetry** agent + +Pushes the Unitree **Go2**'s live status to the YS Lootah fleet server. Sibling of +`sanad_api_r1`; deploy it with the fleet installer (`../../fleet_install.sh`). + +``` +POST {SERVER_URL}/api/v1/fleet/ingest/telemetry Authorization: Bearer +{ "sn":"go2_0000", "mac":"…", "battery":74, "charging":false, + "status":"idle", "position":{"x":…,"y":…}|null, "faults":[], "ts":… } +``` + +Sent every ~2 s; a **heartbeat** (`battery:null, status:offline`) when DDS is +silent so the robot stays online. + +## Data sources (Go2, `unitree_go` DDS) + +| field | source | +|---|---| +| `battery`, `charging` | **`rt/lowstate.bms_state`** (nested — Go2 has no separate BMS topic): `soc` 0–100; charging from `current` sign | +| `faults[]` | `rt/lowstate` motor temps + staleness | +| `status` | derived (charging / moving / idle / offline) | +| `position` | optional: `sportmode` (`rt/lf/sportmodestate`) or `rosbridge` (`/odom`) — default off | +| `mac` | primary NIC | + +> **Safety:** read-only — never commands motion. + +## Run / test + +```bash +../../fleet_install.sh install go2 --sn go2_ +../../fleet_install.sh data go2 +# bare simulate (no robot): +SERVER_URL=… DEVICE_TOKEN=… SN=go2_0 python3 sanad_api_go2.py --simulate -v +``` + +Build on the Go2 compute (arm64); bundles CycloneDDS + the vendored +`unitree_sdk2py` wheel (`vendor/`). `network_mode: host` for DDS. + +> ⚠️ **UNVERIFIED ON HARDWARE.** Written from the `unitree_go` SDK layout but not +> yet run on a real Go2. Confirm the `bms_state` current sign (charging polarity) +> and the `sportmodestate` fields on the robot. Verified so far: image builds, +> `unitree_go` imports, simulate/heartbeat payloads correct. diff --git a/agents/go2/docker-compose.yml b/agents/go2/docker-compose.yml new file mode 100644 index 0000000..e3cd9a7 --- /dev/null +++ b/agents/go2/docker-compose.yml @@ -0,0 +1,20 @@ +# sanad_api_go2 — standalone Go2 fleet TELEMETRY agent. +# (The fleet installer deploys via plain `docker build`/`docker run` + systemd; +# this compose file is for manual local use.) +# +# cp .env.example .env # set SERVER_URL + DEVICE_TOKEN + SN + DDS_INTERFACE +# docker compose up -d --build +# +# Reads the Go2's DDS state (rt/lowstate, unitree_go; battery from bms_state) and +# POSTs telemetry every ~2s. network_mode: host is REQUIRED for DDS visibility. +services: + sanad-api-go2: + build: + context: . + dockerfile: Dockerfile + image: "${SANAD_API_GO2_IMAGE:-sanad-api-go2:latest}" + container_name: sanad-api-go2 + network_mode: host + restart: unless-stopped + env_file: .env + # command: ["--simulate"] # synthetic state, no robot diff --git a/agents/go2/requirements.txt b/agents/go2/requirements.txt new file mode 100644 index 0000000..b462572 --- /dev/null +++ b/agents/go2/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/go2/sanad_api_go2.py b/agents/go2/sanad_api_go2.py new file mode 100644 index 0000000..8effa79 --- /dev/null +++ b/agents/go2/sanad_api_go2.py @@ -0,0 +1,409 @@ +#!/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()) diff --git a/agents/go2/vendor/crc_aarch64.so b/agents/go2/vendor/crc_aarch64.so new file mode 100644 index 0000000..878df74 Binary files /dev/null and b/agents/go2/vendor/crc_aarch64.so differ diff --git a/agents/go2/vendor/crc_amd64.so b/agents/go2/vendor/crc_amd64.so new file mode 100644 index 0000000..258055d Binary files /dev/null and b/agents/go2/vendor/crc_amd64.so differ diff --git a/agents/go2/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl b/agents/go2/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl new file mode 100644 index 0000000..3231178 Binary files /dev/null and b/agents/go2/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl differ diff --git a/agents/r1/.dockerignore b/agents/r1/.dockerignore new file mode 100644 index 0000000..bce1cad --- /dev/null +++ b/agents/r1/.dockerignore @@ -0,0 +1,4 @@ +.env +__pycache__/ +*.pyc +README.md diff --git a/agents/r1/.env.example b/agents/r1/.env.example new file mode 100644 index 0000000..00d7598 --- /dev/null +++ b/agents/r1/.env.example @@ -0,0 +1,35 @@ +# sanad_api_r1 — copy to .env and fill in. docker compose reads it automatically. + +# ── fleet server (REQUIRED — YS Lootah gives you these two) ────────────────── +SERVER_URL=https://fleet.example.com +DEVICE_TOKEN=REPLACE_WITH_DEVICE_TOKEN + +# ── identity ───────────────────────────────────────────────────────────────── +# This robot's fleet id, sent in the telemetry body as "sn". Set the real R1 id. +SN=r1_0000 + +# ── DDS (reading robot state) ──────────────────────────────────────────────── +# The R1 backpack link is eth10 (SanadR1 default). Use the interface that sees +# the robot's DDS traffic. Domain 0 unless your robot is configured otherwise. +DDS_INTERFACE=eth10 +DDS_DOMAIN=0 +# Which NIC's MAC to report as the robot identity (defaults to DDS_INTERFACE). +# MAC_INTERFACE=eth10 + +# ── status / position options ──────────────────────────────────────────────── +# Read the loco FSM for a richer status (READ-ONLY GET RPC, never commands +# motion). 0 = derive status from battery + joint motion only (safe default). +R1_READ_FSM=0 +# Position source: none (default) | rosbridge. The R1 localizes with stereo VSLAM +# on the ROS side; set 'rosbridge' + ROSBRIDGE_URL to report x,y from /odom. +R1_POSITION_SOURCE=none +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/r1/.gitignore b/agents/r1/.gitignore new file mode 100644 index 0000000..cff5543 --- /dev/null +++ b/agents/r1/.gitignore @@ -0,0 +1,3 @@ +.env +__pycache__/ +*.pyc diff --git a/agents/r1/Dockerfile b/agents/r1/Dockerfile new file mode 100644 index 0000000..83ed62f --- /dev/null +++ b/agents/r1/Dockerfile @@ -0,0 +1,47 @@ +# sanad_api_r1 — R1 fleet telemetry agent (reads DDS: rt/lowstate + rt/lf/bmsstate). +# +# BUILD ON THE R1 BACKPACK (native arm64). The DDS stack mirrors SanadR1's proven +# recipe: prebuilt CycloneDDS (apt) + the CycloneDDS python binding (pip, no wheel +# for arm64) + 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_r1.py . + +ENV PYTHONUNBUFFERED=1 \ + DDS_INTERFACE=eth10 \ + DDS_DOMAIN=0 +ENTRYPOINT ["python", "-u", "sanad_api_r1.py"] diff --git a/agents/r1/README.md b/agents/r1/README.md new file mode 100644 index 0000000..5e55095 --- /dev/null +++ b/agents/r1/README.md @@ -0,0 +1,82 @@ +# sanad_api_r1 — R1 fleet **telemetry** agent + +Pushes the Unitree **R1 EDU**'s live status to the YS Lootah fleet server. One +robot type = one folder = its own Docker image (sibling of `sanad_api_g1`). + +This build covers the **"Main statuses"** endpoint — the R1 **map is skipped by +request**: + +``` +POST {SERVER_URL}/api/v1/fleet/ingest/telemetry Authorization: Bearer +{ "sn": "r1_0000", "mac": "…", "battery": 74, "charging": false, + "status": "idle", "position": {"x": …, "y": …}|null, "faults": [] } +``` + +Sent every ~2 s. If DDS state can't be read it still sends a **heartbeat** +(`battery: null`, `status: "offline"`) so the robot shows as online. + +## Where each field comes from (R1 EDU, unitree_hg DDS) + +| field | source | +|---|---| +| `battery`, `charging` | `rt/lf/bmsstate` (`BmsState_`): `soc` 0–100; `charging = current > +0.05 A` — same as SanadR1 `arm_controller.get_battery` | +| `faults[]` | `rt/lowstate` (`LowState_`): low battery, motor over-temp, comms-stale | +| `status` | derived (charging / moving / idle / offline); optional loco **FSM** read (ids `0`/`1`/`4`/`811`, GET-only) with `R1_READ_FSM=1` | +| `position` | R1 localizes with stereo **VSLAM** (ROS side). This agent is ROS-free, so position is `null` unless `R1_POSITION_SOURCE=rosbridge` (reads `/odom`) | +| `mac` | primary NIC hardware address | + +> **Safety:** never commands motion — only the read-only `GET_FSM_ID` RPC is ever +> issued to the R1. + +## Install (full Docker) — on the R1 backpack + +One command — builds the DDS image, creates `.env`, starts the container (auto-restarts on boot): + +```bash +./install.sh # creates .env if missing, then build + up -d +# (edit .env when prompted: SERVER_URL, DEVICE_TOKEN, SN, DDS_INTERFACE=eth10) +./install.sh --logs # follow logs +./install.sh --simulate # one synthetic post (server smoke test, no robot) +./install.sh --down # stop + remove +``` + +Or the raw compose flow: + +```bash +cp .env.example .env # SERVER_URL, DEVICE_TOKEN, SN, DDS_INTERFACE=eth10 +docker compose up -d --build +docker compose logs -f +``` + +Test without a robot (synthetic state, exercises the full upload path): + +```bash +docker compose run --rm sanad-api-r1 --simulate --once # one synthetic post +docker compose run --rm sanad-api-r1 --dry-run # print telemetry, no POST +# or bare: +SERVER_URL=… DEVICE_TOKEN=… SN=r1_0000 python3 sanad_api_r1.py --simulate -v +``` + +### Key env (full list in `.env.example`) + +| var | meaning | +|---|---| +| `SERVER_URL`, `DEVICE_TOKEN` | given by YS Lootah — **required** | +| `SN` | fleet id, e.g. `r1_0000` | +| `DDS_INTERFACE` | robot link — **`eth10`** on the R1 backpack | +| `R1_READ_FSM` | `1` = read loco FSM for status (GET-only) | +| `R1_POSITION_SOURCE` | `none` (default) · `rosbridge` | +| `POLL_INTERVAL` | seconds between posts (default 2) | + +## Build note + +Build **on the R1 backpack** (arm64). The image bundles the DDS stack +(CycloneDDS + the vendored `unitree_sdk2py` wheel under `vendor/`), mirroring +SanadR1's recipe. `network_mode: host` is required for DDS visibility. If +`unitree_sdk2py` can't load, the agent logs a warning and sends heartbeats. + +## Porting + +`sanad_api_go2` is the same pattern; the Go2 uses `unitree_go` (not `unitree_hg`) +DDS, so swap the message imports (`BmsState_`/`LowState_` idl path) and topic +names for the Go2 SDK. diff --git a/agents/r1/docker-compose.yml b/agents/r1/docker-compose.yml new file mode 100644 index 0000000..c9b97b9 --- /dev/null +++ b/agents/r1/docker-compose.yml @@ -0,0 +1,21 @@ +# sanad_api_r1 — standalone R1 fleet TELEMETRY agent. +# +# cp .env.example .env # set SERVER_URL + DEVICE_TOKEN + SN + DDS_INTERFACE +# docker compose up -d --build +# +# Reads the R1's DDS state (rt/lowstate + rt/lf/bmsstate) and POSTs telemetry +# every ~2s to the fleet server. network_mode: host is REQUIRED so the robot's +# DDS traffic is visible (it does not traverse a NAT bridge). +services: + sanad-api-r1: + build: + context: . + dockerfile: Dockerfile + image: "${SANAD_API_R1_IMAGE:-sanad-api-r1:latest}" + container_name: sanad-api-r1 + network_mode: host # DDS visibility + outbound HTTPS + restart: unless-stopped + env_file: .env + # Uncomment to run the synthetic-state generator instead of real DDS + # (useful for a server-side smoke test before the robot is wired): + # command: ["--simulate"] diff --git a/agents/r1/requirements.txt b/agents/r1/requirements.txt new file mode 100644 index 0000000..b462572 --- /dev/null +++ b/agents/r1/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/r1/sanad_api_r1.py b/agents/r1/sanad_api_r1.py new file mode 100644 index 0000000..dcbcd2f --- /dev/null +++ b/agents/r1/sanad_api_r1.py @@ -0,0 +1,529 @@ +#!/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()) diff --git a/agents/r1/vendor/crc_aarch64.so b/agents/r1/vendor/crc_aarch64.so new file mode 100644 index 0000000..878df74 Binary files /dev/null and b/agents/r1/vendor/crc_aarch64.so differ diff --git a/agents/r1/vendor/crc_amd64.so b/agents/r1/vendor/crc_amd64.so new file mode 100644 index 0000000..258055d Binary files /dev/null and b/agents/r1/vendor/crc_amd64.so differ diff --git a/agents/r1/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl b/agents/r1/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl new file mode 100644 index 0000000..3231178 Binary files /dev/null and b/agents/r1/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl differ diff --git a/fleet_install.sh b/fleet_install.sh new file mode 100755 index 0000000..7c3881a --- /dev/null +++ b/fleet_install.sh @@ -0,0 +1,265 @@ +#!/usr/bin/env bash +# fleet_install.sh — deploy/manage a Sanad fleet agent on a robot over SSH, +# as FULL DOCKER with a user-level systemd auto-start service. +# +# INTERACTIVE (no args): ./fleet_install.sh +# → pick robot type → enter IP → it detects whether the agent is already +# installed and offers the right actions (install / uninstall / data / ...). +# +# SCRIPTABLE: +# ./fleet_install.sh install [--sn NAME] [--server-ip IP] [--port N] [--token TOK] [--user U] +# ./fleet_install.sh uninstall [--user U] +# ./fleet_install.sh status +# ./fleet_install.sh data # show the data it is sending +# ./fleet_install.sh logs +# ./fleet_install.sh test [--server-ip IP] [--keep-server] +# +# Deploy model: rsync the canonical workstation copy -> robot, `docker build` +# on the robot (native arm64), `docker create` the container, and a user +# systemd unit (linger-enabled) owns start/stop/auto-start. No docker-compose +# needed on the robot; no sudo needed (user-level systemd). +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)" ) + +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"; } + +SSH_OPTS=(-o BatchMode=yes -o ConnectTimeout=8 -o StrictHostKeyChecking=accept-new) +rmt(){ ssh -n "${SSH_OPTS[@]}" "$USER_@$IP" "$@"; } # -n: never read our stdin +rmt_in(){ ssh "${SSH_OPTS[@]}" "$USER_@$IP" "$@"; } # stdin passthrough (heredocs) +detect_server_ip(){ [ -n "$SERVER_IP" ] && { echo "$SERVER_IP"; return; } + ip -o route get "$IP" 2>/dev/null | grep -oP 'src \K\S+' | head -1; } + +# ---- per-type docker create args (volumes differ) ---- +run_args(){ + local base="--network host --env-file /home/$USER_/$(rdir_of "$1")/.env" + if [ "$1" = g1 ]; then + echo "$base -v /home/$USER_/$(rdir_of "$1")/maps:/data/maps:ro \ + -v /home/$USER_/$(rdir_of "$1")/web_data:/data/web_data:ro \ + -v /home/$USER_/$(rdir_of "$1")/state:/data/state" + else + echo "$base" + fi +} + +# ---- write the robot-side .env ---- +push_env(){ + local t="$1" sip="$2" rdir; rdir="$(rdir_of "$t")" + if [ "$t" = g1 ]; then + rmt_in "cat > ~/$rdir/.env" < ~/$rdir/.env" </dev/null 2>&1" 2>/dev/null +} + +do_install(){ + local t="$1" img rdir unit sip; img="$(img_of "$t")"; rdir="$(rdir_of "$t")"; unit="$(unit_of "$t")" + sip="$(detect_server_ip)"; [ -n "$sip" ] || die "cannot detect server IP toward $IP (use --server-ip)" + echo "== INSTALL $t ($SN) on $IP → fleet server http://$sip:$PORT ==" + echo ">> rsync agent -> $USER_@$IP:~/$rdir/" + rsync -az --delete --exclude '.env' --exclude '__pycache__' --exclude '*.pyc' \ + --exclude 'data/' --exclude 'state/' \ + -e "ssh ${SSH_OPTS[*]}" "$AGENTS/$t/" "$USER_@$IP:~/$rdir/" || die "rsync failed" + rmt "mkdir -p ~/$rdir/maps/sanad ~/$rdir/web_data ~/$rdir/state ~/.config/systemd/user" + push_env "$t" "$sip" + echo ">> docker build on robot (native arm64; R1/Go2 compile DDS on first build) ..." + rmt "cd ~/$rdir && docker build -t $img:latest ." || die "docker build failed" + echo ">> creating container (systemd will own its lifecycle) ..." + rmt "docker rm -f $img >/dev/null 2>&1; docker create --name $img $(run_args "$t") $img:latest >/dev/null" + echo ">> installing user systemd service $unit (auto-start on boot via linger) ..." + rmt_in "cat > ~/.config/systemd/user/$unit" < $SN +After=docker.service network-online.target + +[Service] +Type=simple +Restart=always +RestartSec=5 +ExecStart=/usr/bin/docker start -a $img +ExecStop=/usr/bin/docker stop -t 10 $img + +[Install] +WantedBy=default.target +EOF + rmt "loginctl enable-linger \$USER >/dev/null 2>&1; \ + systemctl --user daemon-reload; \ + systemctl --user enable --now $unit" + sleep 2 + echo ">> service state:"; rmt "systemctl --user --no-pager -l status $unit | sed -n '1,4p'" + echo "== installed. show data: $0 data $t $IP ==" +} + +do_uninstall(){ + local t="$1" img unit rdir; img="$(img_of "$t")"; unit="$(unit_of "$t")"; rdir="$(rdir_of "$t")" + echo "== UNINSTALL $t on $IP ==" + rmt "systemctl --user disable --now $unit >/dev/null 2>&1; \ + rm -f ~/.config/systemd/user/$unit; systemctl --user daemon-reload; \ + docker rm -f $img >/dev/null 2>&1; docker rmi $img:latest >/dev/null 2>&1; \ + rm -rf ~/$rdir; echo ' removed service, container, image, and ~/'$rdir" + echo "== uninstalled ==" +} + +do_status(){ + local t="$1" img unit; img="$(img_of "$t")"; unit="$(unit_of "$t")" + echo "== STATUS $t on $IP ==" + rmt "systemctl --user --no-pager status $unit 2>/dev/null | sed -n '1,4p' || echo 'no service'; echo; \ + docker ps -a --filter name=$img --format 'container: {{.Names}} {{.Status}} ({{.Image}})' || echo 'no container'" +} + +do_logs(){ rmt "docker logs --tail 40 -f $(img_of "$1")"; } + +do_data(){ + local t="$1" img; img="$(img_of "$t")" + echo "== DATA $t on $IP (what it is sending to the fleet server) ==" + rmt "systemctl --user --no-pager status $(unit_of "$t") 2>/dev/null | sed -n '1,3p'; echo '--- recent telemetry/map posts ---'; \ + docker logs --tail 12 $img 2>&1 | grep -E 'telemetry ok|uploaded|nothing to upload|heartbeat|POST failed' | tail -8 || docker logs --tail 12 $img 2>&1" +} + +# ---- deploy-less end-to-end test against the workstation server ---- +start_server(){ local reqlog="/tmp/fleet_reqs_${1}_$$.jsonl"; echo "$reqlog" > /tmp/fleet_last_reqlog + PORT=$PORT REQLOG="$reqlog" python3 "$SCRIPT_DIR/fleet_test_server.py" >/tmp/fleet_srv_$$.log 2>&1 & + echo $! > /tmp/fleet_srv_pid; sleep 1; } +stop_server(){ [ -f /tmp/fleet_srv_pid ] && kill "$(cat /tmp/fleet_srv_pid)" 2>/dev/null; rm -f /tmp/fleet_srv_pid; } + +do_test(){ + local t="$1" img sip; img="$(img_of "$t")" + sip="$(detect_server_ip)"; [ -n "$sip" ] || die "cannot detect server IP (use --server-ip)" + echo "== TEST $t via $IP (workstation server http://$sip:$PORT) ==" + rmt "docker image inspect $img:latest >/dev/null 2>&1" || { echo ">> not installed yet — installing first"; do_install "$t"; } + push_env "$t" "$sip" + start_server "$t"; trap 'stop_server' EXIT + local rc; rc=$(rmt "curl -s -o /dev/null -w '%{http_code}' --max-time 6 http://$sip:$PORT/ping" || echo 000) + [ "$rc" = 200 ] || die "robot cannot reach workstation server (http $rc) — firewall on $sip:$PORT?" + echo " reachability OK (HTTP $rc)" + if [ "$t" = g1 ]; then + rmt "mkdir -p ~/$(rdir_of g1)/maps/sanad ~/$(rdir_of g1)/web_data/sanad/places + head -c 4096 /dev/urandom > ~/$(rdir_of g1)/maps/sanad/floor-test.db + printf '%s' '{\"dock\":{\"x\":1.2,\"y\":3.4,\"qz\":0,\"qw\":1}}' > ~/$(rdir_of g1)/web_data/sanad/places/floor-test.json + rm -f ~/$(rdir_of g1)/state/uploaded.json" + rmt "docker run --rm $(run_args g1) $img:latest --once --force" || true + else + echo ">> real DDS --once:"; rmt "docker run --rm $(run_args "$t") $img:latest --once" || true + echo ">> simulate --once:"; rmt "docker run --rm $(run_args "$t") $img:latest --simulate --once" || true + fi + sleep 1; echo; echo ">> workstation server received:" + local reqlog; reqlog="$(cat /tmp/fleet_last_reqlog)"; local result=0 + python3 - "$reqlog" "$t" <<'PY' && result=0 || result=$? +import json,sys,os +recs=[json.loads(l) for l in open(sys.argv[1])] if os.path.exists(sys.argv[1]) else [] +t=sys.argv[2] +for r in recs: + j=r.get("json") + print(" POST",r["path"],"auth="+("yes" if r["auth"].startswith("Bearer") else "NO")) + if j: print(" ",{k:j[k] for k in j if k!="db_base64"}) + if r.get("meta"): print(" meta:",r["meta"][:160]) +ok=any(x["path"].endswith("/map") and x["ctype"]=="multipart/form-data" for x in recs) if t=="g1" \ + else any(x["path"].endswith("/telemetry") for x in recs) +print(f"\n RESULT: {'PASS' if ok else 'FAIL'} ({len(recs)} request(s))") +sys.exit(0 if ok else 1) +PY + [ "${KEEP_SERVER:-0}" = 1 ] && { echo ">> --keep-server: server still up (pid $(cat /tmp/fleet_srv_pid 2>/dev/null))"; trap - EXIT; } + return $result +} + +# --------------------------- arg parsing --------------------------- # +CMD=""; ROBOT=""; IP=""; SERVER_IP=""; PORT=8799; TOKEN="test-token"; SN=""; USER_="unitree"; KEEP_SERVER=0 +POSA=() +while [ $# -gt 0 ]; do case "$1" in + --server-ip) SERVER_IP="$2"; shift 2;; + --port) PORT="$2"; shift 2;; + --token) TOKEN="$2"; shift 2;; + --sn) SN="$2"; shift 2;; + --user) USER_="$2"; shift 2;; + --keep-server) KEEP_SERVER=1; shift;; + -h|--help) grep -E '^#( |$)' "$0" | sed 's/^# \{0,1\}//'; exit 0;; + *) POSA+=("$1"); shift;; +esac; done +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 +} + +interactive(){ + echo "=== Sanad Fleet Agent Installer ===" + ROBOT="$(choose_type)"; [ -n "$ROBOT" ] || die "invalid robot type" + read -rp "Robot IP: " IP; [ -n "$IP" ] || die "no IP" + read -rp "SSH user [$USER_]: " u; [ -n "$u" ] && USER_="$u" + rmt true 2>/dev/null || die "cannot SSH to $USER_@$IP (key auth?)" + if installed "$ROBOT"; then + echo; echo ">> '$(img_of "$ROBOT")' is ALREADY INSTALLED on $IP." + echo " 1) show data 2) status 3) logs 4) reinstall 5) UNINSTALL 6) quit" + read -rp "Choice: " a + case "$a" in + 1) do_data "$ROBOT";; 2) do_status "$ROBOT";; 3) do_logs "$ROBOT";; + 4) prompt_install; do_install "$ROBOT";; + 5) read -rp "Really uninstall $(img_of "$ROBOT") from $IP? [y/N] " y; [ "$y" = y ] && do_uninstall "$ROBOT" || echo "cancelled";; + *) echo "bye";; + esac + else + echo; echo ">> not installed yet — let's install." + prompt_install + do_install "$ROBOT" + fi +} + +prompt_install(){ + local def="${ROBOT}_${IP##*.}" + 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}" + read -rp "Server port [$PORT]: " p; [ -n "$p" ] && PORT="$p" + read -rp "Device token [$TOKEN]: " tk; [ -n "$tk" ] && TOKEN="$tk" +} + +# --------------------------- dispatch --------------------------- # +[ -z "$CMD" ] && { interactive; exit $?; } +[ -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##*.}" + +case "$CMD" in + install) do_install "$ROBOT";; + uninstall) do_uninstall "$ROBOT";; + status) do_status "$ROBOT";; + data) do_data "$ROBOT";; + logs) do_logs "$ROBOT";; + test) do_test "$ROBOT";; + *) die "unknown command: $CMD (install|uninstall|status|data|logs|test)";; +esac diff --git a/fleet_test_server.py b/fleet_test_server.py new file mode 100755 index 0000000..8ba8885 --- /dev/null +++ b/fleet_test_server.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +"""fleet_test_server — stand-in for the YS Lootah fleet server, for deploy tests. + +Runs on the workstation ("assume my workstation is a server"). Accepts the two +ingest endpoints the agents POST to, logs each request (JSON line -> REQLOG) and +prints a live summary. GET /ping returns 200 for reachability checks. + + PORT=8799 REQLOG=/tmp/fleet_reqs.jsonl python3 fleet_test_server.py +""" +import json +import os +import sys +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer + +PORT = int(os.environ.get("PORT", "8799")) +REQLOG = os.environ.get("REQLOG", "/tmp/fleet_reqs.jsonl") +open(REQLOG, "w").close() + + +class H(BaseHTTPRequestHandler): + def _emit(self, rec): + with open(REQLOG, "a") as f: + f.write(json.dumps(rec) + "\n") + + def do_GET(self): + self.send_response(200) + self.end_headers() + self.wfile.write(b'{"ok":true,"server":"fleet_test_server"}') + + def do_POST(self): + n = int(self.headers.get("Content-Length", 0)) + body = self.rfile.read(n) + ctype = (self.headers.get("Content-Type", "") or "").split(";")[0] + rec = {"path": self.path, "ctype": ctype, + "auth": self.headers.get("Authorization", ""), "len": n} + summary = "" + if ctype == "application/json": + try: + d = json.loads(body) + if "db_base64" in d: + d["db_base64"] = f"[{len(d['db_base64'])} chars]" + rec["json"] = d + if self.path.endswith("/telemetry"): + summary = (f"battery={d.get('battery')} charging={d.get('charging')} " + f"status={d.get('status')} pos={d.get('position')} " + f"faults={len(d.get('faults',[]))} sn={d.get('sn')} mac={d.get('mac')}") + except Exception as e: + rec["json_err"] = str(e) + else: + txt = body.decode("latin-1") + i = txt.find('name="meta"') + if i != -1: + meta = txt[i:].split("\r\n\r\n", 1)[-1].split("\r\n", 1)[0] + rec["meta"] = meta + try: + m = json.loads(meta) + summary = (f"map={m.get('name')} format={m.get('format')} " + f"size={m.get('size_bytes')}B points={len(m.get('points',[]))} sn={m.get('sn')}") + except Exception: + pass + self._emit(rec) + print(f"[RECV] POST {self.path} {summary}", flush=True) + self.send_response(200) + self.end_headers() + self.wfile.write(b'{"ok":true}') + + def log_message(self, *a): + pass + + +if __name__ == "__main__": + print(f"fleet_test_server listening on 0.0.0.0:{PORT} (log -> {REQLOG})", flush=True) + try: + ThreadingHTTPServer(("0.0.0.0", PORT), H).serve_forever() + except KeyboardInterrupt: + sys.exit(0)