kassam/fleet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update 2026-07-16 12:02:30

Browse Source
This commit is contained in:
kassam 2026-07-16 12:02:32 +04:00
parent 098a87e00b
commit 90a3f07a87
10 changed files with 1176 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
PIPELINE.md
View File

@ -42,13 +42,13 @@ repo currently implements the **bold** ones; the rest are documented for later.
| endpoint | method | agent | status | | endpoint | method | agent | status |
|---|---|---|---| |---|---|---|---|
| **`/api/v1/fleet/ingest/telemetry`** | POST | G1, R1, Go2 | ✅ implemented (~2 s; includes `map` status) | | **`/api/v1/fleet/ingest/telemetry`** | POST | G1, R1, Go2 | ✅ ~2 s; 27 fields incl. software/firmware/control + all status mirrors |
| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1, R1, Go2 | ✅ implemented (once per content; rtabmap `.db` + slam_toolbox) | | **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1, R1, Go2 | ✅ once per content; rtabmap `.db` (≤~7 MB) + slam_toolbox `pgm/yaml`→PNG |
| **`/api/v1/fleet/ingest/{sn}/alert`** | POST | G1, R1, Go2 | ✅ implemented (each NEW fault, rising edge, string body) | | **`/api/v1/fleet/ingest/{sn}/alert`** | POST | G1, R1, Go2 | ✅ each NEW fault, rising edge, string body |
| **`/api/v1/fleet/ingest/{sn}/logs`** | POST | G1, R1, Go2 | ✅ implemented (agent log lines every `LOGS_INTERVAL`, default 60 s) | | **`/api/v1/fleet/ingest/{sn}/logs`** | POST | G1, R1, Go2 | ✅ agent + `[sanadr1-logs]` project lines every `LOGS_INTERVAL` (60 s) |
| `/api/v1/fleet/ingest/{sn}/commands` | GET | — | ⏳ spec'd, not built | | **`/api/v1/fleet/ingest/{sn}/remote`** | POST | G1, R1, Go2 | ✅ registers `web` (Sanad dashboard URL) + `ssh` (`ssh unitree@<ip>`) every 60 s |
| `/api/v1/fleet/ingest/{sn}/commands` | GET | — | ⏳ channel works; no command executor built (motion = deliberately out) |
| `/api/v1/fleet/ingest/commands/{id}/ack` | POST | — | ⏳ | | `/api/v1/fleet/ingest/commands/{id}/ack` | POST | — | ⏳ |
| `/api/v1/fleet/ingest/{sn}/remote` | POST | — | ⏳ (tunnel/SSH registration) |
Auth header (all): `Authorization: Bearer <device_token>`. Auth header (all): `Authorization: Bearer <device_token>`.
@ -92,24 +92,57 @@ POST /api/v1/fleet/ingest/telemetry
Authorization: Bearer <device_token> Authorization: Bearer <device_token>
Content-Type: application/json Content-Type: application/json
{ "sn": "E39N4000Q6D7E70F", // the robot's REAL Unitree serial { // ── identity ──
"sn": "E39N4000Q6D7E70F", // the robot's REAL Unitree serial (server key)
"name": "r1_82", // friendly display name
"mac": "4c:bb:47:51:25:9a", "mac": "4c:bb:47:51:25:9a",
"brand": "unitree", "brand": "unitree", "type": "humanoid", "model": "r1", // type: humanoid|dog
"type": "humanoid", // humanoid | dog
"model": "r1", // r1 | g1 | go2 // ── software / firmware ──
"battery": 62, "software": { "ros": "foxy", "os": "Ubuntu 20.04.5 LTS", "os_version": "20.04",
"charging": false, "kernel": "5.10.104-tegra", "arch": "aarch64",
"battery_detail": { "voltage_v": 34.5, "current_a": -3.08, "python": "3.10.20", "agent": "sanad_api_r1 2026.07.13" },
"temp_c": 42, "soh": 99, "cycles": 10 }, "firmware": { "board": "NVIDIA Orin NX Developer Kit", "l4t": "R35.3.1",
"kernel": "5.10.104-tegra", "robot": "0.0", "bms": "0.44" },
// ── power / health ──
"battery": 62, "charging": false,
"battery_detail": { "voltage_v": 34.5, "current_a": -3.08, "temp_c": 42,
"soh": 99, "cycles": 10 },
"motor_temp": { "max": 49.0, "avg": 37.8, "min": 32.0 }, // null = not receiving "motor_temp": { "max": 49.0, "avg": 37.8, "min": 32.0 }, // null = not receiving
"storage": { "total_gb": 98.2, "free_gb": 58.5, "used_percent": 36.2, "storage": { "total_gb": 98.2, "free_gb": 58.5, "used_percent": 36.2,
"data_kb": 770.0 }, // data_kb only when STORAGE_DATA_PATH set "data_kb": 770.0 }, // data_kb only when STORAGE_DATA_PATH set
// ── state ──
"status": "idle", "status": "idle",
"position": { "x": 12.4, "y": 3.1 }, // or null when no localization source "position": { "x": 12.4, "y": 3.1 }, // or null when no localization source
"faults": [], "control": { "fsm_id": 0, "mode": "zero_torque", // zero_torque|damp|lock|running
"armed": false, "walk_ready": false, "teleop_active": false,
"switchable_modes": ["zero_torque","damp","lock","running"],
"remote_switch_enabled": false }, // read-only; no remote motion
"faults": [], // strings, e.g. "LOW_BATTERY: battery 12% (warning)"
// ── sub-system status (each stream's health, mirrored here) ──
"map": { "uploaded": true, "state": "uploaded", "maps_found": 2,
"last_map": "rtabmap", "error": null },
"logs": { "last_sent": "…", "lines_sent": 105, "ok": true },
"project_logs": "sanadr1-logs", // the Sanad app whose logs are tailed (null=none)
"remote": { "url": "http://10.255.254.82:8001", "port": 8001, "kind": "web",
"ok": true, "ssh": "ssh unitree@10.255.254.82", "ssh_ok": true },
"alerts": { "sent": 0, "last": null, "last_time": null, "ok": null },
// ── time ──
"time": "2026-07-13 17:00:22+04:00", // full local datetime (TZ_OFFSET_HOURS, default +4)
"started_at": "2026-07-13 17:00:21+04:00",
"last_start": "2026-07-13 16:57:43+04:00", // previous agent start (persisted)
"uptime_s": 625,
"ts": 1731000000 } "ts": 1731000000 }
``` ```
Sub-objects `map` / `logs` / `alerts` / `remote` / `control` are **status mirrors** —
the real work happens on their own endpoints (below); telemetry just always shows
whether each is healthy so the fleet UI never has to poll them separately.
### 3.3 Status derivation ### 3.3 Status derivation
``` ```

59
README.md
View File

@ -80,36 +80,45 @@ agent to the robot; you never edit files on the robot.
## 3. The three agents ## 3. The three agents
| agent (type) | robot | endpoint | what it sends | cadence | | agent (type) | robot | DDS family | notes |
|---|---|---|---|---| |---|---|---|---|
| `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_g1` (`g1`) | Unitree G1 | `unitree_hg` | canonical source (see below) |
| `sanad_api_r1` (`r1`) | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | | `sanad_api_r1` (`r1`) | Unitree R1 EDU | `unitree_hg` | generated from g1; R1 FSM ids, `eth10` |
| `sanad_api_go2` (`go2`) | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | | `sanad_api_go2` (`go2`) | Unitree Go2 | `unitree_go` | generated from g1; battery nested in `LowState.bms_state` — ⚠ unverified on hardware |
The `g1` agent is ONE service doing BOTH jobs: telemetry every ~2 s **and** a **One agent = one full-feature service.** Each robot runs a single container that
30 s map loop that checks the Sanad dashboard's saved maps and uploads each one does everything, across five endpoints:
ONE time (re-upload only on content change). The map result is embedded in every
telemetry post as the `map` field (`uploaded/no_map/failed` + reason), so a
missing map is always visible on the server. Supported map formats:
**RTAB-Map `.db`** and **slam_toolbox sets** (`<name>.yaml` + `.pgm`
[+ `.posegraph`/`.data`]) — slam maps are converted to the spec's image JSON
(PNG + resolution + origin + width/height). Faults are sent as **strings**
(the ingest 500s on fault objects).
All requests carry `Authorization: Bearer <device_token>`. Full payload schemas - **telemetry** (`…/telemetry`, ~2 s) — 27 fields: identity, `software`, `firmware`,
and the data pipeline are in [PIPELINE.md](PIPELINE.md). battery(+detail), motor temps, storage, status, position, `control` (loco mode),
faults, and status mirrors for map/logs/alerts/remote + full timing.
- **map** (`…/{sn}/map`, on change) — the Sanad/SLAM saved map, uploaded ONCE per
content. Formats: **RTAB-Map `.db`** (skipped over the ~8 MB server cap) and
**slam_toolbox** `pgm/yaml` → image JSON (PNG + resolution + origin). Status shown
in telemetry `map`.
- **alerts** (`…/{sn}/alert`) — each NEW fault, immediately (strings — the ingest
500s on fault objects).
- **logs** (`…/{sn}/logs`, 60 s) — the agent's own lines **+ the robot's Sanad app
logs** (`[sanadr1-logs]`, auto-discovered, backfilled, noise-filtered).
- **remote** (`…/{sn}/remote`, 60 s) — registers the Sanad **dashboard URL** (`web`)
and **`ssh unitree@<ip>`** (`ssh`) — no changes to the Sanad app.
**All three agents are generated from `g1`** by `tools/gen_agents.py` — run it after
any change to `agents/g1/` to keep r1/go2 in lockstep (no manual editing of r1/go2).
All requests carry `Authorization: Bearer <device_token>`. Full payload schema and
pipeline in [PIPELINE.md](PIPELINE.md).
**Design principles** **Design principles**
- **No ROS in the agents.** G1 reads map files directly; R1/Go2 read DDS via - **No ROS.** Maps read from files; state read from DDS via `unitree_sdk2py`.
`unitree_sdk2py`. This keeps images small and portable. - **Read-only toward the robot.** Never commands motion — the control panel is
- **Read-only.** Telemetry agents never command motion (only passive DDS reads +, status-only; mode switching is deliberately not built (it can drop the robot).
optionally, the read-only `GET_FSM_ID` RPC). - **Never crash the loop.** Every tick is wrapped; a heartbeat keeps the robot
- **Never crash the loop.** Every tick is wrapped; transient errors are logged and "online" when state is unreadable.
retried. Telemetry sends a heartbeat when it can't read state so the robot stays - **Change-detected + one-time uploads.** Maps re-send only on content change.
"online". - **Resilient shipping.** Failed log ships are requeued (last ~400 lines) until the
- **Change-detected uploads.** The map is only re-sent when its content hash server accepts them.
changes.
--- ---

17
agents/g1/.env.example
View File

