Update 2026-07-10 11:24:53
This commit is contained in:
commit
db757d0b55
4
.gitignore
vendored
Normal file
4
.gitignore
vendored
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
Logs/
|
||||||
|
*.log
|
||||||
94
README.md
Normal file
94
README.md
Normal file
@ -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 <g1|r1|go2> <ip> [--sn NAME] [--server-ip IP] [--port N] [--token TOK]
|
||||||
|
./fleet_install.sh data <g1|r1|go2> <ip> # show what it's currently sending
|
||||||
|
./fleet_install.sh status <g1|r1|go2> <ip>
|
||||||
|
./fleet_install.sh logs <g1|r1|go2> <ip>
|
||||||
|
./fleet_install.sh test <g1|r1|go2> <ip> # end-to-end vs workstation server
|
||||||
|
./fleet_install.sh uninstall <g1|r1|go2> <ip> # 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-<type>.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@<ip> 'systemctl --user status sanad-api-r1'
|
||||||
|
ssh unitree@<ip> '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/<robot>/*.db` + `web/data/<robot>/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 <device_token>`.
|
||||||
|
|
||||||
|
## Notes / follow-ups
|
||||||
|
|
||||||
|
- **Point at the real fleet server:** re-run `install … --server-ip <fleet> --token <real> --sn <id>`.
|
||||||
|
- **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.
|
||||||
5
agents/g1/.dockerignore
Normal file
5
agents/g1/.dockerignore
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
.env
|
||||||
|
data/
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
README.md
|
||||||
43
agents/g1/.env.example
Normal file
43
agents/g1/.env.example
Normal file
@ -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 (<MAPS_DIR>/<ROBOT>/*.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/<robot>/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
|
||||||
4
agents/g1/.gitignore
vendored
Normal file
4
agents/g1/.gitignore
vendored
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
.env
|
||||||
|
data/
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
19
agents/g1/Dockerfile
Normal file
19
agents/g1/Dockerfile
Normal file
@ -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"]
|
||||||
130
agents/g1/README.md
Normal file
130
agents/g1/README.md
Normal file
@ -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 <device_token>
|
||||||
|
```
|
||||||
|
|
||||||
|
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/<robot>/<name>.db`), with each map's
|
||||||
|
named places in `web/data/<robot>/places/<name>.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.
|
||||||
35
agents/g1/docker-compose.yml
Normal file
35
agents/g1/docker-compose.yml
Normal file
@ -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 <robot>/*.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
|
||||||
|
# <repo>/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 <robot>/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"
|
||||||
2
agents/g1/requirements.txt
Normal file
2
agents/g1/requirements.txt
Normal file
@ -0,0 +1,2 @@
|
|||||||
|
# sanad_api_g1 map uploader — deliberately tiny (no ROS, no DDS).
|
||||||
|
requests>=2.31,<3
|
||||||
485
agents/g1/sanad_api_g1.py
Normal file
485
agents/g1/sanad_api_g1.py
Normal file
@ -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/<robot>/<name>.db`` + a ``maps_meta.json`` sidecar), with each map's
|
||||||
|
named places kept in ``web/data/<robot>/places/<name>.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 <robot>/*.db and/or *.db)
|
||||||
|
DATA_DIR mounted web_nav3 web/data dir (per-map places live under <robot>/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): {"<name>": {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/<robot>/ 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())
|
||||||
4
agents/go2/.dockerignore
Normal file
4
agents/go2/.dockerignore
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
.env
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
README.md
|
||||||
29
agents/go2/.env.example
Normal file
29
agents/go2/.env.example
Normal file
@ -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
|
||||||
3
agents/go2/.gitignore
vendored
Normal file
3
agents/go2/.gitignore
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
.env
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
47
agents/go2/Dockerfile
Normal file
47
agents/go2/Dockerfile
Normal file
@ -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"]
|
||||||
42
agents/go2/README.md
Normal file
42
agents/go2/README.md
Normal file
@ -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 <device_token>
|
||||||
|
{ "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 <ip> --sn go2_<id>
|
||||||
|
../../fleet_install.sh data go2 <ip>
|
||||||
|
# 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.
|
||||||
20
agents/go2/docker-compose.yml
Normal file
20
agents/go2/docker-compose.yml
Normal file
@ -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
|
||||||
7
agents/go2/requirements.txt
Normal file
7
agents/go2/requirements.txt
Normal file
@ -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.
|
||||||
409
agents/go2/sanad_api_go2.py
Normal file
409
agents/go2/sanad_api_go2.py
Normal file
@ -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())
|
||||||
BIN
agents/go2/vendor/crc_aarch64.so
vendored
Normal file
BIN
agents/go2/vendor/crc_aarch64.so
vendored
Normal file
Binary file not shown.
BIN
agents/go2/vendor/crc_amd64.so
vendored
Normal file
BIN
agents/go2/vendor/crc_amd64.so
vendored
Normal file
Binary file not shown.
BIN
agents/go2/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
BIN
agents/go2/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
Binary file not shown.
4
agents/r1/.dockerignore
Normal file
4
agents/r1/.dockerignore
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
.env
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
README.md
|
||||||
35
agents/r1/.env.example
Normal file
35
agents/r1/.env.example
Normal file
@ -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
|
||||||
3
agents/r1/.gitignore
vendored
Normal file
3
agents/r1/.gitignore
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
.env
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
47
agents/r1/Dockerfile
Normal file
47
agents/r1/Dockerfile
Normal file
@ -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"]
|
||||||
82
agents/r1/README.md
Normal file
82
agents/r1/README.md
Normal file
@ -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 <device_token>
|
||||||
|
{ "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.
|
||||||
21
agents/r1/docker-compose.yml
Normal file
21
agents/r1/docker-compose.yml
Normal file
@ -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"]
|
||||||
7
agents/r1/requirements.txt
Normal file
7
agents/r1/requirements.txt
Normal file
@ -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.
|
||||||
529
agents/r1/sanad_api_r1.py
Normal file
529
agents/r1/sanad_api_r1.py
Normal file
@ -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())
|
||||||
BIN
agents/r1/vendor/crc_aarch64.so
vendored
Normal file
BIN
agents/r1/vendor/crc_aarch64.so
vendored
Normal file
Binary file not shown.
BIN
agents/r1/vendor/crc_amd64.so
vendored
Normal file
BIN
agents/r1/vendor/crc_amd64.so
vendored
Normal file
Binary file not shown.
BIN
agents/r1/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
BIN
agents/r1/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
Binary file not shown.
265
fleet_install.sh
Executable file
265
fleet_install.sh
Executable file
@ -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 <g1|r1|go2> <ip> [--sn NAME] [--server-ip IP] [--port N] [--token TOK] [--user U]
|
||||||
|
# ./fleet_install.sh uninstall <g1|r1|go2> <ip> [--user U]
|
||||||
|
# ./fleet_install.sh status <g1|r1|go2> <ip>
|
||||||
|
# ./fleet_install.sh data <g1|r1|go2> <ip> # show the data it is sending
|
||||||
|
# ./fleet_install.sh logs <g1|r1|go2> <ip>
|
||||||
|
# ./fleet_install.sh test <g1|r1|go2> <ip> [--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" <<EOF
|
||||||
|
SERVER_URL=http://$sip:$PORT
|
||||||
|
DEVICE_TOKEN=$TOKEN
|
||||||
|
SN=$SN
|
||||||
|
ROBOT=sanad
|
||||||
|
MAPS_DIR=/data/maps
|
||||||
|
DATA_DIR=/data/web_data
|
||||||
|
STATE_DIR=/data/state
|
||||||
|
MAP_UPLOAD_MODE=multipart
|
||||||
|
VERIFY_TLS=0
|
||||||
|
POLL_INTERVAL=30
|
||||||
|
EOF
|
||||||
|
else
|
||||||
|
local iface=eth0; [ "$t" = r1 ] && iface=eth10
|
||||||
|
rmt_in "cat > ~/$rdir/.env" <<EOF
|
||||||
|
SERVER_URL=http://$sip:$PORT
|
||||||
|
DEVICE_TOKEN=$TOKEN
|
||||||
|
SN=$SN
|
||||||
|
DDS_INTERFACE=$iface
|
||||||
|
DDS_DOMAIN=0
|
||||||
|
VERIFY_TLS=0
|
||||||
|
POLL_INTERVAL=2
|
||||||
|
EOF
|
||||||
|
fi
|
||||||
|
}
|
||||||
|
|
||||||
|
installed(){ # 0 = installed (unit file OR container present)
|
||||||
|
rmt "test -f ~/.config/systemd/user/$(unit_of "$1") || docker container inspect $(img_of "$1") >/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" <<EOF
|
||||||
|
[Unit]
|
||||||
|
Description=Sanad Fleet Agent ($t) -> $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 <install|uninstall|status|data|logs|test> <g1|r1|go2> <ip> [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
|
||||||
76
fleet_test_server.py
Executable file
76
fleet_test_server.py
Executable file
@ -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)
|
||||||
Loading…
x
Reference in New Issue
Block a user