Update 2026-07-10 12:16:04
This commit is contained in:
parent
6c864d91ae
commit
0bb7d3ed83
@ -42,8 +42,8 @@ repo currently implements the **bold** ones; the rest are documented for later.
|
||||
|
||||
| endpoint | method | agent | status |
|
||||
|---|---|---|---|
|
||||
| **`/api/v1/fleet/ingest/telemetry`** | POST | R1, Go2 | ✅ implemented |
|
||||
| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1 | ✅ implemented |
|
||||
| **`/api/v1/fleet/ingest/telemetry`** | POST | G1 (`g1t`), R1, Go2 | ✅ implemented |
|
||||
| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1 (`g1`) | ✅ implemented |
|
||||
| `/api/v1/fleet/ingest/{sn}/commands` | GET | — | ⏳ spec'd, not built |
|
||||
| `/api/v1/fleet/ingest/commands/{id}/ack` | POST | — | ⏳ |
|
||||
| `/api/v1/fleet/ingest/{sn}/alert` | POST | — | ⏳ (critical faults; ordinary ones ride in telemetry `faults[]`) |
|
||||
|
||||
26
README.md
26
README.md
@ -61,6 +61,10 @@ Project/fleet/
|
||||
│ ├── requirements.txt
|
||||
│ ├── docker-compose.yml (manual local use only)
|
||||
│ └── .env.example
|
||||
├── g1t/ ← TELEMETRY (G1, unitree_hg)
|
||||
│ ├── sanad_api_g1t.py
|
||||
│ ├── Dockerfile (DDS) + vendor/
|
||||
│ └── .env.example
|
||||
├── r1/ ← TELEMETRY (unitree_hg)
|
||||
│ ├── sanad_api_r1.py
|
||||
│ ├── Dockerfile (DDS: CycloneDDS + unitree_sdk2py)
|
||||
@ -80,11 +84,17 @@ agent to the robot; you never edit files on the robot.
|
||||
|
||||
## 3. The three agents
|
||||
|
||||
| agent | robot | endpoint | what it sends | cadence |
|
||||
| agent (type) | robot | endpoint | what it sends | cadence |
|
||||
|---|---|---|---|---|
|
||||
| `sanad_api_g1` | Unitree G1 | `POST /api/v1/fleet/ingest/{sn}/map` | RTAB-Map `.db` + places (points) | on change (~30 s scan) |
|
||||
| `sanad_api_r1` | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | battery, charging, status, position, faults, mac | every ~2 s |
|
||||
| `sanad_api_go2` | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same as R1 | every ~2 s |
|
||||
| `sanad_api_g1` (`g1`) | Unitree G1 | `POST /api/v1/fleet/ingest/{sn}/map` | RTAB-Map `.db` + places (points) | on change (~30 s scan) |
|
||||
| `sanad_api_g1t` (`g1t`) | Unitree G1 | `POST /api/v1/fleet/ingest/telemetry` | battery, charging, status, position, faults, mac | every ~2 s |
|
||||
| `sanad_api_r1` (`r1`) | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s |
|
||||
| `sanad_api_go2` (`go2`) | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s |
|
||||
|
||||
The G1 runs **both** `g1` (map) and `g1t` (telemetry) — two independent
|
||||
containers/services, both keyed by the **same `sn`** (same physical robot). `g1t`
|
||||
uses the same `unitree_hg` DDS as R1, and reads `position` from the firmware odom
|
||||
topic (`rt/lf/odommodestate`) over DDS.
|
||||
|
||||
All requests carry `Authorization: Bearer <device_token>`. Full payload schemas
|
||||
and the data pipeline are in [PIPELINE.md](PIPELINE.md).
|
||||
@ -345,11 +355,11 @@ bridge).
|
||||
|
||||
## 12. Fleet inventory
|
||||
|
||||
| robot | agent | IP | SSH | arch | DDS iface | SN |
|
||||
| robot | agent(s) | IP | SSH | arch | DDS iface | SN |
|
||||
|---|---|---|---|---|---|---|
|
||||
| G1 | map | `10.255.254.58` | `unitree` (key) | arm64 | — | `g1_7892` |
|
||||
| R1 | telemetry | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `r1_82` |
|
||||
| Go2 | telemetry | *(TBD)* | `unitree` | arm64 | `eth0` | — |
|
||||
| G1 | map (`g1`) + telemetry (`g1t`) | `10.255.254.58` | `unitree` (key) | arm64 | `eth0` | `g1_7892` |
|
||||
| R1 | telemetry (`r1`) | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `r1_82` |
|
||||
| Go2 | telemetry (`go2`) | *(TBD)* | `unitree` | arm64 | `eth0` | — |
|
||||
|
||||
Workstation (deploy host + test fleet server): **`10.255.254.83`** (`wlp4s0`).
|
||||
|
||||
|
||||
4
agents/g1t/.dockerignore
Normal file
4
agents/g1t/.dockerignore
Normal file
@ -0,0 +1,4 @@
|
||||
.env
|
||||
__pycache__/
|
||||
*.pyc
|
||||
README.md
|
||||
35
agents/g1t/.env.example
Normal file
35
agents/g1t/.env.example
Normal file
@ -0,0 +1,35 @@
|
||||
# sanad_api_g1t — copy to .env and fill in. (G1 telemetry; sibling of the G1 map agent.)
|
||||
|
||||
# ── fleet server (REQUIRED — YS Lootah gives you these two) ──────────────────
|
||||
SERVER_URL=https://fleet.example.com
|
||||
DEVICE_TOKEN=REPLACE_WITH_DEVICE_TOKEN
|
||||
|
||||
# ── identity ─────────────────────────────────────────────────────────────────
|
||||
# Use the SAME sn as the G1 map agent — it's the same physical robot.
|
||||
SN=g1_7892
|
||||
|
||||
# ── DDS (reading robot state — unitree_hg, same family as R1) ────────────────
|
||||
# G1 DDS link is usually eth0.
|
||||
DDS_INTERFACE=eth0
|
||||
DDS_DOMAIN=0
|
||||
# MAC_INTERFACE=eth0
|
||||
|
||||
# ── status / position ────────────────────────────────────────────────────────
|
||||
# Read the loco FSM for a richer status (READ-ONLY GET RPC, never moves).
|
||||
# 0 = derive status from battery + joint motion (safe default).
|
||||
G1_READ_FSM=0
|
||||
# Position source:
|
||||
# odom (default) — firmware leg-odometry x,y from rt/lf/odommodestate (drifts, resets at boot)
|
||||
# rosbridge — map-frame /odom over rosbridge (needs the nav bringup + ROSBRIDGE_URL)
|
||||
# none — omit position
|
||||
G1_POSITION_SOURCE=odom
|
||||
ROSBRIDGE_URL=ws://127.0.0.1:9090
|
||||
|
||||
# ── fault thresholds ─────────────────────────────────────────────────────────
|
||||
LOW_SOC=15
|
||||
MOTOR_TEMP_MAX=85
|
||||
|
||||
# ── cadence / transport ──────────────────────────────────────────────────────
|
||||
POLL_INTERVAL=2
|
||||
VERIFY_TLS=1
|
||||
HTTP_TIMEOUT=10
|
||||
3
agents/g1t/.gitignore
vendored
Normal file
3
agents/g1t/.gitignore
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
.env
|
||||
__pycache__/
|
||||
*.pyc
|
||||
46
agents/g1t/Dockerfile
Normal file
46
agents/g1t/Dockerfile
Normal file
@ -0,0 +1,46 @@
|
||||
# sanad_api_g1t — G1 fleet telemetry agent (DDS: rt/lf/bmsstate + rt/lowstate + odom).
|
||||
#
|
||||
# BUILD ON THE G1 JETSON (native arm64). DDS stack: prebuilt CycloneDDS (apt) +
|
||||
# the CycloneDDS python binding (pip) + the vendored unitree_sdk2py wheel. Run with
|
||||
# --network host so the robot's DDS is visible (see docker-compose.yml).
|
||||
FROM python:3.10-slim-bookworm
|
||||
|
||||
# CycloneDDS C lib + idlc (bookworm ships 0.10.2 — matches the robot), build tools
|
||||
# for the python binding, iproute2 for iface checks, libgomp1 for the TLS guard.
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
build-essential cmake \
|
||||
cyclonedds-dev cyclonedds-tools \
|
||||
iproute2 libgomp1 ca-certificates \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# aarch64 "static TLS block" guard (harmless on amd64).
|
||||
ENV LD_PRELOAD=libgomp.so.1
|
||||
# cyclonedds build helper looks for libddsc.so under $CYCLONEDDS_HOME/lib; Debian
|
||||
# installs into the multiarch dir — symlink whatever arch built into /usr/lib.
|
||||
ENV CYCLONEDDS_HOME=/usr
|
||||
RUN set -e; for lib in libddsc.so libcycloneddsidl.so; do \
|
||||
f=$(ls /usr/lib/*/"$lib" 2>/dev/null | head -1); \
|
||||
[ -n "$f" ] && ln -sf "$f" /usr/lib/"$lib" || true; \
|
||||
done
|
||||
|
||||
WORKDIR /app
|
||||
COPY requirements.txt .
|
||||
RUN pip install --no-cache-dir -r requirements.txt
|
||||
# CycloneDDS python binding, compiled against the C lib above. 0.10.2's build
|
||||
# helper imports wheel.bdist_wheel (removed in wheel>=0.46) → pin the toolchain.
|
||||
RUN pip install --no-cache-dir "setuptools<80" "wheel<0.46" \
|
||||
&& pip install --no-cache-dir --no-build-isolation cyclonedds==0.10.2
|
||||
|
||||
# Unitree SDK (vendored wheel — not on PyPI) + native crc lib (wheel omits it).
|
||||
# Both arch crc libs are copied; unitree_sdk2py loads the one matching the image.
|
||||
COPY vendor/unitree_sdk2py-*.whl /tmp/
|
||||
RUN pip install --no-cache-dir --no-deps /tmp/unitree_sdk2py-*.whl
|
||||
COPY vendor/crc_aarch64.so vendor/crc_amd64.so \
|
||||
/usr/local/lib/python3.10/site-packages/unitree_sdk2py/utils/lib/
|
||||
|
||||
COPY sanad_api_g1t.py .
|
||||
|
||||
ENV PYTHONUNBUFFERED=1 \
|
||||
DDS_INTERFACE=eth0 \
|
||||
DDS_DOMAIN=0
|
||||
ENTRYPOINT ["python", "-u", "sanad_api_g1t.py"]
|
||||
40
agents/g1t/README.md
Normal file
40
agents/g1t/README.md
Normal file
@ -0,0 +1,40 @@
|
||||
# sanad_api_g1t — G1 fleet **telemetry** agent
|
||||
|
||||
Streams the Unitree **G1**'s live status to the YS Lootah fleet server. It's the
|
||||
**telemetry** sibling of the G1 **map** agent (`../g1`) — the G1 runs both, each
|
||||
its own container/service, both keyed by the **same `sn`** (same physical robot).
|
||||
|
||||
```
|
||||
POST {SERVER_URL}/api/v1/fleet/ingest/telemetry Authorization: Bearer <device_token>
|
||||
{ "sn":"g1_7892", "mac":"…", "battery":74, "charging":false,
|
||||
"status":"idle", "position":{"x":…,"y":…}|null, "faults":[], "ts":… }
|
||||
```
|
||||
|
||||
Every ~2 s; a **heartbeat** (`battery:null, status:offline`) when DDS is silent.
|
||||
|
||||
## Data sources (G1, `unitree_hg` DDS — same family as R1)
|
||||
|
||||
| field | source |
|
||||
|---|---|
|
||||
| `battery`, `charging` | `rt/lf/bmsstate` (`BmsState_`): `soc` 0–100; charging from `current` sign |
|
||||
| `faults[]` | `rt/lowstate` (`LowState_`): motor temps + staleness |
|
||||
| `position` `{x,y}` | **`rt/lf/odommodestate`** (`SportModeState_`) — firmware leg-odometry over DDS, no ROS (odom frame: drifts, resets at boot). Optional map-frame via `rosbridge`. |
|
||||
| `status` | derived (charging/moving/idle/offline); optional loco **FSM** (`G1_READ_FSM=1`, ids **200** ready / 4 stand / 2 squat / 702 lie2stand — GET-only) |
|
||||
| `mac` | primary NIC |
|
||||
|
||||
> **Safety:** read-only — never commands motion.
|
||||
|
||||
## Run / test
|
||||
|
||||
```bash
|
||||
# via the fleet installer (adds a 'g1t' type):
|
||||
../../fleet_install.sh install g1t <ip> --sn g1_7892
|
||||
../../fleet_install.sh data g1t <ip>
|
||||
|
||||
# bare simulate (no robot):
|
||||
SERVER_URL=… DEVICE_TOKEN=… SN=g1_7892 python3 sanad_api_g1t.py --simulate -v
|
||||
```
|
||||
|
||||
Build on the G1 Jetson (arm64); bundles CycloneDDS + the vendored `unitree_sdk2py`
|
||||
wheel (`vendor/`). `network_mode: host` for DDS. Coexists with the map agent and
|
||||
with a running Sanad (DDS allows many readers of the same topics).
|
||||
20
agents/g1t/docker-compose.yml
Normal file
20
agents/g1t/docker-compose.yml
Normal file
@ -0,0 +1,20 @@
|
||||
# sanad_api_g1t — standalone G1 fleet TELEMETRY agent.
|
||||
# (The fleet installer deploys via docker build/run + systemd; this compose is
|
||||
# for manual local use.)
|
||||
#
|
||||
# cp .env.example .env # SERVER_URL + DEVICE_TOKEN + SN + DDS_INTERFACE
|
||||
# docker compose up -d --build
|
||||
#
|
||||
# Reads the G1's DDS state (rt/lf/bmsstate + rt/lowstate + rt/lf/odommodestate)
|
||||
# and POSTs telemetry every ~2s. network_mode: host is REQUIRED for DDS.
|
||||
services:
|
||||
sanad-api-g1t:
|
||||
build:
|
||||
context: .
|
||||
dockerfile: Dockerfile
|
||||
image: "${SANAD_API_G1T_IMAGE:-sanad-api-g1t:latest}"
|
||||
container_name: sanad-api-g1t
|
||||
network_mode: host
|
||||
restart: unless-stopped
|
||||
env_file: .env
|
||||
# command: ["--simulate"] # synthetic state, no robot
|
||||
7
agents/g1t/requirements.txt
Normal file
7
agents/g1t/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.
|
||||
467
agents/g1t/sanad_api_g1t.py
Normal file
467
agents/g1t/sanad_api_g1t.py
Normal file
@ -0,0 +1,467 @@
|
||||
#!/usr/bin/env python3
|
||||
"""sanad_api_g1t — G1 fleet TELEMETRY agent.
|
||||
|
||||
Pushes the Unitree **G1**'s live status to the YS Lootah fleet server:
|
||||
|
||||
POST {SERVER_URL}/api/v1/fleet/ingest/telemetry (Bearer device token)
|
||||
body: { "sn", "mac", "battery", "charging", "status", "position":{x,y}, "faults":[] }
|
||||
|
||||
This is the telemetry sibling of the G1 MAP uploader (sanad_api_g1) — the G1 runs
|
||||
both, each its own container/service, both keyed by the SAME `sn` (same robot).
|
||||
|
||||
DATA SOURCES (Unitree G1, unitree_hg DDS — same family as the R1)
|
||||
----------------------------------------------------------------
|
||||
battery / charging : rt/lf/bmsstate (BmsState_) soc 0-100; charging = current>+0.05A
|
||||
faults / liveness : rt/lowstate (LowState_) motor temps + message staleness
|
||||
position {x,y} : rt/lf/odommodestate (SportModeState_, unitree_go idl) —
|
||||
firmware leg-odometry x,y over DDS, no ROS. Odom frame:
|
||||
resets at boot, drifts. (Map-frame via rosbridge/TF optional.)
|
||||
status : G1 loco FSM via GET RPC 7001 (ids 200 walk-ready / 4 stand /
|
||||
2 squat / 702 lie2stand) — READ-ONLY, optional (G1_READ_FSM=1).
|
||||
Default derives status from BMS + motion.
|
||||
mac : primary NIC hardware address.
|
||||
|
||||
NO ROS. DDS via unitree_sdk2py. Degrades to heartbeats if unitree_sdk2py is
|
||||
unavailable; --simulate tests the upload path without a robot. Read-only: never
|
||||
commands motion (only the GET_FSM_ID RPC is ever issued).
|
||||
|
||||
CONFIG — environment (see .env.example)
|
||||
---------------------------------------
|
||||
SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required)
|
||||
SN this robot's fleet id (SAME as the G1 map agent) default g1_0000
|
||||
DDS_INTERFACE robot network iface for DDS default eth0
|
||||
DDS_DOMAIN DDS domain id default 0
|
||||
MAC_INTERFACE iface whose MAC to report default = DDS_INTERFACE
|
||||
G1_READ_FSM 1 = read loco FSM for status default 0
|
||||
G1_POSITION_SOURCE odom | rosbridge | none default odom
|
||||
ROSBRIDGE_URL ws://127.0.0.1:9090 (position) default ws://127.0.0.1:9090
|
||||
LOW_SOC / MOTOR_TEMP_MAX fault thresholds default 15 / 85
|
||||
POLL_INTERVAL seconds between posts default 2
|
||||
VERIFY_TLS / HTTP_TIMEOUT TLS verify (1) / timeout (10)
|
||||
|
||||
CLI: --simulate | --once | --dry-run | --interval N | -v
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
import requests
|
||||
|
||||
log = logging.getLogger("sanad_api_g1t")
|
||||
|
||||
|
||||
def _load_dotenv(path: str = ".env") -> None:
|
||||
p = Path(path)
|
||||
if not p.exists():
|
||||
return
|
||||
for line in p.read_text().splitlines():
|
||||
line = line.strip()
|
||||
if not line or line.startswith("#") or "=" not in line:
|
||||
continue
|
||||
k, _, v = line.partition("=")
|
||||
os.environ.setdefault(k.strip(), v.strip().strip('"').strip("'"))
|
||||
|
||||
|
||||
def _env(name: str, default: str = "") -> str:
|
||||
return os.environ.get(name, default).strip()
|
||||
|
||||
|
||||
def _env_bool(name: str, default: bool) -> bool:
|
||||
return _env(name, "1" if default else "0").lower() in ("1", "true", "yes", "on")
|
||||
|
||||
|
||||
@dataclass
|
||||
class Config:
|
||||
server_url: str
|
||||
device_token: str
|
||||
sn: str
|
||||
dds_interface: str
|
||||
dds_domain: int
|
||||
mac_interface: str
|
||||
read_fsm: bool
|
||||
position_source: str
|
||||
rosbridge_url: str
|
||||
low_soc: int
|
||||
motor_temp_max: float
|
||||
poll_interval: float
|
||||
endpoint: str
|
||||
verify_tls: bool
|
||||
http_timeout: float
|
||||
|
||||
@classmethod
|
||||
def from_env(cls) -> "Config":
|
||||
server = _env("SERVER_URL").rstrip("/")
|
||||
token = _env("DEVICE_TOKEN")
|
||||
missing = [n for n, v in (("SERVER_URL", server), ("DEVICE_TOKEN", token)) if not v]
|
||||
if missing:
|
||||
raise SystemExit(f"[config] missing required env: {', '.join(missing)}")
|
||||
iface = _env("DDS_INTERFACE", "eth0")
|
||||
return cls(
|
||||
server_url=server, device_token=token,
|
||||
sn=_env("SN", "g1_0000"),
|
||||
dds_interface=iface, dds_domain=int(_env("DDS_DOMAIN", "0")),
|
||||
mac_interface=_env("MAC_INTERFACE", iface),
|
||||
read_fsm=_env_bool("G1_READ_FSM", False),
|
||||
position_source=_env("G1_POSITION_SOURCE", "odom").lower(),
|
||||
rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"),
|
||||
low_soc=int(_env("LOW_SOC", "15")),
|
||||
motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")),
|
||||
poll_interval=float(_env("POLL_INTERVAL", "2")),
|
||||
endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"),
|
||||
verify_tls=_env_bool("VERIFY_TLS", True),
|
||||
http_timeout=float(_env("HTTP_TIMEOUT", "10")),
|
||||
)
|
||||
|
||||
def telemetry_url(self) -> str:
|
||||
return self.server_url + self.endpoint
|
||||
|
||||
def auth_headers(self) -> Dict[str, str]:
|
||||
return {"Authorization": f"Bearer {self.device_token}"}
|
||||
|
||||
|
||||
def read_mac(interface: str) -> str:
|
||||
p = Path(f"/sys/class/net/{interface}/address")
|
||||
try:
|
||||
mac = p.read_text().strip()
|
||||
if mac and mac != "00:00:00:00:00:00":
|
||||
return mac.lower()
|
||||
except Exception:
|
||||
pass
|
||||
n = uuid.getnode()
|
||||
return ":".join(f"{(n >> b) & 0xff:02x}" for b in range(40, -1, -8))
|
||||
|
||||
|
||||
class DDSReader:
|
||||
"""Subscribes rt/lf/bmsstate + rt/lowstate (+ rt/lf/odommodestate for position).
|
||||
Passive reads; the only RPC ever issued is GET_FSM_ID."""
|
||||
|
||||
def __init__(self, cfg: Config):
|
||||
self.cfg = cfg
|
||||
self._lock = threading.Lock()
|
||||
self._bms: Optional[Dict[str, Any]] = None
|
||||
self._bms_ts = 0.0
|
||||
self._low_ts = 0.0
|
||||
self._temps: List[float] = []
|
||||
self._max_dq = 0.0
|
||||
self._xy: Optional[Dict[str, float]] = None
|
||||
self._loco = None
|
||||
self.ok = False
|
||||
self._start()
|
||||
|
||||
def _start(self) -> None:
|
||||
try:
|
||||
from unitree_sdk2py.core.channel import (
|
||||
ChannelFactoryInitialize, ChannelSubscriber)
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
|
||||
try:
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import BmsState_
|
||||
except Exception:
|
||||
BmsState_ = None
|
||||
SportModeState_ = None
|
||||
if self.cfg.position_source == "odom":
|
||||
try:
|
||||
# G1 firmware publishes odom as unitree_go SportModeState_.
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
||||
except Exception:
|
||||
SportModeState_ = None
|
||||
except Exception as e:
|
||||
log.warning("unitree_sdk2py unavailable (%s) — telemetry runs in heartbeat mode", e)
|
||||
return
|
||||
try:
|
||||
ChannelFactoryInitialize(self.cfg.dds_domain, self.cfg.dds_interface)
|
||||
self._low_sub = ChannelSubscriber("rt/lowstate", LowState_)
|
||||
self._low_sub.Init(self._on_low, 10)
|
||||
if BmsState_ is not None:
|
||||
self._bms_sub = ChannelSubscriber("rt/lf/bmsstate", BmsState_)
|
||||
self._bms_sub.Init(self._on_bms, 10)
|
||||
else:
|
||||
log.warning("BmsState_ not in this unitree_sdk2py — battery will be null")
|
||||
if SportModeState_ is not None:
|
||||
self._odom_sub = ChannelSubscriber("rt/lf/odommodestate", SportModeState_)
|
||||
self._odom_sub.Init(self._on_odom, 10)
|
||||
if self.cfg.read_fsm:
|
||||
self._init_loco()
|
||||
self.ok = True
|
||||
log.info("DDS up: domain=%d iface=%s (bmsstate + lowstate%s)",
|
||||
self.cfg.dds_domain, self.cfg.dds_interface,
|
||||
" + odom" if SportModeState_ is not None else "")
|
||||
except Exception as e:
|
||||
log.warning("DDS init failed (%s) — heartbeat mode", e)
|
||||
|
||||
def _init_loco(self) -> None:
|
||||
try:
|
||||
from unitree_sdk2py.rpc.client import Client # type: ignore
|
||||
except Exception as e:
|
||||
log.warning("loco RPC client unavailable (%s) — status from BMS/motion only", e)
|
||||
return
|
||||
try:
|
||||
c = Client("loco", 0); c.Init(); c.SetTimeout(3.0)
|
||||
self._loco = c
|
||||
log.info("loco FSM read enabled (GET-only, no motion)")
|
||||
except Exception as e:
|
||||
log.warning("loco client init failed (%s) — status from BMS/motion only", e)
|
||||
self._loco = None
|
||||
|
||||
def _on_bms(self, msg) -> None:
|
||||
try:
|
||||
soc = int(getattr(msg, "soc", 0) or 0)
|
||||
cur_mA = int(getattr(msg, "current", 0) or 0)
|
||||
with self._lock:
|
||||
self._bms = {"soc": max(0, min(100, soc)), "current_a": round(cur_mA / 1000.0, 2)}
|
||||
self._bms_ts = time.monotonic()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _on_low(self, msg) -> None:
|
||||
try:
|
||||
temps: List[float] = []
|
||||
max_dq = 0.0
|
||||
for m in (getattr(msg, "motor_state", None) or []):
|
||||
t = getattr(m, "temperature", None)
|
||||
if t is not None:
|
||||
try:
|
||||
vals = [float(x) for x in t] if hasattr(t, "__iter__") else [float(t)]
|
||||
temps.extend(v for v in vals if -40 <= v <= 200)
|
||||
except Exception:
|
||||
pass
|
||||
dq = getattr(m, "dq", None)
|
||||
if dq is not None:
|
||||
try:
|
||||
max_dq = max(max_dq, abs(float(dq)))
|
||||
except Exception:
|
||||
pass
|
||||
with self._lock:
|
||||
self._low_ts = time.monotonic()
|
||||
self._temps = temps
|
||||
self._max_dq = max_dq
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _on_odom(self, msg) -> None:
|
||||
try:
|
||||
pos = getattr(msg, "position", None)
|
||||
if pos is not None and len(pos) >= 2:
|
||||
with self._lock:
|
||||
self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)}
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def snapshot(self) -> Dict[str, Any]:
|
||||
with self._lock:
|
||||
now = time.monotonic()
|
||||
return {
|
||||
"bms": dict(self._bms) if self._bms else None,
|
||||
"low_age": (now - self._low_ts) if self._low_ts else None,
|
||||
"temps": list(self._temps),
|
||||
"max_dq": self._max_dq,
|
||||
"xy": dict(self._xy) if self._xy else None,
|
||||
}
|
||||
|
||||
def fsm_id(self) -> Optional[int]:
|
||||
if not self._loco:
|
||||
return None
|
||||
try:
|
||||
code, data = self._loco._Call(7001, "{}") # GET_FSM_ID — read-only
|
||||
if code == 0 and data:
|
||||
return int(json.loads(data).get("data", data)) if data.strip().startswith("{") else int(data)
|
||||
except Exception as e:
|
||||
log.debug("fsm read failed: %s", e)
|
||||
return None
|
||||
|
||||
|
||||
# G1 FSM ids (differ from the R1's): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand.
|
||||
_FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"}
|
||||
|
||||
|
||||
class RosbridgePosition:
|
||||
def __init__(self, cfg: Config):
|
||||
self.cfg = cfg
|
||||
self._xy: Optional[Dict[str, float]] = None
|
||||
self._lock = threading.Lock()
|
||||
self._stop = False
|
||||
try:
|
||||
import websocket # noqa: F401
|
||||
except Exception as e:
|
||||
log.warning("websocket-client absent (%s) — rosbridge position disabled", e)
|
||||
self._ok = False
|
||||
return
|
||||
self._ok = True
|
||||
threading.Thread(target=self._run, daemon=True).start()
|
||||
|
||||
def _run(self) -> None:
|
||||
import websocket
|
||||
sub = json.dumps({"op": "subscribe", "topic": "/odom",
|
||||
"type": "nav_msgs/Odometry", "throttle_rate": 500})
|
||||
while not self._stop:
|
||||
try:
|
||||
ws = websocket.create_connection(self.cfg.rosbridge_url, timeout=5)
|
||||
ws.send(sub)
|
||||
while not self._stop:
|
||||
msg = json.loads(ws.recv())
|
||||
pos = (((msg.get("msg") or {}).get("pose") or {}).get("pose") or {}).get("position")
|
||||
if pos:
|
||||
with self._lock:
|
||||
self._xy = {"x": round(float(pos["x"]), 3), "y": round(float(pos["y"]), 3)}
|
||||
except Exception as e:
|
||||
log.debug("rosbridge position reconnect: %s", e)
|
||||
time.sleep(3)
|
||||
|
||||
def get(self) -> Optional[Dict[str, float]]:
|
||||
with self._lock:
|
||||
return dict(self._xy) if self._xy else None
|
||||
|
||||
|
||||
def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]:
|
||||
faults: List[Dict[str, Any]] = []
|
||||
bms = snap.get("bms")
|
||||
if bms and bms.get("soc", 100) <= cfg.low_soc:
|
||||
faults.append({"code": "LOW_BATTERY", "severity": "warning", "message": f"battery {bms['soc']}%"})
|
||||
temps = snap.get("temps") or []
|
||||
if temps and max(temps) >= cfg.motor_temp_max:
|
||||
faults.append({"code": "MOTOR_OVERTEMP", "severity": "warning", "message": f"motor temp {max(temps):.0f}C"})
|
||||
if snap.get("low_age") is not None and snap["low_age"] > 3.0:
|
||||
faults.append({"code": "COMMS_STALE", "severity": "critical", "message": f"no rt/lowstate for {snap['low_age']:.0f}s"})
|
||||
return faults
|
||||
|
||||
|
||||
def derive_status(cfg: Config, snap: Dict[str, Any], fsm: Optional[int]) -> str:
|
||||
base = _FSM_STATUS.get(fsm) if fsm is not None else None
|
||||
bms = snap.get("bms")
|
||||
charging = bool(bms and bms.get("current_a", 0.0) > 0.05)
|
||||
alive = snap.get("low_age") is not None and snap["low_age"] <= 3.0
|
||||
if not alive and bms is None:
|
||||
return "offline"
|
||||
if charging:
|
||||
return "charging"
|
||||
if snap.get("max_dq", 0.0) > 0.15:
|
||||
return "moving"
|
||||
return base or "idle"
|
||||
|
||||
|
||||
def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
|
||||
pos: Optional[RosbridgePosition],
|
||||
sim: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
|
||||
if sim is not None:
|
||||
snap = {"bms": {"soc": sim["battery"], "current_a": 0.5 if sim["charging"] else -0.3},
|
||||
"low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0),
|
||||
"xy": sim.get("position")}
|
||||
fsm = sim.get("fsm")
|
||||
else:
|
||||
snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0, "xy": None}
|
||||
fsm = reader.fsm_id() if (reader and cfg.read_fsm) else None
|
||||
|
||||
bms = snap.get("bms")
|
||||
battery = bms["soc"] if bms else None
|
||||
charging = bool(bms and bms.get("current_a", 0.0) > 0.05)
|
||||
status = derive_status(cfg, snap, fsm)
|
||||
faults = derive_faults(cfg, snap)
|
||||
|
||||
position = snap.get("xy")
|
||||
if position is None and pos is not None:
|
||||
position = pos.get()
|
||||
|
||||
return {
|
||||
"sn": cfg.sn, "mac": mac,
|
||||
"battery": battery, "charging": charging, "status": status,
|
||||
"position": position, "faults": faults, "ts": int(time.time()),
|
||||
}
|
||||
|
||||
|
||||
def post_telemetry(cfg: Config, payload: Dict[str, Any], session: requests.Session) -> bool:
|
||||
try:
|
||||
r = session.post(cfg.telemetry_url(), json=payload, headers=cfg.auth_headers(),
|
||||
timeout=cfg.http_timeout, verify=cfg.verify_tls)
|
||||
except requests.RequestException as e:
|
||||
log.error("telemetry POST failed (transport): %s", e)
|
||||
return False
|
||||
if not r.ok:
|
||||
log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200])
|
||||
return False
|
||||
log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d -> HTTP %s",
|
||||
payload["battery"], payload["charging"], payload["status"],
|
||||
payload["position"], len(payload["faults"]), r.status_code)
|
||||
return True
|
||||
|
||||
|
||||
def _sim_state(i: int) -> Dict[str, Any]:
|
||||
charging = (i % 6) in (0, 1)
|
||||
battery = max(5, 90 - (i % 40))
|
||||
moving = (i % 3) == 2 and not charging
|
||||
return {"battery": battery, "charging": charging, "temp": 45 + (i % 10),
|
||||
"max_dq": 0.4 if moving else 0.0, "fsm": 200 if moving else 4,
|
||||
"position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}}
|
||||
|
||||
|
||||
def main(argv: Optional[List[str]] = None) -> int:
|
||||
ap = argparse.ArgumentParser(description="G1 fleet telemetry agent")
|
||||
ap.add_argument("--simulate", action="store_true")
|
||||
ap.add_argument("--once", action="store_true")
|
||||
ap.add_argument("--dry-run", action="store_true")
|
||||
ap.add_argument("--interval", type=float, default=None)
|
||||
ap.add_argument("-v", "--verbose", action="store_true")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
logging.basicConfig(level=logging.DEBUG if args.verbose else logging.INFO,
|
||||
format="%(asctime)s %(levelname)s %(name)s: %(message)s")
|
||||
_load_dotenv()
|
||||
cfg = Config.from_env()
|
||||
if args.interval is not None:
|
||||
cfg.poll_interval = args.interval
|
||||
|
||||
mac = read_mac(cfg.mac_interface)
|
||||
log.info("sanad_api_g1t telemetry — sn=%s mac=%s server=%s iface=%s domain=%d pos=%s%s",
|
||||
cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain,
|
||||
cfg.position_source, " [SIMULATE]" if args.simulate else "")
|
||||
|
||||
reader = None
|
||||
pos = None
|
||||
if not args.simulate:
|
||||
reader = DDSReader(cfg)
|
||||
if cfg.position_source == "rosbridge":
|
||||
pos = RosbridgePosition(cfg)
|
||||
time.sleep(1.0)
|
||||
|
||||
session = requests.Session()
|
||||
tick = 0
|
||||
|
||||
def one() -> None:
|
||||
nonlocal tick
|
||||
sim = _sim_state(tick) if args.simulate else None
|
||||
payload = build_telemetry(cfg, mac, reader, pos, sim=sim)
|
||||
if args.dry_run:
|
||||
log.info("[dry-run] %s", json.dumps(payload))
|
||||
else:
|
||||
post_telemetry(cfg, payload, session)
|
||||
tick += 1
|
||||
|
||||
if args.once:
|
||||
one(); return 0
|
||||
if args.dry_run:
|
||||
for _ in range(3):
|
||||
one(); time.sleep(min(cfg.poll_interval, 1.0))
|
||||
return 0
|
||||
|
||||
log.info("loop every %.1fs (Ctrl-C to stop)", cfg.poll_interval)
|
||||
while True:
|
||||
try:
|
||||
one()
|
||||
except Exception as e:
|
||||
log.exception("tick failed: %s", e)
|
||||
try:
|
||||
time.sleep(cfg.poll_interval)
|
||||
except KeyboardInterrupt:
|
||||
log.info("stopped"); return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
BIN
agents/g1t/vendor/crc_aarch64.so
vendored
Normal file
BIN
agents/g1t/vendor/crc_aarch64.so
vendored
Normal file
Binary file not shown.
BIN
agents/g1t/vendor/crc_amd64.so
vendored
Normal file
BIN
agents/g1t/vendor/crc_amd64.so
vendored
Normal file
Binary file not shown.
BIN
agents/g1t/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
BIN
agents/g1t/vendor/unitree_sdk2py-1.0.1-py3-none-any.whl
vendored
Normal file
Binary file not shown.
@ -25,10 +25,10 @@
|
||||
<g>
|
||||
<rect x="40" y="130" width="280" height="112" rx="10" fill="#ffffff" stroke="#cbd5e1"/>
|
||||
<rect x="40" y="130" width="6" height="112" rx="3" fill="#b45309"/>
|
||||
<text x="62" y="160" font-size="15" font-weight="700" fill="#0f172a">G1 — map uploader</text>
|
||||
<text x="62" y="184" font-size="11.5" fill="#475569">source: navigation map files</text>
|
||||
<text x="62" y="205" font-size="11" fill="#64748b" font-family="ui-monospace,Menlo,Consolas,monospace">web_nav3 maps/*.db + places</text>
|
||||
<text x="62" y="224" font-size="11" fill="#64748b" font-family="ui-monospace,Menlo,Consolas,monospace">maps_meta.json</text>
|
||||
<text x="62" y="160" font-size="15" font-weight="700" fill="#0f172a">G1 — map + telemetry</text>
|
||||
<text x="62" y="184" font-size="11.5" fill="#475569">map: web_nav3 .db + places (files)</text>
|
||||
<text x="62" y="205" font-size="11" fill="#64748b" font-family="ui-monospace,Menlo,Consolas,monospace">telem: rt/lf/bmsstate · rt/lowstate</text>
|
||||
<text x="62" y="224" font-size="11" fill="#64748b" font-family="ui-monospace,Menlo,Consolas,monospace">position: rt/lf/odommodestate</text>
|
||||
</g>
|
||||
<!-- R1 -->
|
||||
<g>
|
||||
|
||||
|
Before Width: | Height: | Size: 7.6 KiB After Width: | Height: | Size: 7.6 KiB |
@ -22,13 +22,15 @@ set -uo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
|
||||
AGENTS="$SCRIPT_DIR/agents"
|
||||
TYPES="g1 r1 go2"
|
||||
declare -A KIND=( [g1]="MAP uploader" [r1]="TELEMETRY (unitree_hg)" [go2]="TELEMETRY (unitree_go)" )
|
||||
TYPES="g1 g1t r1 go2"
|
||||
declare -A KIND=( [g1]="MAP uploader" [g1t]="TELEMETRY (G1, unitree_hg)" [r1]="TELEMETRY (unitree_hg)" [go2]="TELEMETRY (unitree_go)" )
|
||||
|
||||
die(){ echo "ERROR: $*" >&2; exit 1; }
|
||||
img_of(){ echo "sanad-api-$1"; } # image + container + unit share this base
|
||||
rdir_of(){ echo "sanad_api_$1"; }
|
||||
unit_of(){ echo "sanad-api-$1.service"; }
|
||||
# G1 map + G1 telemetry are the SAME physical robot → share one sn.
|
||||
sn_default(){ case "$1" in g1|g1t) echo "g1_${IP##*.}";; *) echo "${1}_${IP##*.}";; esac; }
|
||||
|
||||
SSH_OPTS=(-o BatchMode=yes -o ConnectTimeout=8 -o StrictHostKeyChecking=accept-new)
|
||||
rmt(){ ssh -n "${SSH_OPTS[@]}" "$USER_@$IP" "$@"; } # -n: never read our stdin
|
||||
@ -209,10 +211,12 @@ CMD="${POSA[0]:-}"; ROBOT="${POSA[1]:-}"; IP="${POSA[2]:-}"
|
||||
|
||||
# --------------------------- interactive --------------------------- #
|
||||
choose_type(){
|
||||
echo "Which robot?" >&2; local i=1
|
||||
for t in $TYPES; do echo " $i) $t — ${KIND[$t]}" >&2; i=$((i+1)); done
|
||||
read -rp "Choice [1-3]: " c
|
||||
case "$c" in 1) echo g1;; 2) echo r1;; 3) echo go2;; g1|r1|go2) echo "$c";; *) echo "";; esac
|
||||
echo "Which robot / agent?" >&2; local i=1; local -a arr=()
|
||||
for t in $TYPES; do echo " $i) $t — ${KIND[$t]}" >&2; arr[$i]="$t"; i=$((i+1)); done
|
||||
read -rp "Choice: " c
|
||||
if [[ "$c" =~ ^[0-9]+$ ]] && [ -n "${arr[$c]:-}" ]; then echo "${arr[$c]}"
|
||||
elif echo "$TYPES" | grep -qw "$c"; then echo "$c"
|
||||
else echo ""; fi
|
||||
}
|
||||
|
||||
interactive(){
|
||||
@ -239,7 +243,7 @@ interactive(){
|
||||
}
|
||||
|
||||
prompt_install(){
|
||||
local def="${ROBOT}_${IP##*.}"
|
||||
local def; def="$(sn_default "$ROBOT")"
|
||||
read -rp "Robot name / SN [$def]: " s; SN="${s:-$def}"
|
||||
local autos; autos="$(detect_server_ip)"
|
||||
read -rp "Fleet server IP [${autos:-required}]: " si; [ -n "$si" ] && SERVER_IP="$si" || SERVER_IP="${SERVER_IP:-$autos}"
|
||||
@ -252,7 +256,7 @@ prompt_install(){
|
||||
[ -n "$ROBOT" ] && [ -n "$IP" ] || die "usage: $0 <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##*.}"
|
||||
[ -z "$SN" ] && SN="$(sn_default "$ROBOT")"
|
||||
|
||||
case "$CMD" in
|
||||
install) do_install "$ROBOT";;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user