@ -63,3 +63,20 @@ REMOTE_KIND=web
# REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable) # REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable)
# REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery # REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery
REMOTE_INTERVAL=60 REMOTE_INTERVAL=60
# ── control panel (READ-ONLY loco mode; remote SWITCH is off by design) ───────
# The agent reads the Sanad dashboard's /api/controller/status (no DDS, no
# motion) and reports the current mode (zero_torque/damp/lock/running) + the
# switchable set in telemetry.control. CONTROL_ENABLE stays 0: remote mode-
# SWITCHING is deliberately NOT built (it commands motion and can drop the robot;
# switching is done in person on the Sanad dashboard). Leave URL blank to use the
# auto-discovered dashboard port.
CONTROL_STATUS_URL=
CONTROL_ENABLE=0
# ── log-driven alerts (Gemini billing + any robot ERROR) ─────────────────────
# The agent scans the robot's project (sanadr1) logs; each NEW matching signature
# fires an alert (deduped per ALERT_LOG_COOLDOWN). "CODE=regex" entries split by ;;
# ALERT_LOG_PATTERNS=GEMINI_BILLING=prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini;;ROBOT_ERROR=\bERROR\b|Traceback|Exception|CRITICAL
ALERT_LOG_COOLDOWN=300
ALERT_SCAN_INTERVAL=10

223
agents/g1/sanad_api_g1.py
View File

@ -122,6 +122,10 @@ class Config:
rosbridge_url: str rosbridge_url: str
low_soc: int low_soc: int
motor_temp_max: float motor_temp_max: float
alert_log_patterns: str
alert_log_cooldown: float
alert_scan_interval: float
alert_backfill_bytes: int
poll_interval: float poll_interval: float
telemetry_endpoint: str telemetry_endpoint: str
# map sync # map sync
@ -151,6 +155,8 @@ class Config:
ssh_enable: bool ssh_enable: bool
ssh_user: str ssh_user: str
ssh_port: int ssh_port: int
control_url: str
control_enable: bool
# project logs (e.g. the robot's Sanad app) shipped alongside agent logs # project logs (e.g. the robot's Sanad app) shipped alongside agent logs
project_log_container: str project_log_container: str
project_log_path: str project_log_path: str
@ -188,8 +194,18 @@ class Config:
read_fsm=_env_bool("G1_READ_FSM", False), read_fsm=_env_bool("G1_READ_FSM", False),
position_source=_env("G1_POSITION_SOURCE", "odom").lower(), position_source=_env("G1_POSITION_SOURCE", "odom").lower(),
rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"), rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"),
low_soc=int(_env("LOW_SOC", "15")), low_soc=int(_env("LOW_SOC", "50")),
motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")),
# log-driven alerts: "CODE=regex" entries separated by ";;" (regex may
# contain '|'). Scanned against the robot's project logs (sanadr1).
# NOTE: matching is CASE-SENSITIVE (log levels are uppercase); use an
# inline (?i) prefix for case-insensitive text (Gemini messages).
alert_log_patterns=_env("ALERT_LOG_PATTERNS",
"GEMINI_BILLING=(?i)prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini"
";;ROBOT_ERROR=\\bERROR\\b|\\bCRITICAL\\b|^Traceback"),
alert_log_cooldown=float(_env("ALERT_LOG_COOLDOWN", "300")), # per-signature re-alert gap
alert_scan_interval=float(_env("ALERT_SCAN_INTERVAL", "10")),
alert_backfill_bytes=int(_env("ALERT_BACKFILL_BYTES", str(8 * 1024 * 1024))),
poll_interval=float(_env("POLL_INTERVAL", "2")), poll_interval=float(_env("POLL_INTERVAL", "2")),
telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"),
robot=_env("ROBOT", "sanad"), robot=_env("ROBOT", "sanad"),
@ -216,6 +232,8 @@ class Config:
ssh_enable=_env_bool("SSH_REGISTER", True), ssh_enable=_env_bool("SSH_REGISTER", True),
ssh_user=_env("SSH_USER", "unitree"), ssh_user=_env("SSH_USER", "unitree"),
ssh_port=int(_env("SSH_PORT", "22")), ssh_port=int(_env("SSH_PORT", "22")),
control_url=_env("CONTROL_STATUS_URL", ""), # Sanad /api/controller/status
control_enable=_env_bool("CONTROL_ENABLE", False), # remote mode-SWITCH (motion!) — off
project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"),
project_log_path=_env("PROJECT_LOG_PATH", ""), project_log_path=_env("PROJECT_LOG_PATH", ""),
project_log_label=_env("PROJECT_LOG_LABEL", ""), project_log_label=_env("PROJECT_LOG_LABEL", ""),
@ -580,6 +598,49 @@ class DDSReader:
# G1 FSM ids (differ from the R1's): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand. # 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"} _FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"}
# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).
_CONTROL_MODES = {200: "running", 4: "lock", 2: "squat", 702: "lie2stand", 0: "zero_torque", 1: "damp"}
_CONTROL_SWITCHABLE = ["zero_torque", "damp", "lock", "running"]
_control_cache: Dict[str, Any] = {"ts": 0.0, "data": None}
def read_control(cfg: Config) -> Optional[Dict[str, Any]]:
"""READ-ONLY control status from the Sanad dashboard's /api/controller/status
(no DDS, no motion). Reports the current loco mode + the switchable set.
Remote SWITCHING is a separate, motion-capable path gated by CONTROL_ENABLE."""
now = time.monotonic()
if _control_cache["data"] is not None and now - _control_cache["ts"] < 1.5:
return _control_cache["data"]
url = cfg.control_url
if not url:
port = _REMOTE_STAT.get("port")
if not port:
return None
url = f"http://127.0.0.1:{port}/api/controller/status"
try:
r = requests.get(url, timeout=2)
d = r.json() if r.ok else None
except Exception:
d = None
if not isinstance(d, dict):
_control_cache.update(ts=now, data=None)
return None
fid = d.get("fsm_id")
out = {
"fsm_id": fid,
"mode": _CONTROL_MODES.get(fid, "unknown") if fid is not None else None,
"armed": d.get("armed"),
"walk_ready": d.get("walk_ready"),
"teleop_active": d.get("teleop_active"),
"last_velocity": d.get("last_velocity"),
"sdk_available": d.get("sdk_available"),
"switchable_modes": _CONTROL_SWITCHABLE,
"remote_switch_enabled": bool(cfg.control_enable), # remote mode-switch armed?
}
_control_cache.update(ts=now, data=out)
return out
class RosbridgePosition: class RosbridgePosition:
@ -1417,26 +1478,155 @@ def remote_loop(cfg: Config, session: requests.Session) -> None:
_ALERT_SEEN: set = set() _ALERT_SEEN: set = set()
def _post_alert(cfg: Config, session: requests.Session, text: str) -> bool:
"""POST a single alert string to /{sn}/alert; updates the alert status."""
try:
r = session.post(cfg.alert_url(),
json={"sn": cfg.sn, "name": cfg.name,
"alert": text, "message": text, "ts": int(time.time())},
headers=cfg.auth_headers(),
timeout=cfg.http_timeout, verify=cfg.verify_tls)
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=bool(r.ok))
if r.ok:
_ALERTS_STAT["sent"] += 1
log.info("alert sent: %s -> HTTP %s", text[:120], r.status_code)
return bool(r.ok)
except requests.RequestException as e:
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=False)
log.debug("alert send failed (%s): %s", text[:60], e)
return False
def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None:
"""POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.
Includes LOW_BATTERY (<= LOW_SOC, default 50%), MOTOR_OVERTEMP, COMMS_STALE."""
global _ALERT_SEEN global _ALERT_SEEN
current = set(faults) current = set(faults)
new = current - _ALERT_SEEN new = current - _ALERT_SEEN
_ALERT_SEEN = current _ALERT_SEEN = current
for f in sorted(new): for f in sorted(new):
_post_alert(cfg, session, f)
class LogAlertScanner:
"""Scans the robot's project logs (sanadr1) for error/billing patterns and
fires an alert on each NEW signature (deduped with a cooldown). Catches the
Gemini 'credits depleted' billing error and any ERROR/Traceback the app logs."""
def __init__(self, cfg: Config):
import re
self._path: Optional[Path] = None
self._pos = 0
self._cooldown = cfg.alert_log_cooldown
self._seen: Dict[str, float] = {}
self._patterns: List[Any] = []
self._pending: List[Any] = []
for entry in cfg.alert_log_patterns.split(";;"):
if "=" in entry:
code, rx = entry.split("=", 1)
try: try:
r = session.post(cfg.alert_url(), # case-SENSITIVE (uppercase log levels); use (?i) inline for text
json={"sn": cfg.sn, "name": cfg.name, self._patterns.append((code.strip(), re.compile(rx)))
"alert": f, "message": f, "ts": int(time.time())}, except re.error as e:
headers=cfg.auth_headers(), log.warning("bad alert pattern %s: %s", code, e)
timeout=cfg.http_timeout, verify=cfg.verify_tls) if _PROJECT_TAIL is not None and _PROJECT_TAIL.active and _PROJECT_TAIL._cur:
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) self._path = _PROJECT_TAIL._cur
if r.ok: try:
_ALERTS_STAT["sent"] += 1 size = self._path.stat().st_size
log.info("alert sent: %s -> HTTP %s", f, r.status_code) self._pos = size # ongoing reads start at EOF
except requests.RequestException as e: # STARTUP BACKFILL: scan a large tail once (access-logs bury real
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) # errors) so a currently-active error (e.g. Gemini billing) alerts
log.debug("alert send failed (%s): %s", f, e) # right away. Only the matching lines are kept — cheap.
back = min(size, cfg.alert_backfill_bytes)
with self._path.open("rb") as f:
f.seek(size - back)
raw = f.read(back).decode("utf-8", "replace").splitlines()
if back < size and raw:
raw = raw[1:] # first line is partial
matches = []
for ln in raw:
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
matches.append((code, ln))
break
# keep the most-recent unique signature per (code+line), cap 20
seen = set()
uniq = []
for code, ln in reversed(matches):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
if sig in seen:
continue
seen.add(sig)
uniq.append((code, ln))
self._pending = list(reversed(uniq))[-20:]
except Exception:
self._pos = 0
if self._path and self._patterns:
log.info("log-alert scan on %s (%d patterns, %d backfilled)",
self._path.name, len(self._patterns), len(self._pending))
def scan(self, cfg: Config, session: requests.Session) -> None:
if not self._path or not self._patterns:
return
import re
now0 = time.monotonic()
if self._pending: # flush startup backfill first
pend, self._pending = self._pending, []
for code, ln in pend:
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
self._seen[sig] = now0
_post_alert(cfg, session, f"{code}: {ln[:220]}")
try:
st = self._path.stat()
if st.st_size < self._pos: # rotated
self._pos = 0
if st.st_size <= self._pos:
return
with self._path.open("rb") as f:
f.seek(self._pos)
chunk = f.read(min(st.st_size - self._pos, 512 * 1024))
self._pos = f.tell()
except Exception as e:
log.debug("alert scan read failed: %s", e)
return
now = time.monotonic()
for ln in chunk.decode("utf-8", "replace").splitlines():
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120] # dedup key
if now - self._seen.get(sig, -1e9) > self._cooldown:
self._seen[sig] = now
_post_alert(cfg, session, f"{code}: {ln[:220]}")
break
_LOG_ALERTS: Optional[LogAlertScanner] = None
def alert_scan_loop(cfg: Config, session: requests.Session) -> None:
while True:
time.sleep(cfg.alert_scan_interval)
try:
if _LOG_ALERTS is not None:
_LOG_ALERTS.scan(cfg, session)
except Exception as e:
log.debug("alert scan loop: %s", e)
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
@ -1520,6 +1710,7 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
"storage": read_storage(cfg), "storage": read_storage(cfg),
"status": status, "status": status,
"position": position, # null when no odom/localization source "position": position, # null when no odom/localization source
"control": read_control(cfg), # loco mode (zero_torque/damp/lock/running) + switchable set
"faults": faults, "faults": faults,
"map": get_map_status(), # SHOWS whether the saved map made it to the server "map": get_map_status(), # SHOWS whether the saved map made it to the server
"logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok)
@ -1603,8 +1794,9 @@ def main(argv: Optional[List[str]] = None) -> int:
return 0 return 0
_init_start_times(cfg) _init_start_times(cfg)
global _PROJECT_TAIL global _PROJECT_TAIL, _LOG_ALERTS
_PROJECT_TAIL = ProjectLogTail(cfg) _PROJECT_TAIL = ProjectLogTail(cfg)
_LOG_ALERTS = LogAlertScanner(cfg) # error/billing alerts from the project logs
mac = read_mac(cfg.mac_interface) mac = read_mac(cfg.mac_interface)
log.info("sanad_api_g1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s " log.info("sanad_api_g1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s "
"map_dir=%s map_every=%.0fs%s", "map_dir=%s map_every=%.0fs%s",
@ -1655,6 +1847,7 @@ def main(argv: Optional[List[str]] = None) -> int:
# loop mode: map sync + log shipping + remote registration in bg threads # loop mode: map sync + log shipping + remote registration in bg threads
threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=alert_scan_loop, args=(cfg, session), daemon=True).start()
if cfg.remote_enable: if cfg.remote_enable:
threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start()
log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)", log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)",

19
agents/go2/.env.example
View File

@ -20,7 +20,7 @@ GO2_POSITION_SOURCE=none
ROSBRIDGE_URL=ws://127.0.0.1:9090 ROSBRIDGE_URL=ws://127.0.0.1:9090
# ── fault thresholds ───────────────────────────────────────────────────────── # ── fault thresholds ─────────────────────────────────────────────────────────
LOW_SOC=15 LOW_SOC=50
MOTOR_TEMP_MAX=85 MOTOR_TEMP_MAX=85
# ── cadence / transport ────────────────────────────────────────────────────── # ── cadence / transport ──────────────────────────────────────────────────────
@ -69,3 +69,20 @@ REMOTE_KIND=web
# REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable) # REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable)
# REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery # REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery
REMOTE_INTERVAL=60 REMOTE_INTERVAL=60
# ── control panel (READ-ONLY loco mode; remote SWITCH is off by design) ───────
# The agent reads the Sanad dashboard's /api/controller/status (no DDS, no
# motion) and reports the current mode (zero_torque/damp/lock/running) + the
# switchable set in telemetry.control. CONTROL_ENABLE stays 0: remote mode-
# SWITCHING is deliberately NOT built (it commands motion and can drop the robot;
# switching is done in person on the Sanad dashboard). Leave URL blank to use the
# auto-discovered dashboard port.
CONTROL_STATUS_URL=
CONTROL_ENABLE=0
# ── log-driven alerts (Gemini billing + any robot ERROR) ─────────────────────
# The agent scans the robot's project (sanadr1) logs; each NEW matching signature
# fires an alert (deduped per ALERT_LOG_COOLDOWN). "CODE=regex" entries split by ;;
# ALERT_LOG_PATTERNS=GEMINI_BILLING=prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini;;ROBOT_ERROR=\bERROR\b|Traceback|Exception|CRITICAL
ALERT_LOG_COOLDOWN=300
ALERT_SCAN_INTERVAL=10

506
agents/go2/sanad_api_go2.py
View File

@ -1,38 +1,48 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
"""sanad_api_go2 — Go2 fleet TELEMETRY agent. """sanad_api_go2 — Go2 fleet agent: TELEMETRY + MAP sync in ONE service.
Pushes the Unitree **Go2**'s live status to the YS Lootah fleet server: The single Go2 agent (one container, one systemd service) that:
POST {SERVER_URL}/api/v1/fleet/ingest/telemetry (Bearer device token) 1. TELEMETRY — every ~2 s POSTs the robot's live status:
body: { "sn", "mac", "battery", "charging", "status", "position":{x,y}, "faults":[] } POST {SERVER_URL}/api/v1/fleet/ingest/telemetry
{ sn, name, mac, brand, type, model, battery, charging, battery_detail,
motor_temp, storage, status, position, faults, map, ts }
Same shape/cadence as the R1 agent. The DIFFERENCE is the DDS family: 2. MAP — a background loop (every MAP_POLL_INTERVAL, default 30 s) checks the
Sanad dashboard's saved maps (web_nav3: RTAB-Map .db + places) and uploads
each map ONE time (re-upload only if its content changes):
POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map (multipart: db + meta)
* Go2 uses **unitree_go** (not unitree_hg). The map result is SHOWN inside every telemetry post as the "map" field:
* Go2 battery lives INSIDE LowState_ as a nested ``bms_state`` (soc/current) — "map": { "uploaded": true|false,
there is NO separate rt/lf/bmsstate topic like the G1/R1. So this agent reads "state": "uploaded" | "no_map" | "failed" | "pending",
battery straight from rt/lowstate.bms_state. "maps_found": N, "last_map": "floor-1"|null,
* position (optional) from SportModeState (rt/lf/sportmodestate) or rosbridge /odom. "error": "no saved map found …"|null, "checked_ts": … }
so the server always sees whether the map made it — and why not.
⚠️ UNVERIFIED ON HARDWARE: written from the unitree_go SDK layout but not yet run DATA SOURCES (Unitree Go2, unitree_go DDS (⚠ UNVERIFIED on hardware))
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 battery / charging : rt/lf/bmsstate (BmsState_) soc, current, voltage, temp, soh, cycles
tests the upload path without a robot. SAFETY: never commands motion (read-only). faults / motor temp: rt/lowstate (LowState_) per-motor temps + staleness
position {x,y} : rt/lf/odommodestate (SportModeState_) firmware odom over DDS
status : derived (charging/moving/idle/offline); optional FSM GET RPC
(G1 ids: 200 walk-ready / 4 stand / 2 squat / 702 lie2stand)
storage : host disk via the read-only /:/host mount (+ Sanad data dir size)
maps : MAPS_DIR/<robot>/*.db (+ maps_meta.json), places under
DATA_DIR/<robot>/places/<map>.json — the web_nav3 stores
CONFIG — environment (see .env.example) Read-only toward the robot: never commands motion. Degrades to heartbeats if
--------------------------------------- unitree_sdk2py is unavailable; --simulate fakes only the DDS side (the map scan
SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required) stays real).
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 CONFIG — environment (see .env.example). Key vars:
SERVER_URL, DEVICE_TOKEN, SN (required at install), ROBOT_NAME,
DDS_INTERFACE (eth0), POLL_INTERVAL (2), TELEMETRY_ENDPOINT,
ROBOT (web_nav3 robot name, default sanad), MAPS_DIR, DATA_DIR, STATE_DIR,
MAP_SELECT (all|active|newest), MAP_UPLOAD_MODE (multipart|base64json),
MAP_ENDPOINT, MAP_POLL_INTERVAL (30), VERIFY_TLS, HTTP_TIMEOUT
CLI: --simulate | --once | --dry-run | --force (map re-upload) | --list (maps) | -v
""" """
from __future__ import annotations from __future__ import annotations
@ -58,6 +68,9 @@ import requests
log = logging.getLogger("sanad_api_go2") log = logging.getLogger("sanad_api_go2")
# --------------------------------------------------------------------------- #
# env helpers
# --------------------------------------------------------------------------- #
def _load_dotenv(path: str = ".env") -> None: def _load_dotenv(path: str = ".env") -> None:
p = Path(path) p = Path(path)
if not p.exists(): if not p.exists():
@ -86,7 +99,9 @@ def _now_str() -> str:
return _dt.datetime.now(tz).isoformat(sep=" ", timespec="seconds") return _dt.datetime.now(tz).isoformat(sep=" ", timespec="seconds")
# --------------------------------------------------------------------------- #
# config
# --------------------------------------------------------------------------- #
@dataclass @dataclass
class Config: class Config:
server_url: str server_url: str
@ -98,15 +113,21 @@ class Config:
model: str model: str
storage_path: str storage_path: str
data_path: str data_path: str
# telemetry / DDS
dds_interface: str dds_interface: str
dds_domain: int dds_domain: int
mac_interface: str mac_interface: str
read_fsm: bool
position_source: str position_source: str
rosbridge_url: str rosbridge_url: str
low_soc: int low_soc: int
motor_temp_max: float motor_temp_max: float
alert_log_patterns: str
alert_log_cooldown: float
alert_scan_interval: float
alert_backfill_bytes: int
poll_interval: float poll_interval: float
endpoint: str telemetry_endpoint: str
# map sync # map sync
robot: str robot: str
maps_dir: Path maps_dir: Path
@ -123,6 +144,7 @@ class Config:
alert_endpoint: str alert_endpoint: str
logs_endpoint: str logs_endpoint: str
logs_interval: float logs_interval: float
# remote dashboard (register the Sanad UI URL for the fleet to embed)
remote_enable: bool remote_enable: bool
remote_endpoint: str remote_endpoint: str
remote_kind: str remote_kind: str
@ -133,6 +155,8 @@ class Config:
ssh_enable: bool ssh_enable: bool
ssh_user: str ssh_user: str
ssh_port: int ssh_port: int
control_url: str
control_enable: bool
# project logs (e.g. the robot's Sanad app) shipped alongside agent logs # project logs (e.g. the robot's Sanad app) shipped alongside agent logs
project_log_container: str project_log_container: str
project_log_path: str project_log_path: str
@ -140,6 +164,7 @@ class Config:
project_log_backfill: int project_log_backfill: int
project_log_exclude: str project_log_exclude: str
ros_distro: str ros_distro: str
# transport
verify_tls: bool verify_tls: bool
http_timeout: float http_timeout: float
@ -154,7 +179,8 @@ class Config:
data_dir = _env("DATA_DIR") data_dir = _env("DATA_DIR")
legacy = _env("LEGACY_PLACES") legacy = _env("LEGACY_PLACES")
return cls( return cls(
server_url=server, device_token=token, server_url=server,
device_token=token,
sn=_env("SN", "go2_0000"), sn=_env("SN", "go2_0000"),
name=_env("ROBOT_NAME", "") or _env("SN", "go2_0000"), name=_env("ROBOT_NAME", "") or _env("SN", "go2_0000"),
brand=_env("ROBOT_BRAND", "unitree"), brand=_env("ROBOT_BRAND", "unitree"),
@ -162,14 +188,26 @@ class Config:
model=_env("ROBOT_MODEL", "go2"), model=_env("ROBOT_MODEL", "go2"),
storage_path=_env("STORAGE_PATH", ""), storage_path=_env("STORAGE_PATH", ""),
data_path=_env("STORAGE_DATA_PATH", ""), data_path=_env("STORAGE_DATA_PATH", ""),
dds_interface=iface, dds_domain=int(_env("DDS_DOMAIN", "0")), dds_interface=iface,
dds_domain=int(_env("DDS_DOMAIN", "0")),
mac_interface=_env("MAC_INTERFACE", iface), mac_interface=_env("MAC_INTERFACE", iface),
read_fsm=_env_bool("GO2_READ_FSM", False),
position_source=_env("GO2_POSITION_SOURCE", "none").lower(), position_source=_env("GO2_POSITION_SOURCE", "none").lower(),
rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"), rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"),
low_soc=int(_env("LOW_SOC", "15")), low_soc=int(_env("LOW_SOC", "50")),
motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")),
# log-driven alerts: "CODE=regex" entries separated by ";;" (regex may
# contain '|'). Scanned against the robot's project logs (sanadr1).
# NOTE: matching is CASE-SENSITIVE (log levels are uppercase); use an
# inline (?i) prefix for case-insensitive text (Gemini messages).
alert_log_patterns=_env("ALERT_LOG_PATTERNS",
"GEMINI_BILLING=(?i)prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini"
";;ROBOT_ERROR=\\bERROR\\b|\\bCRITICAL\\b|^Traceback"),
alert_log_cooldown=float(_env("ALERT_LOG_COOLDOWN", "300")), # per-signature re-alert gap
alert_scan_interval=float(_env("ALERT_SCAN_INTERVAL", "10")),
alert_backfill_bytes=int(_env("ALERT_BACKFILL_BYTES", str(8 * 1024 * 1024))),
poll_interval=float(_env("POLL_INTERVAL", "2")), poll_interval=float(_env("POLL_INTERVAL", "2")),
endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"),
robot=_env("ROBOT", "sanad"), robot=_env("ROBOT", "sanad"),
maps_dir=Path(_env("MAPS_DIR", "/data/maps")), maps_dir=Path(_env("MAPS_DIR", "/data/maps")),
web_data_dir=Path(data_dir) if data_dir else None, web_data_dir=Path(data_dir) if data_dir else None,
@ -189,24 +227,28 @@ class Config:
remote_kind=_env("REMOTE_KIND", "web"), remote_kind=_env("REMOTE_KIND", "web"),
remote_host=_env("REMOTE_HOST", ""), remote_host=_env("REMOTE_HOST", ""),
remote_ports=_env("REMOTE_PORTS", "8001,8014,8011,8012,8013,8000,8080"), remote_ports=_env("REMOTE_PORTS", "8001,8014,8011,8012,8013,8000,8080"),
remote_url=_env("REMOTE_URL", ""), remote_url=_env("REMOTE_URL", ""), # explicit URL (e.g. a public tunnel) wins
remote_interval=float(_env("REMOTE_INTERVAL", "60")), remote_interval=float(_env("REMOTE_INTERVAL", "60")),
ssh_enable=_env_bool("SSH_REGISTER", True), ssh_enable=_env_bool("SSH_REGISTER", True),
ssh_user=_env("SSH_USER", "unitree"), ssh_user=_env("SSH_USER", "unitree"),
ssh_port=int(_env("SSH_PORT", "22")), ssh_port=int(_env("SSH_PORT", "22")),
control_url=_env("CONTROL_STATUS_URL", ""), # Sanad /api/controller/status
control_enable=_env_bool("CONTROL_ENABLE", False), # remote mode-SWITCH (motion!) — off
project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"),
project_log_path=_env("PROJECT_LOG_PATH", ""), project_log_path=_env("PROJECT_LOG_PATH", ""),
project_log_label=_env("PROJECT_LOG_LABEL", ""), project_log_label=_env("PROJECT_LOG_LABEL", ""),
project_log_backfill=int(_env("PROJECT_LOG_BACKFILL", "100")), project_log_backfill=int(_env("PROJECT_LOG_BACKFILL", "100")),
# drop uvicorn/access-log noise so shipped lines match the project's
# own LIVE LOGS panel exactly (app-logger lines + tracebacks only)
project_log_exclude=_env("PROJECT_LOG_EXCLUDE", project_log_exclude=_env("PROJECT_LOG_EXCLUDE",
r"^(INFO|WARNING|ERROR|DEBUG|CRITICAL):\s"), r"^(INFO|WARNING|ERROR|DEBUG|CRITICAL):\s"),
ros_distro=_env("SOFTWARE_ROS", ""), ros_distro=_env("SOFTWARE_ROS", ""),
verify_tls=_env_bool("VERIFY_TLS", True), verify_tls=_env_bool("VERIFY_TLS", True),
http_timeout=float(_env("HTTP_TIMEOUT", "10")), http_timeout=float(_env("HTTP_TIMEOUT", "30")),
) )
def telemetry_url(self) -> str: def telemetry_url(self) -> str:
return self.server_url + self.endpoint return self.server_url + self.telemetry_endpoint
def map_url(self) -> str: def map_url(self) -> str:
return self.server_url + self.map_endpoint_tmpl.format(sn=self.sn) return self.server_url + self.map_endpoint_tmpl.format(sn=self.sn)
@ -224,6 +266,9 @@ class Config:
return {"Authorization": f"Bearer {self.device_token}"} return {"Authorization": f"Bearer {self.device_token}"}
# --------------------------------------------------------------------------- #
# mac + storage
# --------------------------------------------------------------------------- #
def read_mac(interface: str) -> str: def read_mac(interface: str) -> str:
p = Path(f"/sys/class/net/{interface}/address") p = Path(f"/sys/class/net/{interface}/address")
try: try:
@ -360,20 +405,26 @@ def read_storage(cfg: Config) -> Optional[Dict[str, Any]]:
return out return out
# --------------------------------------------------------------------------- #
# DDS reader (telemetry side — degrades to heartbeats if SDK absent)
# --------------------------------------------------------------------------- #
class DDSReader: class DDSReader:
"""Subscribes rt/lowstate (unitree_go LowState_) and (optionally) sportmodestate. """Subscribes rt/lowstate (unitree_go LowState_); battery is NESTED in
Battery comes from the nested LowState_.bms_state. Passive reads only.""" LowState_.bms_state (Go2 has no separate rt/lf/bmsstate). Optional position
from rt/lf/sportmodestate. Passive reads only; the only RPC ever issued is
GET_FSM_ID (Go2 has no loco FSM RPC, so it stays off)."""
def __init__(self, cfg: Config): def __init__(self, cfg: Config):
self.cfg = cfg self.cfg = cfg
self._lock = threading.Lock() self._lock = threading.Lock()
self._bms: Optional[Dict[str, Any]] = None self._bms: Optional[Dict[str, Any]] = None
self._bms_ts = 0.0
self._low_ts = 0.0 self._low_ts = 0.0
self._sport_ts = 0.0
self._temps: List[float] = [] self._temps: List[float] = []
self._max_dq = 0.0 self._max_dq = 0.0
self._xy: Optional[Dict[str, float]] = None self._xy: Optional[Dict[str, float]] = None
self._fw: Dict[str, Any] = {} # live fw versions (robot ctrl + bms) self._fw: Dict[str, Any] = {}
self._loco = None
self.ok = False self.ok = False
self._start() self._start()
@ -383,7 +434,7 @@ class DDSReader:
ChannelFactoryInitialize, ChannelSubscriber) ChannelFactoryInitialize, ChannelSubscriber)
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
SportModeState_ = None SportModeState_ = None
if self.cfg.position_source == "sportmode": if self.cfg.position_source in ("sportmode", "odom"):
try: try:
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
except Exception: except Exception:
@ -397,34 +448,24 @@ class DDSReader:
self._low_sub.Init(self._on_low, 10) self._low_sub.Init(self._on_low, 10)
if SportModeState_ is not None: if SportModeState_ is not None:
self._sport_sub = ChannelSubscriber("rt/lf/sportmodestate", SportModeState_) self._sport_sub = ChannelSubscriber("rt/lf/sportmodestate", SportModeState_)
self._sport_sub.Init(self._on_sport, 10) self._sport_sub.Init(self._on_odom, 10)
self.ok = True self.ok = True
log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state)", log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state%s)",
self.cfg.dds_domain, self.cfg.dds_interface) self.cfg.dds_domain, self.cfg.dds_interface,
" + sportmode" if SportModeState_ is not None else "")
except Exception as e: except Exception as e:
log.warning("DDS init failed (%s) — heartbeat mode", e) log.warning("DDS init failed (%s) — heartbeat mode", e)
def _init_loco(self) -> None:
return # Go2 has no r1/g1-style loco FSM RPC
def _on_low(self, msg) -> None: def _on_low(self, msg) -> None:
try: try:
try: # battery from the nested BMS state
v = getattr(msg, "version", None)
if v is not None and "robot" not in self._fw:
vals = [int(x) for x in v] if hasattr(v, "__iter__") else [int(v)]
self._fw["robot"] = ".".join(map(str, vals))
except Exception:
pass
bms = getattr(msg, "bms_state", None) or getattr(msg, "bms", None) bms = getattr(msg, "bms_state", None) or getattr(msg, "bms", None)
if bms is not None: if bms is not None:
soc = int(getattr(bms, "soc", 0) or 0) soc = int(getattr(bms, "soc", 0) or 0)
cur = int(getattr(bms, "current", 0) or 0) # mA cur = int(getattr(bms, "current", 0) or 0) # mA
try:
vh, vl = getattr(bms, "version_high", None), getattr(bms, "version_low", None)
if vh is not None:
self._fw["bms"] = f"{int(vh)}.{int(vl or 0)}"
except Exception:
pass
# Go2: pack voltage is LowState_.power_v (V); pack temp from the
# BMS NTC sensors (bq_ntc / mcu_ntc, °C). All defensive getattrs.
volt_v = None volt_v = None
try: try:
pv = getattr(msg, "power_v", None) pv = getattr(msg, "power_v", None)
@ -434,16 +475,22 @@ class DDSReader:
volt_v = None volt_v = None
temp_c = None temp_c = None
try: try:
ntc_vals = [] ntc = []
for attr in ("bq_ntc", "mcu_ntc"): for attr in ("bq_ntc", "mcu_ntc"):
nt = getattr(bms, attr, None) nt = getattr(bms, attr, None)
if nt is not None: if nt is not None:
vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)] vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)]
ntc_vals.extend(v for v in vals if -40 <= v <= 150) ntc.extend(v for v in vals if -40 <= v <= 150)
if ntc_vals: if ntc:
temp_c = max(ntc_vals) temp_c = max(ntc)
except Exception: except Exception:
temp_c = None temp_c = None
try:
vh, vl = getattr(bms, "version_high", None), getattr(bms, "version_low", None)
if vh is not None:
self._fw["bms"] = f"{int(vh)}.{int(vl or 0)}"
except Exception:
pass
with self._lock: with self._lock:
self._bms = { self._bms = {
"soc": max(0, min(100, soc)), "soc": max(0, min(100, soc)),
@ -453,6 +500,7 @@ class DDSReader:
"soh": int(getattr(bms, "soh", 0) or 0), "soh": int(getattr(bms, "soh", 0) or 0),
"cycles": int(getattr(bms, "cycle", 0) or 0), "cycles": int(getattr(bms, "cycle", 0) or 0),
} }
self._bms_ts = time.monotonic()
temps: List[float] = [] temps: List[float] = []
max_dq = 0.0 max_dq = 0.0
for m in (getattr(msg, "motor_state", None) or []): for m in (getattr(msg, "motor_state", None) or []):
@ -477,13 +525,12 @@ class DDSReader:
except Exception: except Exception:
pass pass
def _on_sport(self, msg) -> None: def _on_odom(self, msg) -> None:
try: try:
pos = getattr(msg, "position", None) pos = getattr(msg, "position", None)
if pos is not None and len(pos) >= 2: if pos is not None and len(pos) >= 2:
with self._lock: with self._lock:
self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)} self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)}
self._sport_ts = time.monotonic()
except Exception: except Exception:
pass pass
@ -499,6 +546,56 @@ class DDSReader:
"fw": dict(self._fw), "fw": dict(self._fw),
} }
def fsm_id(self) -> Optional[int]:
return None # Go2 has no loco FSM id RPC
# Go2 has no r1/g1 FSM-id scheme (SportClient modes). Placeholders — UNVERIFIED.
_FSM_STATUS = {}
# Control-panel mode labels + the switchable set.
_CONTROL_MODES = {0: "idle", 1: "stand", 2: "walk"}
_CONTROL_SWITCHABLE = ["damp", "stand", "walk"]
_control_cache: Dict[str, Any] = {"ts": 0.0, "data": None}
def read_control(cfg: Config) -> Optional[Dict[str, Any]]:
"""READ-ONLY control status from the Sanad dashboard's /api/controller/status
(no DDS, no motion). Reports the current loco mode + the switchable set.
Remote SWITCHING is a separate, motion-capable path gated by CONTROL_ENABLE."""
now = time.monotonic()
if _control_cache["data"] is not None and now - _control_cache["ts"] < 1.5:
return _control_cache["data"]
url = cfg.control_url
if not url:
port = _REMOTE_STAT.get("port")
if not port:
return None
url = f"http://127.0.0.1:{port}/api/controller/status"
try:
r = requests.get(url, timeout=2)
d = r.json() if r.ok else None
except Exception:
d = None
if not isinstance(d, dict):
_control_cache.update(ts=now, data=None)
return None
fid = d.get("fsm_id")
out = {
"fsm_id": fid,
"mode": _CONTROL_MODES.get(fid, "unknown") if fid is not None else None,
"armed": d.get("armed"),
"walk_ready": d.get("walk_ready"),
"teleop_active": d.get("teleop_active"),
"last_velocity": d.get("last_velocity"),
"sdk_available": d.get("sdk_available"),
"switchable_modes": _CONTROL_SWITCHABLE,
"remote_switch_enabled": bool(cfg.control_enable), # remote mode-switch armed?
}
_control_cache.update(ts=now, data=out)
return out
class RosbridgePosition: class RosbridgePosition:
def __init__(self, cfg: Config): def __init__(self, cfg: Config):
@ -509,7 +606,7 @@ class RosbridgePosition:
try: try:
import websocket # noqa: F401 import websocket # noqa: F401
except Exception as e: except Exception as e:
log.warning("websocket-client absent (%s) — position disabled", e) log.warning("websocket-client absent (%s) — rosbridge position disabled", e)
self._ok = False self._ok = False
return return
self._ok = True self._ok = True
@ -1238,7 +1335,9 @@ def logs_loop(cfg: Config, session: requests.Session) -> None:
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
# remote dashboard (discover Sanad web UI, register its URL via /{sn}/remote) # remote dashboard — discover the Sanad web UI and register its URL for the
# fleet dashboard to embed ("full dashboard through the fleet API"). No changes
# to the Sanad app: we only probe its port and POST the URL to /{sn}/remote.
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
_REMOTE_STAT: Dict[str, Any] = {"url": None, "port": None, "kind": None, "ok": None, _REMOTE_STAT: Dict[str, Any] = {"url": None, "port": None, "kind": None, "ok": None,
"ssh": None, "ssh_ok": None} "ssh": None, "ssh_ok": None}
@ -1333,59 +1432,161 @@ def remote_loop(cfg: Config, session: requests.Session) -> None:
_ALERT_SEEN: set = set() _ALERT_SEEN: set = set()
def _post_alert(cfg: Config, session: requests.Session, text: str) -> bool:
"""POST a single alert string to /{sn}/alert; updates the alert status."""
try:
r = session.post(cfg.alert_url(),
json={"sn": cfg.sn, "name": cfg.name,
"alert": text, "message": text, "ts": int(time.time())},
headers=cfg.auth_headers(),
timeout=cfg.http_timeout, verify=cfg.verify_tls)
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=bool(r.ok))
if r.ok:
_ALERTS_STAT["sent"] += 1
log.info("alert sent: %s -> HTTP %s", text[:120], r.status_code)
return bool(r.ok)
except requests.RequestException as e:
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=False)
log.debug("alert send failed (%s): %s", text[:60], e)
return False
def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None:
"""POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.
Includes LOW_BATTERY (<= LOW_SOC, default 50%), MOTOR_OVERTEMP, COMMS_STALE."""
global _ALERT_SEEN global _ALERT_SEEN
current = set(faults) current = set(faults)
new = current - _ALERT_SEEN new = current - _ALERT_SEEN
_ALERT_SEEN = current _ALERT_SEEN = current
for f in sorted(new): for f in sorted(new):
_post_alert(cfg, session, f)
class LogAlertScanner:
"""Scans the robot's project logs (sanadr1) for error/billing patterns and
fires an alert on each NEW signature (deduped with a cooldown). Catches the
Gemini 'credits depleted' billing error and any ERROR/Traceback the app logs."""
def __init__(self, cfg: Config):
import re
self._path: Optional[Path] = None
self._pos = 0
self._cooldown = cfg.alert_log_cooldown
self._seen: Dict[str, float] = {}
self._patterns: List[Any] = []
self._pending: List[Any] = []
for entry in cfg.alert_log_patterns.split(";;"):
if "=" in entry:
code, rx = entry.split("=", 1)
try: try:
r = session.post(cfg.alert_url(), # case-SENSITIVE (uppercase log levels); use (?i) inline for text
json={"sn": cfg.sn, "name": cfg.name, self._patterns.append((code.strip(), re.compile(rx)))
"alert": f, "message": f, "ts": int(time.time())}, except re.error as e:
headers=cfg.auth_headers(), log.warning("bad alert pattern %s: %s", code, e)
timeout=cfg.http_timeout, verify=cfg.verify_tls) if _PROJECT_TAIL is not None and _PROJECT_TAIL.active and _PROJECT_TAIL._cur:
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) self._path = _PROJECT_TAIL._cur
if r.ok:
_ALERTS_STAT["sent"] += 1
log.info("alert sent: %s -> HTTP %s", f, r.status_code)
except requests.RequestException as e:
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False)
log.debug("alert send failed (%s): %s", f, e)
_ALERT_SEEN: set = set()
def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None:
"""POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings."""
global _ALERT_SEEN
current = set(faults)
new = current - _ALERT_SEEN
_ALERT_SEEN = current
for f in sorted(new):
try: try:
r = session.post(cfg.alert_url(), size = self._path.stat().st_size
json={"sn": cfg.sn, "name": cfg.name, self._pos = size # ongoing reads start at EOF
"alert": f, "message": f, "ts": int(time.time())}, # STARTUP BACKFILL: scan a large tail once (access-logs bury real
headers=cfg.auth_headers(), # errors) so a currently-active error (e.g. Gemini billing) alerts
timeout=cfg.http_timeout, verify=cfg.verify_tls) # right away. Only the matching lines are kept — cheap.
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) back = min(size, cfg.alert_backfill_bytes)
if r.ok: with self._path.open("rb") as f:
_ALERTS_STAT["sent"] += 1 f.seek(size - back)
log.info("alert sent: %s -> HTTP %s", f, r.status_code) raw = f.read(back).decode("utf-8", "replace").splitlines()
except requests.RequestException as e: if back < size and raw:
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) raw = raw[1:] # first line is partial
log.debug("alert send failed (%s): %s", f, e) matches = []
for ln in raw:
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
matches.append((code, ln))
break
# keep the most-recent unique signature per (code+line), cap 20
seen = set()
uniq = []
for code, ln in reversed(matches):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
if sig in seen:
continue
seen.add(sig)
uniq.append((code, ln))
self._pending = list(reversed(uniq))[-20:]
except Exception:
self._pos = 0
if self._path and self._patterns:
log.info("log-alert scan on %s (%d patterns, %d backfilled)",
self._path.name, len(self._patterns), len(self._pending))
def scan(self, cfg: Config, session: requests.Session) -> None:
if not self._path or not self._patterns:
return
import re
now0 = time.monotonic()
if self._pending: # flush startup backfill first
pend, self._pending = self._pending, []
for code, ln in pend:
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
self._seen[sig] = now0
_post_alert(cfg, session, f"{code}: {ln[:220]}")
try:
st = self._path.stat()
if st.st_size < self._pos: # rotated
self._pos = 0
if st.st_size <= self._pos:
return
with self._path.open("rb") as f:
f.seek(self._pos)
chunk = f.read(min(st.st_size - self._pos, 512 * 1024))
self._pos = f.tell()
except Exception as e:
log.debug("alert scan read failed: %s", e)
return
now = time.monotonic()
for ln in chunk.decode("utf-8", "replace").splitlines():
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120] # dedup key
if now - self._seen.get(sig, -1e9) > self._cooldown:
self._seen[sig] = now
_post_alert(cfg, session, f"{code}: {ln[:220]}")
break
_LOG_ALERTS: Optional[LogAlertScanner] = None
def alert_scan_loop(cfg: Config, session: requests.Session) -> None:
while True:
time.sleep(cfg.alert_scan_interval)
try:
if _LOG_ALERTS is not None:
_LOG_ALERTS.scan(cfg, session)
except Exception as e:
log.debug("alert scan loop: %s", e)
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
# telemetry assembly # telemetry assembly
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]: def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[str]:
# STRINGS, not objects — the fleet ingest 500s on fault objects. # STRINGS, not objects — the fleet ingest 500s on fault objects.
faults: List[str] = [] faults: List[str] = []
bms = snap.get("bms") bms = snap.get("bms")
@ -1399,7 +1600,8 @@ def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]:
return faults return faults
def derive_status(cfg: Config, snap: Dict[str, Any]) -> str: 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") bms = snap.get("bms")
charging = bool(bms and bms.get("current_a", 0.0) > 0.05) 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 alive = snap.get("low_age") is not None and snap["low_age"] <= 3.0
@ -1409,7 +1611,7 @@ def derive_status(cfg: Config, snap: Dict[str, Any]) -> str:
return "charging" return "charging"
if snap.get("max_dq", 0.0) > 0.15: if snap.get("max_dq", 0.0) > 0.15:
return "moving" return "moving"
return "idle" return base or "idle"
def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
@ -1417,26 +1619,26 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
sim: Optional[Dict[str, Any]] = None) -> Dict[str, Any]: sim: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
if sim is not None: if sim is not None:
snap = {"bms": {"soc": sim["battery"], "current_a": 0.5 if sim["charging"] else -0.3, snap = {"bms": {"soc": sim["battery"], "current_a": 0.5 if sim["charging"] else -0.3,
"voltage_v": 28.6, "temp_c": 36, "soh": 100, "cycles": 45}, "voltage_v": 47.5, "temp_c": 36, "soh": 100, "cycles": 45},
"low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0), "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0),
"xy": sim.get("position")} "xy": sim.get("position")}
fsm = sim.get("fsm")
else: else:
snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0, "xy": None} 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") bms = snap.get("bms")
battery = bms["soc"] if bms else None battery = bms["soc"] if bms else None
charging = bool(bms and bms.get("current_a", 0.0) > 0.05) charging = bool(bms and bms.get("current_a", 0.0) > 0.05)
status = derive_status(cfg, snap) status = derive_status(cfg, snap, fsm)
faults = derive_faults(cfg, snap) faults = derive_faults(cfg, snap)
# Battery detail (voltage / current / pack temp / health / cycles).
battery_detail = None battery_detail = None
if bms: if bms:
battery_detail = {"voltage_v": bms.get("voltage_v"), "current_a": bms.get("current_a"), battery_detail = {"voltage_v": bms.get("voltage_v"), "current_a": bms.get("current_a"),
"temp_c": bms.get("temp_c"), "soh": bms.get("soh"), "temp_c": bms.get("temp_c"), "soh": bms.get("soh"),
"cycles": bms.get("cycles")} "cycles": bms.get("cycles")}
# Motor temperature stats; null = temps not receiving.
temps = snap.get("temps") or [] temps = snap.get("temps") or []
motor_temp = ({"max": round(max(temps), 1), "avg": round(sum(temps) / len(temps), 1), motor_temp = ({"max": round(max(temps), 1), "avg": round(sum(temps) / len(temps), 1),
"min": round(min(temps), 1)} if temps else None) "min": round(min(temps), 1)} if temps else None)
@ -1447,23 +1649,30 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
return { return {
"sn": cfg.sn, "sn": cfg.sn,
"name": cfg.name, # friendly display name (e.g. go2_XX) "name": cfg.name, # friendly display name (e.g. go2_77)
"mac": mac, "mac": mac,
"brand": cfg.brand, "brand": cfg.brand,
"type": cfg.robot_type, # humanoid | dog "type": cfg.robot_type, # humanoid
"model": cfg.model, # r1 | g1 | go2 "model": cfg.model, # g1
"battery": battery, "charging": charging, "software": read_software(cfg), # ros/os/kernel/arch/python/agent
"firmware": {**read_firmware_static(), **(snap.get("fw") or {})},
# board/l4t/kernel + live robot/bms fw versions
"battery": battery, # null = couldn't read (heartbeat)
"charging": charging,
"battery_detail": battery_detail, "battery_detail": battery_detail,
"motor_temp": motor_temp, # null = not receiving "motor_temp": motor_temp, # null = not receiving
"storage": read_storage(cfg), "storage": read_storage(cfg),
"status": status, "status": status,
"position": position, "faults": faults, "position": position, # null when no odom/localization source
"control": read_control(cfg), # loco mode (zero_torque/damp/lock/running) + switchable set
"faults": faults,
"map": get_map_status(), # SHOWS whether the saved map made it to the server "map": get_map_status(), # SHOWS whether the saved map made it to the server
"logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok)
"project_logs": (_PROJECT_TAIL.label "project_logs": (_PROJECT_TAIL.label
if _PROJECT_TAIL is not None and _PROJECT_TAIL.active if _PROJECT_TAIL is not None and _PROJECT_TAIL.active
else None), # e.g. "sanadr1-logs"; null = no project found else None), # e.g. "sanadr1-logs"; null = no project found
"remote": (dict(_REMOTE_STAT) if _REMOTE_STAT.get("url") else None), "remote": (dict(_REMOTE_STAT) if _REMOTE_STAT.get("url") else None),
# Sanad dashboard URL registered for the fleet UI
"alerts": dict(_ALERTS_STAT), # alert status (sent, last, last_time, ok) "alerts": dict(_ALERTS_STAT), # alert status (sent, last, last_time, ok)
"time": _now_str(), # full date+time of this post "time": _now_str(), # full date+time of this post
"started_at": _STARTED["now"], # when this agent run started "started_at": _STARTED["now"], # when this agent run started
@ -1483,9 +1692,10 @@ def post_telemetry(cfg: Config, payload: Dict[str, Any], session: requests.Sessi
if not r.ok: if not r.ok:
log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200]) log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200])
return False return False
log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d -> HTTP %s", mp = payload.get("map") or {}
log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d map=%s -> HTTP %s",
payload["battery"], payload["charging"], payload["status"], payload["battery"], payload["charging"], payload["status"],
payload["position"], len(payload["faults"]), r.status_code) payload["position"], len(payload["faults"]), mp.get("state"), r.status_code)
return True return True
@ -1494,14 +1704,14 @@ def _sim_state(i: int) -> Dict[str, Any]:
battery = max(5, 90 - (i % 40)) battery = max(5, 90 - (i % 40))
moving = (i % 3) == 2 and not charging moving = (i % 3) == 2 and not charging
return {"battery": battery, "charging": charging, "temp": 45 + (i % 10), return {"battery": battery, "charging": charging, "temp": 45 + (i % 10),
"max_dq": 0.4 if moving else 0.0, "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)}} "position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}}
def cmd_list(cfg: Config) -> None: def cmd_list(cfg: Config) -> None:
maps = discover_maps(cfg) maps = discover_maps(cfg)
if not maps: if not maps:
print(f"(no maps under {cfg.maps_dir} for robot '{cfg.robot}')") print(f"(no .db maps under {cfg.maps_dir} for robot '{cfg.robot}')")
return return
print(f"{len(maps)} map(s) under {cfg.maps_dir} (robot={cfg.robot}):") print(f"{len(maps)} map(s) under {cfg.maps_dir} (robot={cfg.robot}):")
for m in maps: for m in maps:
@ -1509,19 +1719,23 @@ def cmd_list(cfg: Config) -> None:
print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points {m.description}") print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points {m.description}")
# --------------------------------------------------------------------------- #
# main
# --------------------------------------------------------------------------- #
def main(argv: Optional[List[str]] = None) -> int: def main(argv: Optional[List[str]] = None) -> int:
ap = argparse.ArgumentParser(description="Go2 fleet telemetry agent") ap = argparse.ArgumentParser(description="Go2 fleet agent: telemetry + map sync")
ap.add_argument("--simulate", action="store_true") ap.add_argument("--simulate", action="store_true", help="synthetic DDS state (map scan stays real)")
ap.add_argument("--once", action="store_true") ap.add_argument("--once", action="store_true", help="one map pass + one telemetry post, then exit")
ap.add_argument("--dry-run", action="store_true") ap.add_argument("--dry-run", action="store_true", help="build payloads, never POST")
ap.add_argument("--force", action="store_true", help="re-upload maps even if unchanged") ap.add_argument("--force", action="store_true", help="re-upload maps even if unchanged")
ap.add_argument("--list", action="store_true", help="list discovered maps and exit") ap.add_argument("--list", action="store_true", help="list discovered maps and exit")
ap.add_argument("--interval", type=float, default=None) ap.add_argument("--interval", type=float, default=None, help="override telemetry POLL_INTERVAL")
ap.add_argument("-v", "--verbose", action="store_true") ap.add_argument("-v", "--verbose", action="store_true")
args = ap.parse_args(argv) args = ap.parse_args(argv)
logging.basicConfig(level=logging.DEBUG if args.verbose else logging.INFO, logging.basicConfig(level=logging.DEBUG if args.verbose else logging.INFO,
format="%(asctime)s %(levelname)s %(name)s: %(message)s") format="%(asctime)s %(levelname)s %(name)s: %(message)s")
# buffer our own log lines for shipping to /{sn}/logs
_LOG_RING.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(name)s: %(message)s")) _LOG_RING.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(name)s: %(message)s"))
logging.getLogger().addHandler(_LOG_RING) logging.getLogger().addHandler(_LOG_RING)
_load_dotenv() _load_dotenv()
@ -1534,12 +1748,14 @@ def main(argv: Optional[List[str]] = None) -> int:
return 0 return 0
_init_start_times(cfg) _init_start_times(cfg)
global _PROJECT_TAIL global _PROJECT_TAIL, _LOG_ALERTS
_PROJECT_TAIL = ProjectLogTail(cfg) _PROJECT_TAIL = ProjectLogTail(cfg)
_LOG_ALERTS = LogAlertScanner(cfg) # error/billing alerts from the project logs
mac = read_mac(cfg.mac_interface) mac = read_mac(cfg.mac_interface)
log.info("sanad_api_go2 telemetry — sn=%s mac=%s server=%s iface=%s domain=%d%s", log.info("sanad_api_go2 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s "
cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain, "map_dir=%s map_every=%.0fs%s",
cfg.sn, cfg.name, mac, cfg.server_url, cfg.dds_interface,
cfg.position_source, cfg.maps_dir, cfg.map_poll_interval,
" [SIMULATE]" if args.simulate else "") " [SIMULATE]" if args.simulate else "")
reader = None reader = None
@ -1553,7 +1769,7 @@ def main(argv: Optional[List[str]] = None) -> int:
session = requests.Session() session = requests.Session()
tick = 0 tick = 0
def one() -> None: def one_telemetry() -> None:
nonlocal tick nonlocal tick
sim = _sim_state(tick) if args.simulate else None sim = _sim_state(tick) if args.simulate else None
payload = build_telemetry(cfg, mac, reader, pos, sim=sim) payload = build_telemetry(cfg, mac, reader, pos, sim=sim)
@ -1565,29 +1781,41 @@ def main(argv: Optional[List[str]] = None) -> int:
tick += 1 tick += 1
if args.once or args.dry_run: if args.once or args.dry_run:
# one map pass first so the telemetry "map" field reflects it
map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run) map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run)
if cfg.remote_enable and not args.dry_run:
register_remote(cfg, session)
elif cfg.remote_enable:
d = discover_dashboard(cfg)
if d:
_REMOTE_STAT.update(url=d["url"], port=d["port"], kind=cfg.remote_kind, ok=None)
if args.once: if args.once:
one(); ship_logs(cfg, session); return 0 one_telemetry()
if args.dry_run: ship_logs(cfg, session)
return 0
for _ in range(3): for _ in range(3):
one(); time.sleep(min(cfg.poll_interval, 1.0)) one_telemetry()
time.sleep(min(cfg.poll_interval, 1.0))
return 0 return 0
# loop mode: map sync + log shipping + remote registration in bg threads
threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=alert_scan_loop, args=(cfg, session), daemon=True).start()
if cfg.remote_enable: if cfg.remote_enable:
threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start()
log.info("loop every %.1fs; map every %.0fs; logs every %.0fs (Ctrl-C to stop)", log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)",
cfg.poll_interval, cfg.map_poll_interval, cfg.logs_interval) cfg.poll_interval, cfg.map_poll_interval, cfg.logs_interval)
while True: while True:
try: try:
one() one_telemetry()
except Exception as e: except Exception as e:
log.exception("tick failed: %s", e) log.exception("telemetry tick failed: %s", e)
try: try:
time.sleep(cfg.poll_interval) time.sleep(cfg.poll_interval)
except KeyboardInterrupt: except KeyboardInterrupt:
log.info("stopped"); return 0 log.info("stopped")
return 0
if __name__ == "__main__": if __name__ == "__main__":

19
agents/r1/.env.example
View File

@ -26,7 +26,7 @@ R1_POSITION_SOURCE=none
ROSBRIDGE_URL=ws://127.0.0.1:9090 ROSBRIDGE_URL=ws://127.0.0.1:9090
# ── fault thresholds ───────────────────────────────────────────────────────── # ── fault thresholds ─────────────────────────────────────────────────────────
LOW_SOC=15 LOW_SOC=50
MOTOR_TEMP_MAX=85 MOTOR_TEMP_MAX=85
# ── cadence / transport ────────────────────────────────────────────────────── # ── cadence / transport ──────────────────────────────────────────────────────
@ -75,3 +75,20 @@ REMOTE_KIND=web
# REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable) # REMOTE_HOST= # default: the robot's primary LAN IP (fleet-reachable)
# REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery # REMOTE_URL= # pin an explicit URL (e.g. a public reverse-tunnel) — wins over discovery
REMOTE_INTERVAL=60 REMOTE_INTERVAL=60
# ── control panel (READ-ONLY loco mode; remote SWITCH is off by design) ───────
# The agent reads the Sanad dashboard's /api/controller/status (no DDS, no
# motion) and reports the current mode (zero_torque/damp/lock/running) + the
# switchable set in telemetry.control. CONTROL_ENABLE stays 0: remote mode-
# SWITCHING is deliberately NOT built (it commands motion and can drop the robot;
# switching is done in person on the Sanad dashboard). Leave URL blank to use the
# auto-discovered dashboard port.
CONTROL_STATUS_URL=
CONTROL_ENABLE=0
# ── log-driven alerts (Gemini billing + any robot ERROR) ─────────────────────
# The agent scans the robot's project (sanadr1) logs; each NEW matching signature
# fires an alert (deduped per ALERT_LOG_COOLDOWN). "CODE=regex" entries split by ;;
# ALERT_LOG_PATTERNS=GEMINI_BILLING=prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini;;ROBOT_ERROR=\bERROR\b|Traceback|Exception|CRITICAL
ALERT_LOG_COOLDOWN=300
ALERT_SCAN_INTERVAL=10

225
agents/r1/sanad_api_r1.py
View File

@ -122,6 +122,10 @@ class Config:
rosbridge_url: str rosbridge_url: str
low_soc: int low_soc: int
motor_temp_max: float motor_temp_max: float
alert_log_patterns: str
alert_log_cooldown: float
alert_scan_interval: float
alert_backfill_bytes: int
poll_interval: float poll_interval: float
telemetry_endpoint: str telemetry_endpoint: str
# map sync # map sync
@ -151,6 +155,8 @@ class Config:
ssh_enable: bool ssh_enable: bool
ssh_user: str ssh_user: str
ssh_port: int ssh_port: int
control_url: str
control_enable: bool
# project logs (e.g. the robot's Sanad app) shipped alongside agent logs # project logs (e.g. the robot's Sanad app) shipped alongside agent logs
project_log_container: str project_log_container: str
project_log_path: str project_log_path: str
@ -188,8 +194,18 @@ class Config:
read_fsm=_env_bool("R1_READ_FSM", False), read_fsm=_env_bool("R1_READ_FSM", False),
position_source=_env("R1_POSITION_SOURCE", "none").lower(), position_source=_env("R1_POSITION_SOURCE", "none").lower(),
rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"), rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"),
low_soc=int(_env("LOW_SOC", "15")), low_soc=int(_env("LOW_SOC", "50")),
motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")),
# log-driven alerts: "CODE=regex" entries separated by ";;" (regex may
# contain '|'). Scanned against the robot's project logs (sanadr1).
# NOTE: matching is CASE-SENSITIVE (log levels are uppercase); use an
# inline (?i) prefix for case-insensitive text (Gemini messages).
alert_log_patterns=_env("ALERT_LOG_PATTERNS",
"GEMINI_BILLING=(?i)prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini"
";;ROBOT_ERROR=\\bERROR\\b|\\bCRITICAL\\b|^Traceback"),
alert_log_cooldown=float(_env("ALERT_LOG_COOLDOWN", "300")), # per-signature re-alert gap
alert_scan_interval=float(_env("ALERT_SCAN_INTERVAL", "10")),
alert_backfill_bytes=int(_env("ALERT_BACKFILL_BYTES", str(8 * 1024 * 1024))),
poll_interval=float(_env("POLL_INTERVAL", "2")), poll_interval=float(_env("POLL_INTERVAL", "2")),
telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"),
robot=_env("ROBOT", "sanad"), robot=_env("ROBOT", "sanad"),
@ -216,6 +232,8 @@ class Config:
ssh_enable=_env_bool("SSH_REGISTER", True), ssh_enable=_env_bool("SSH_REGISTER", True),
ssh_user=_env("SSH_USER", "unitree"), ssh_user=_env("SSH_USER", "unitree"),
ssh_port=int(_env("SSH_PORT", "22")), ssh_port=int(_env("SSH_PORT", "22")),
control_url=_env("CONTROL_STATUS_URL", ""), # Sanad /api/controller/status
control_enable=_env_bool("CONTROL_ENABLE", False), # remote mode-SWITCH (motion!) — off
project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"),
project_log_path=_env("PROJECT_LOG_PATH", ""), project_log_path=_env("PROJECT_LOG_PATH", ""),
project_log_label=_env("PROJECT_LOG_LABEL", ""), project_log_label=_env("PROJECT_LOG_LABEL", ""),
@ -579,7 +597,50 @@ class DDSReader:
# R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running. # R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running.
_FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "standing", 811: "ready"} _FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "locked_standing", 811: "running"}
# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).
_CONTROL_MODES = {0: "zero_torque", 1: "damp", 4: "lock", 811: "running"}
_CONTROL_SWITCHABLE = ["zero_torque", "damp", "lock", "running"]
_control_cache: Dict[str, Any] = {"ts": 0.0, "data": None}
def read_control(cfg: Config) -> Optional[Dict[str, Any]]:
"""READ-ONLY control status from the Sanad dashboard's /api/controller/status
(no DDS, no motion). Reports the current loco mode + the switchable set.
Remote SWITCHING is a separate, motion-capable path gated by CONTROL_ENABLE."""
now = time.monotonic()
if _control_cache["data"] is not None and now - _control_cache["ts"] < 1.5:
return _control_cache["data"]
url = cfg.control_url
if not url:
port = _REMOTE_STAT.get("port")
if not port:
return None
url = f"http://127.0.0.1:{port}/api/controller/status"
try:
r = requests.get(url, timeout=2)
d = r.json() if r.ok else None
except Exception:
d = None
if not isinstance(d, dict):
_control_cache.update(ts=now, data=None)
return None
fid = d.get("fsm_id")
out = {
"fsm_id": fid,
"mode": _CONTROL_MODES.get(fid, "unknown") if fid is not None else None,
"armed": d.get("armed"),
"walk_ready": d.get("walk_ready"),
"teleop_active": d.get("teleop_active"),
"last_velocity": d.get("last_velocity"),
"sdk_available": d.get("sdk_available"),
"switchable_modes": _CONTROL_SWITCHABLE,
"remote_switch_enabled": bool(cfg.control_enable), # remote mode-switch armed?
}
_control_cache.update(ts=now, data=out)
return out
class RosbridgePosition: class RosbridgePosition:
@ -1417,26 +1478,155 @@ def remote_loop(cfg: Config, session: requests.Session) -> None:
_ALERT_SEEN: set = set() _ALERT_SEEN: set = set()
def _post_alert(cfg: Config, session: requests.Session, text: str) -> bool:
"""POST a single alert string to /{sn}/alert; updates the alert status."""
try:
r = session.post(cfg.alert_url(),
json={"sn": cfg.sn, "name": cfg.name,
"alert": text, "message": text, "ts": int(time.time())},
headers=cfg.auth_headers(),
timeout=cfg.http_timeout, verify=cfg.verify_tls)
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=bool(r.ok))
if r.ok:
_ALERTS_STAT["sent"] += 1
log.info("alert sent: %s -> HTTP %s", text[:120], r.status_code)
return bool(r.ok)
except requests.RequestException as e:
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=False)
log.debug("alert send failed (%s): %s", text[:60], e)
return False
def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None:
"""POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.
Includes LOW_BATTERY (<= LOW_SOC, default 50%), MOTOR_OVERTEMP, COMMS_STALE."""
global _ALERT_SEEN global _ALERT_SEEN
current = set(faults) current = set(faults)
new = current - _ALERT_SEEN new = current - _ALERT_SEEN
_ALERT_SEEN = current _ALERT_SEEN = current
for f in sorted(new): for f in sorted(new):
_post_alert(cfg, session, f)
class LogAlertScanner:
"""Scans the robot's project logs (sanadr1) for error/billing patterns and
fires an alert on each NEW signature (deduped with a cooldown). Catches the
Gemini 'credits depleted' billing error and any ERROR/Traceback the app logs."""
def __init__(self, cfg: Config):
import re
self._path: Optional[Path] = None
self._pos = 0
self._cooldown = cfg.alert_log_cooldown
self._seen: Dict[str, float] = {}
self._patterns: List[Any] = []
self._pending: List[Any] = []
for entry in cfg.alert_log_patterns.split(";;"):
if "=" in entry:
code, rx = entry.split("=", 1)
try: try:
r = session.post(cfg.alert_url(), # case-SENSITIVE (uppercase log levels); use (?i) inline for text
json={"sn": cfg.sn, "name": cfg.name, self._patterns.append((code.strip(), re.compile(rx)))
"alert": f, "message": f, "ts": int(time.time())}, except re.error as e:
headers=cfg.auth_headers(), log.warning("bad alert pattern %s: %s", code, e)
timeout=cfg.http_timeout, verify=cfg.verify_tls) if _PROJECT_TAIL is not None and _PROJECT_TAIL.active and _PROJECT_TAIL._cur:
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) self._path = _PROJECT_TAIL._cur
if r.ok: try:
_ALERTS_STAT["sent"] += 1 size = self._path.stat().st_size
log.info("alert sent: %s -> HTTP %s", f, r.status_code) self._pos = size # ongoing reads start at EOF
except requests.RequestException as e: # STARTUP BACKFILL: scan a large tail once (access-logs bury real
_ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) # errors) so a currently-active error (e.g. Gemini billing) alerts
log.debug("alert send failed (%s): %s", f, e) # right away. Only the matching lines are kept — cheap.
back = min(size, cfg.alert_backfill_bytes)
with self._path.open("rb") as f:
f.seek(size - back)
raw = f.read(back).decode("utf-8", "replace").splitlines()
if back < size and raw:
raw = raw[1:] # first line is partial
matches = []
for ln in raw:
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
matches.append((code, ln))
break
# keep the most-recent unique signature per (code+line), cap 20
seen = set()
uniq = []
for code, ln in reversed(matches):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
if sig in seen:
continue
seen.add(sig)
uniq.append((code, ln))
self._pending = list(reversed(uniq))[-20:]
except Exception:
self._pos = 0
if self._path and self._patterns:
log.info("log-alert scan on %s (%d patterns, %d backfilled)",
self._path.name, len(self._patterns), len(self._pending))
def scan(self, cfg: Config, session: requests.Session) -> None:
if not self._path or not self._patterns:
return
import re
now0 = time.monotonic()
if self._pending: # flush startup backfill first
pend, self._pending = self._pending, []
for code, ln in pend:
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
self._seen[sig] = now0
_post_alert(cfg, session, f"{code}: {ln[:220]}")
try:
st = self._path.stat()
if st.st_size < self._pos: # rotated
self._pos = 0
if st.st_size <= self._pos:
return
with self._path.open("rb") as f:
f.seek(self._pos)
chunk = f.read(min(st.st_size - self._pos, 512 * 1024))
self._pos = f.tell()
except Exception as e:
log.debug("alert scan read failed: %s", e)
return
now = time.monotonic()
for ln in chunk.decode("utf-8", "replace").splitlines():
ln = ln.strip()
if ln.startswith("{"):
try:
ln = (json.loads(ln).get("log") or "").rstrip()
except Exception:
pass
if not ln:
continue
for code, rx in self._patterns:
if rx.search(ln):
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120] # dedup key
if now - self._seen.get(sig, -1e9) > self._cooldown:
self._seen[sig] = now
_post_alert(cfg, session, f"{code}: {ln[:220]}")
break
_LOG_ALERTS: Optional[LogAlertScanner] = None
def alert_scan_loop(cfg: Config, session: requests.Session) -> None:
while True:
time.sleep(cfg.alert_scan_interval)
try:
if _LOG_ALERTS is not None:
_LOG_ALERTS.scan(cfg, session)
except Exception as e:
log.debug("alert scan loop: %s", e)
# --------------------------------------------------------------------------- # # --------------------------------------------------------------------------- #
@ -1520,6 +1710,7 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
"storage": read_storage(cfg), "storage": read_storage(cfg),
"status": status, "status": status,
"position": position, # null when no odom/localization source "position": position, # null when no odom/localization source
"control": read_control(cfg), # loco mode (zero_torque/damp/lock/running) + switchable set
"faults": faults, "faults": faults,
"map": get_map_status(), # SHOWS whether the saved map made it to the server "map": get_map_status(), # SHOWS whether the saved map made it to the server
"logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok)
@ -1603,8 +1794,9 @@ def main(argv: Optional[List[str]] = None) -> int:
return 0 return 0
_init_start_times(cfg) _init_start_times(cfg)
global _PROJECT_TAIL global _PROJECT_TAIL, _LOG_ALERTS
_PROJECT_TAIL = ProjectLogTail(cfg) _PROJECT_TAIL = ProjectLogTail(cfg)
_LOG_ALERTS = LogAlertScanner(cfg) # error/billing alerts from the project logs
mac = read_mac(cfg.mac_interface) mac = read_mac(cfg.mac_interface)
log.info("sanad_api_r1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s " log.info("sanad_api_r1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s "
"map_dir=%s map_every=%.0fs%s", "map_dir=%s map_every=%.0fs%s",
@ -1655,6 +1847,7 @@ def main(argv: Optional[List[str]] = None) -> int:
# loop mode: map sync + log shipping + remote registration in bg threads # loop mode: map sync + log shipping + remote registration in bg threads
threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start()
threading.Thread(target=alert_scan_loop, args=(cfg, session), daemon=True).start()
if cfg.remote_enable: if cfg.remote_enable:
threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start() threading.Thread(target=remote_loop, args=(cfg, session), daemon=True).start()
log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)", log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)",

6
fleet_install.sh
View File

@ -111,6 +111,7 @@ DDS_INTERFACE=$iface
DDS_DOMAIN=0 DDS_DOMAIN=0
VERIFY_TLS=$vtls VERIFY_TLS=$vtls
POLL_INTERVAL=2 POLL_INTERVAL=2
LOW_SOC=50
ROBOT=sanad ROBOT=sanad
MAPS_DIR=/data/maps MAPS_DIR=/data/maps
DATA_DIR=/data/web_data DATA_DIR=/data/web_data
@ -120,6 +121,11 @@ MAP_UPLOAD_MODE=multipart
MAP_POLL_INTERVAL=30 MAP_POLL_INTERVAL=30
LOGS_INTERVAL=60 LOGS_INTERVAL=60
SOFTWARE_ROS=foxy SOFTWARE_ROS=foxy
PROJECT_LOG_CONTAINER=auto
REMOTE_ENABLE=1
CONTROL_ENABLE=0
ALERT_SCAN_INTERVAL=10
ALERT_LOG_COOLDOWN=300
EOF EOF
} }

253
tools/gen_agents.py Normal file
View File

@ -0,0 +1,253 @@
#!/usr/bin/env python3
"""Regenerate the r1 and go2 fleet agents from the canonical g1 agent.
g1 (agents/g1/sanad_api_g1.py) is the SOURCE OF TRUTH for all shared features
(telemetry, map sync, logs, alerts, project-logs, remote, control, software,
firmware, timing). r1 is a near-exact subset of g1 (same unitree_hg DDS; only
naming + FSM ids differ). go2 differs in the DDS family (unitree_go: battery is
nested in LowState.bms_state), so its DDSReader is swapped wholesale.
Run this after ANY change to g1 to keep the three agents consistent:
python3 tools/gen_agents.py
"""
import re
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
G1 = (ROOT / "agents/g1/sanad_api_g1.py").read_text()
# ─────────────────────────── r1 (unitree_hg subset) ───────────────────────────
def make_r1(src: str) -> str:
reps = [
("sanad_api_g1", "sanad_api_r1"),
("— G1 fleet agent", "— R1 fleet agent"),
("The single G1 agent", "The single R1 agent"),
("Unitree G1, unitree_hg DDS", "Unitree R1 EDU, unitree_hg DDS — same family as the G1"),
('description="G1 fleet agent: telemetry + map sync"',
'description="R1 fleet agent: telemetry + map sync"'),
('_env("SN", "g1_0000")', '_env("SN", "r1_0000")'),
('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "r1"'),
('"G1_READ_FSM"', '"R1_READ_FSM"'),
('_env("G1_POSITION_SOURCE", "odom")', '_env("R1_POSITION_SOURCE", "none")'),
('"fsm": 200 if moving else 4', '"fsm": 811 if moving else 4'),
("(G1 ids: 200 walk-ready / 4 stand / 2 squat / 702 lie2stand)",
"(R1 ids: 0 zero-torque / 1 damping / 4 standing / 811 gait-running)"),
("e.g. g1_58", "e.g. r1_82"),
("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth10)"),
# FSM + control-mode maps (R1 ids)
('# G1 FSM ids (differ from the R1\'s): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand.\n'
'_FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"}\n'
'# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n'
'_CONTROL_MODES = {200: "running", 4: "lock", 2: "squat", 702: "lie2stand", 0: "zero_torque", 1: "damp"}',
'# R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running.\n'
'_FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "locked_standing", 811: "running"}\n'
'# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n'
'_CONTROL_MODES = {0: "zero_torque", 1: "damp", 4: "lock", 811: "running"}'),
]
out = src
for a, b in reps:
if a not in out:
sys.exit(f"[r1] anchor missing: {a!r}")
out = out.replace(a, b)
return out
# ─────────────────────────── go2 (unitree_go DDS) ─────────────────────────────
GO2_DDS_READER = '''class DDSReader:
"""Subscribes rt/lowstate (unitree_go LowState_); battery is NESTED in
LowState_.bms_state (Go2 has no separate rt/lf/bmsstate). Optional position
from rt/lf/sportmodestate. Passive reads only; the only RPC ever issued is
GET_FSM_ID (Go2 has no loco FSM RPC, so it stays off)."""
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._fw: Dict[str, Any] = {}
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_go.msg.dds_ import LowState_
SportModeState_ = None
if self.cfg.position_source in ("sportmode", "odom"):
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_odom, 10)
self.ok = True
log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state%s)",
self.cfg.dds_domain, self.cfg.dds_interface,
" + sportmode" if SportModeState_ is not None else "")
except Exception as e:
log.warning("DDS init failed (%s) — heartbeat mode", e)
def _init_loco(self) -> None:
return # Go2 has no r1/g1-style loco FSM RPC
def _on_low(self, msg) -> None:
try:
# battery from the nested BMS state
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
volt_v = None
try:
pv = getattr(msg, "power_v", None)
if pv:
volt_v = round(float(pv), 1)
except Exception:
volt_v = None
temp_c = None
try:
ntc = []
for attr in ("bq_ntc", "mcu_ntc"):
nt = getattr(bms, attr, None)
if nt is not None:
vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)]
ntc.extend(v for v in vals if -40 <= v <= 150)
if ntc:
temp_c = max(ntc)
except Exception:
temp_c = None
try:
vh, vl = getattr(bms, "version_high", None), getattr(bms, "version_low", None)
if vh is not None:
self._fw["bms"] = f"{int(vh)}.{int(vl or 0)}"
except Exception:
pass
with self._lock:
self._bms = {
"soc": max(0, min(100, soc)),
"current_a": round(cur / 1000.0, 2),
"voltage_v": volt_v,
"temp_c": temp_c,
"soh": int(getattr(bms, "soh", 0) or 0),
"cycles": int(getattr(bms, "cycle", 0) or 0),
}
self._bms_ts = time.monotonic()
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)]
# 0 = slot not reporting (unpopulated motor), not a real temp
temps.extend(v for v in vals if 0 < 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,
"fw": dict(self._fw),
}
def fsm_id(self) -> Optional[int]:
return None # Go2 has no loco FSM id RPC
'''
GO2_CONTROL = ('# Go2 has no r1/g1 FSM-id scheme (SportClient modes). Placeholders — UNVERIFIED.\n'
'_FSM_STATUS = {}\n'
'# Control-panel mode labels + the switchable set.\n'
'_CONTROL_MODES = {0: "idle", 1: "stand", 2: "walk"}\n'
'_CONTROL_SWITCHABLE = ["damp", "stand", "walk"]')
def make_go2(src: str) -> str:
reps = [
("sanad_api_g1", "sanad_api_go2"),
("— G1 fleet agent", "— Go2 fleet agent"),
("The single G1 agent", "The single Go2 agent"),
("Unitree G1, unitree_hg DDS", "Unitree Go2, unitree_go DDS (⚠ UNVERIFIED on hardware)"),
('description="G1 fleet agent: telemetry + map sync"',
'description="Go2 fleet agent: telemetry + map sync"'),
('_env("SN", "g1_0000")', '_env("SN", "go2_0000")'),
('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "go2"'),
('_env("ROBOT_TYPE", "humanoid")', '_env("ROBOT_TYPE", "dog")'),
('"G1_READ_FSM"', '"GO2_READ_FSM"'),
('_env("G1_POSITION_SOURCE", "odom")', '_env("GO2_POSITION_SOURCE", "none")'),
("e.g. g1_58", "e.g. go2_77"),
("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth0)"),
]
out = src
for a, b in reps:
if a not in out:
sys.exit(f"[go2] anchor missing: {a!r}")
out = out.replace(a, b)
# swap DDSReader class (from 'class DDSReader:' up to the FSM/control block)
start = out.index("class DDSReader:")
ctrl_start = out.index('# G1 FSM ids (differ from the R1', start)
out = out[:start] + GO2_DDS_READER + out[ctrl_start:]
# swap the FSM/control-modes block
ctrl_a = out.index('# G1 FSM ids (differ from the R1')
ctrl_b = out.index('_CONTROL_SWITCHABLE = ["zero_torque", "damp", "lock", "running"]')
ctrl_b = out.index("\n", ctrl_b)
out = out[:ctrl_a] + GO2_CONTROL + out[ctrl_b:]
return out
def main() -> int:
r1 = make_r1(G1)
go2 = make_go2(G1)
(ROOT / "agents/r1/sanad_api_r1.py").write_text(r1)
(ROOT / "agents/go2/sanad_api_go2.py").write_text(go2)
print(f"generated r1 ({len(r1.splitlines())} lines)")
print(f"generated go2 ({len(go2.splitlines())} lines)")
return 0
if __name__ == "__main__":
sys.exit(main())