From cc4df6dbadce6d64a235f2c08b1f9ac1dce5e975 Mon Sep 17 00:00:00 2001 From: kassam Date: Sat, 4 Jul 2026 23:37:56 +0400 Subject: [PATCH] Update 2026-07-04 23:37:55 --- .gitignore | 23 ++ README.md | 397 +++++++++++++++++++ assets/greeting.README.md | 43 +++ config.py | 357 +++++++++++++++++ gowelcome/__init__.py | 10 + gowelcome/control/__init__.py | 6 + gowelcome/control/pid.py | 113 ++++++ gowelcome/control/servoing.py | 111 ++++++ gowelcome/geo/__init__.py | 14 + gowelcome/geo/geofence.py | 126 ++++++ gowelcome/geo/gps.py | 334 ++++++++++++++++ gowelcome/perception/__init__.py | 15 + gowelcome/perception/detector.py | 116 ++++++ gowelcome/perception/road_mask.py | 91 +++++ gowelcome/perception/vision_thread.py | 134 +++++++ gowelcome/robot/__init__.py | 1 + gowelcome/robot/audio.py | 504 ++++++++++++++++++++++++ gowelcome/robot/factory.py | 59 +++ gowelcome/robot/go2_robot.py | 440 +++++++++++++++++++++ gowelcome/robot/interface.py | 123 ++++++ gowelcome/robot/mock_robot.py | 183 +++++++++ gowelcome/robot/webrtc_robot.py | 514 +++++++++++++++++++++++++ gowelcome/statemachine.py | 527 ++++++++++++++++++++++++++ gowelcome/types.py | 119 ++++++ gowelcome/web/__init__.py | 5 + gowelcome/web/stream.py | 291 ++++++++++++++ main.py | 497 ++++++++++++++++++++++++ pytest.ini | 6 + requirement.sh | 59 +++ requirements.txt | 36 ++ ruff.toml | 7 + scripts/run_mock.sh | 17 + scripts/run_robot.sh | 29 ++ scripts/smoke_test.py | 185 +++++++++ tests/__init__.py | 6 + tests/test_dashboard.py | 108 ++++++ tests/test_factory_transport.py | 81 ++++ tests/test_geo_statemachine.py | 280 ++++++++++++++ tests/test_geofence.py | 96 +++++ tests/test_gps.py | 72 ++++ tests/test_pid.py | 112 ++++++ tests/test_road_logic.py | 93 +++++ tests/test_servoing.py | 144 +++++++ tests/test_statemachine.py | 331 ++++++++++++++++ tests/test_web.py | 72 ++++ welcome.sh | 46 +++ 46 files changed, 6933 insertions(+) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 assets/greeting.README.md create mode 100644 config.py create mode 100644 gowelcome/__init__.py create mode 100644 gowelcome/control/__init__.py create mode 100644 gowelcome/control/pid.py create mode 100644 gowelcome/control/servoing.py create mode 100644 gowelcome/geo/__init__.py create mode 100644 gowelcome/geo/geofence.py create mode 100644 gowelcome/geo/gps.py create mode 100644 gowelcome/perception/__init__.py create mode 100644 gowelcome/perception/detector.py create mode 100644 gowelcome/perception/road_mask.py create mode 100644 gowelcome/perception/vision_thread.py create mode 100644 gowelcome/robot/__init__.py create mode 100644 gowelcome/robot/audio.py create mode 100644 gowelcome/robot/factory.py create mode 100644 gowelcome/robot/go2_robot.py create mode 100644 gowelcome/robot/interface.py create mode 100644 gowelcome/robot/mock_robot.py create mode 100644 gowelcome/robot/webrtc_robot.py create mode 100644 gowelcome/statemachine.py create mode 100644 gowelcome/types.py create mode 100644 gowelcome/web/__init__.py create mode 100644 gowelcome/web/stream.py create mode 100755 main.py create mode 100644 pytest.ini create mode 100755 requirement.sh create mode 100644 requirements.txt create mode 100644 ruff.toml create mode 100755 scripts/run_mock.sh create mode 100755 scripts/run_robot.sh create mode 100644 scripts/smoke_test.py create mode 100644 tests/__init__.py create mode 100644 tests/test_dashboard.py create mode 100644 tests/test_factory_transport.py create mode 100644 tests/test_geo_statemachine.py create mode 100644 tests/test_geofence.py create mode 100644 tests/test_gps.py create mode 100644 tests/test_pid.py create mode 100644 tests/test_road_logic.py create mode 100644 tests/test_servoing.py create mode 100644 tests/test_statemachine.py create mode 100644 tests/test_web.py create mode 100755 welcome.sh diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e113e87 --- /dev/null +++ b/.gitignore @@ -0,0 +1,23 @@ +# Python +__pycache__/ +*.py[cod] +*.egg-info/ +.venv/ +venv/ + +# Test / tooling caches +.pytest_cache/ +.mypy_cache/ +.ruff_cache/ + +# YOLO weights (auto-downloaded; large) and the greeting clip (user-provided) +*.pt +*.engine +assets/greeting.wav + +# OS / editor +.DS_Store +*.swp +*.pyc +Logs/ +*.log diff --git a/README.md b/README.md new file mode 100644 index 0000000..c79d60f --- /dev/null +++ b/README.md @@ -0,0 +1,397 @@ +# GoWelcome + +An autonomous **Unitree Go2** "backyard greeter". The robot dog wanders a yard, +spots a person with on-board vision, walks up to them via visual servoing, and +plays a friendly greeting (audio + gestures) -- all while steering clear of +roads and vehicles. + +GoWelcome is **mapless**, **reactive**, and **NON-ROS2**: a single Python +process running a fixed-rate state machine on top of the official +`unitree_sdk2py`. No SLAM, no global planner -- just perceive, decide, move. + +--- + +## Behaviour: the 5-state machine + +Arbitration priority each tick (highest first): +**e-stop / pause** → **stale-perception halt** → **AVOID_DANGER (cars/obstacles)** +→ **GPS hold (no fix)** → finish **GREET** → **BOUNDARY (geofence)** → +**APPROACH** → **WANDER + dog-play**. So cars always win, the dog never chases a +person out of the area, and idle play only happens when nothing else is going on. + +``` + +----------+ person (conf>thr) +-----------+ box fills +---------+ + | WANDER | ------------------> | APPROACH | -----------> | GREET | + | + play | <------ lost ------ | | stop_ratio | wav+wag | + +----------+ cooldown <----------------------------------+---------+ + ^ ^ ^ + | | +--- back inside ---- +--------------+ near geofence edge (GPS) + | | | BOUNDARY | home back toward centre + | +------ clear ---------- +--------------+ + | ^ + | road / vehicle near any state can preempt to + +------------------------ +--------------+ + | AVOID_DANGER | back up + pivot away + +--------------+ +``` + +- **WANDER** -- cruise forward (`wander.forward_speed`) with a gentle yaw sweep, + scanning the yard, and occasionally performing an idle **dog-play** action + (see below). It also **keeps its distance from the road/cars**: as pavement or + a vehicle appears ahead it veers toward the clear side and slows down (the + vision-only containment — see below). Transitions to **APPROACH** when a person + is detected above `perception.person_conf`. +- **APPROACH** -- run the visual servo (below) to centre and close on the + person. Falls back to **WANDER** if the person is missing for + `loop.person_lost_frames`. Transitions to **GREET** when the person's box + fills `servo.stop_height_ratio` of the frame height. +- **GREET** -- settle to a full stop (`greet.settle_time`), play the greeting + `wav`, run `greet.gestures` (spaced by `greet.gesture_gap`), then return to + **WANDER** and ignore people for `greet.cooldown` seconds. +- **AVOID_DANGER** -- preempts every other state (highest safety). Triggered by + HSV road coverage over `perception.road_trigger_coverage`, or a vehicle box + taller than `perception.danger_min_height_ratio`. Backs up (`avoid.backup_speed`) + then pivots away from the road until clear for `avoid.clear_frames` frames. +- **BOUNDARY** -- *optional* GPS keep-in-area behaviour (off by default; see + below). When enabled and the dog nears the geofence edge it homes back toward + the centre, and won't leave the area even to chase a person. + +A perception time-out (`safety.perception_timeout`) in any state stops the +robot until fresh frames arrive. + +--- + +## Stay-in-area: vision (default), GPS optional + +The dog patrols an **open area with no physical fence**. By **default it stays in +the area with vision** — no GPS needed: + +- **Soft road/car repulsion (WANDER):** as pavement appears in the lower frame + (HSV road mask), or a vehicle is detected (YOLO), the dog **veers toward the + clear side and slows down** *before* reaching the hard reaction — actively + keeping its distance from the road/cars (`avoid.soft_road_coverage`, + `road_repulsion_gain`, `car_repulsion_gain`). +- **Hard reaction (AVOID_DANGER):** up close (road fills the centre past + `perception.road_trigger_coverage`, or a near car), it backs up and pivots + away. The firmware **LiDAR hard-stop** sits underneath all of it. + +This relies on a usable visual border (a clear grass→pavement edge, decent +lighting) and is the recommended setup. Tune the gains in `config.py` +(`AvoidConfig`); set `avoid.soft_avoid_enabled = False` to keep only the hard +reaction. + +### Optional: GPS geofence (`--geofence`) + +For a hard metric boundary, add an external GPS receiver and enable the geofence +with `--geofence` (off by default). It adds the **BOUNDARY** state, homing the +dog back toward a centre point. + +> ⚠️ **The Go2 has no built-in GPS.** This requires an **external GPS receiver on +> the onboard computer** (USB u-blox-class into the Jetson), read via `gpsd` or +> serial NMEA. Standard GPS is accurate to **±2–5 m**, so keep `geofence.radius_m` +> well inside the real edge (use RTK GPS for tight bounds near a road). + +How it works: +1. On startup the first good fix becomes the fence **centre** (`center_mode: + onstart`), or set explicit `center_lat/lon` (`center_mode: fixed`), or press + **"Set fence centre here"** on the dashboard. +2. The dog roams freely within `geofence.radius_m`. Within `geofence.margin_m` + of the edge it enters **BOUNDARY** and homes back toward the centre, steering + with the GPS **course-over-ground** (no compass needed), until `release_m` + back inside (hysteresis). +3. **Fail-safe:** if GPS is lost/stale (`gps.stale_after`) the dog **stops** + (`geofence.no_fix_behavior: stop`) rather than roam blind near the edge. + +GPS source (`gps.source`): `auto` (probe gpsd, else serial), `gpsd`, `serial` +(NMEA on `gps.serial_port`), or `mock` (a simulated receiver that integrates the +commanded motion — for testing). `--gps`/`--radius` imply `--geofence`. + +--- + +## Act like a dog (idle play) + +While WANDERing, an idle scheduler occasionally performs a random dog action +(`play.actions`: `stretch`, `wiggle`, `scrape`/dig, `dance1`, `wallow`, ...), +pausing briefly to do the trick. Intensity is **runtime-settable** (default +`moderate`): `calm` (~75 s between actions), `moderate` (~30 s), `playful` +(~15 s) — change it any time from the dashboard or with `--play`. (The greeting +itself adds Hello/Heart "wags".) Play never overrides safety, the geofence, or a +greeting. + +--- + +## Architecture + +``` + camera frames + | + v ++---------------------+ latest() +-------------------------+ +| PerceptionThread | -------------------> | GoWelcomeStateMachine | +| (background thread) | PerceptionResult | step(dt) -> State | +| YOLO + HSV road | | WANDER/APPROACH/GREET/ | ++---------------------+ | AVOID_DANGER | + ^ +-------------------------+ + | get_frame() | + | | drive() / stop() / + | | gesture() / play_greeting() + | v + | +--------------------------+ + +--------------------------------- | RobotInterface | + | (abstract contract) | + +--------------------------+ + / | \ + +------------------+ +------------------+ +------------------+ + | Go2WebRTCRobot | | Go2Robot | | MockRobot | + | unitree_webrtc_ | | unitree_sdk2py | | webcam / video, | + | connect (DEFAULT)| | over CycloneDDS | | no hardware, for | + | wifi, AIR/PRO/EDU| | (--transport dds)| | off-robot dev/CI | + | + AudioHub audio | | wired / EDU | | | + +------------------+ +------------------+ +------------------+ + (async bridge: WebRTC event loop on its own thread) +``` + +- **`config.py`** -- every tunable lives here in grouped dataclasses + (`GoWelcomeConfig`). CLI flags in `main.py` override a handful at startup. +- **`gowelcome/types.py`** -- frozen data contracts: `State`, `Detection`, + `RoadInfo`, `PerceptionResult`. The shared language between layers. +- **`gowelcome/robot/interface.py`** -- the `RobotInterface` and `AudioBackend` + ABCs plus the `GESTURES` vocabulary. The behaviour layer talks only to these. +- **`PerceptionThread`** -- grabs frames from the robot, runs YOLO + the HSV + road mask off the control loop, and publishes the newest `PerceptionResult` + via `latest()`. +- **`GoWelcomeStateMachine`** -- the reactive brain; `step(dt)` reads the latest + perception and issues robot commands, returning the current `State`. + +Velocity convention everywhere (matches `SportClient.Move`): +`vx` forward+, `vy` left+, `vyaw` CCW/left+ (rad/s). + +--- + +## Transports & greeting audio + +GoWelcome talks to the robot through one of two transports (`--transport`): + +| Transport | Library | Works on | Greeting audio | When | +|-----------|---------|----------|----------------|------| +| **`webrtc`** *(default)* | `unitree_webrtc_connect` (app protocol) | Go2 **AIR/PRO/EDU over wifi** | ✅ **from the dog's speaker** (AudioHub) | default; no jailbreak | +| `dds` | official `unitree_sdk2py` (CycloneDDS) | Go2 **EDU, wired** | ❌ none on Go2 → host speaker | `--transport dds` | + +**Greeting from the dog (WebRTC default).** On startup GoWelcome uploads +`assets/greeting.wav` to the robot via **AudioHub** and plays it by uuid on each +greeting — sound comes from the **Go2's own speaker**. Pick the method with +`--audio-method`: +- `audiohub` *(default)* — upload once, `play_by_uuid` per greeting (persistent, low latency). +- `stream` — stream the file live each greeting via an aiortc `MediaPlayer`. + +**DDS transport audio.** The official SDK has **no Go2 audio path** (its +`AudioClient` is G1-only). On `--transport dds`, greeting audio falls back to a +**pluggable host backend** (`--audio host|go2|null`) that plays on the machine +running GoWelcome. The field-proven pattern (from the team's G1 `Sanad` stack) +is a USB/Bluetooth speaker on the onboard computer, pinned by its PulseAudio sink: + +```bash +pactl list short sinks # find your speaker's sink +python main.py --transport dds --interface eth0 \ + --audio-device alsa_output.usb-Anker_PowerConf_A3321-DEV-SN1-01.analog-stereo +``` + +Drop your clip at `assets/greeting.wav` — see `assets/greeting.README.md`. (DDS +host playback wants 16 kHz mono 16-bit PCM; AudioHub accepts any wav.) + +--- + +## Install + +### Off-robot (mock / development / tests) + +```bash +python3 -m venv .venv && source .venv/bin/activate +pip install -r requirements.txt # numpy, opencv-python, ultralytics, simpleaudio +``` + +You do **not** need any robot library for `--mock`. + +### On the real robot — WebRTC (default) + +```bash +sudo apt install -y portaudio19-dev +pip install -r requirements.txt # includes unitree_webrtc_connect +``` +For **Go2 firmware ≥ 1.1.15** you also need the per-device AES-128 key (once): +fetch it with the connector's `examples/fetch_aes_key.py`, then pass `--aes-key`. + +### On the real robot — DDS (alternative, EDU/wired) + +Install the official Unitree SDK on the robot's host (not a plain pip install): + +```bash +# https://github.com/unitreerobotics/unitree_sdk2_python +git clone https://github.com/unitreerobotics/unitree_sdk2_python +cd unitree_sdk2_python && pip install -e . # pulls in cyclonedds +``` + +`pyserial` is **not** required. + +--- + +## Run + +```bash +# Off-robot, webcam index 0, silent audio: +python main.py --mock --audio null --source 0 +# or: ./scripts/run_mock.sh + +# Off-robot from a video file with the debug window: +python main.py --mock --source backyard.mp4 + +# Real Go2 over WebRTC (default) — greeting plays from the dog's speaker: +python main.py --robot-ip 192.168.1.50 # add --aes-key on fw >= 1.1.15 +# or: ./welcome.sh --robot-ip 192.168.1.50 # TEST SUSPENDED FIRST + +# Real Go2 over DDS (EDU/wired): +python main.py --transport dds --interface eth0 + +# Useful flags: +python main.py --robot-ip 192.168.1.50 --device cuda --headless --web +python main.py --mock --dry-run # perceive + decide, never move +``` + +| Flag | Config field set | +|-----------------|------------------------------------------| +| `--mock` | `mock` | +| `--transport` | `transport` (`webrtc`/`dds`) | +| `--robot-ip` | `webrtc.ip` (localsta) | +| `--serial` | `webrtc.serial_number` | +| `--aes-key` | `webrtc.aes_128_key` (fw ≥ 1.1.15) | +| `--connection` | `webrtc.connection_method` | +| `--audio-method`| `webrtc.audio_method` (`audiohub`/`stream`) | +| `--interface` | `network.interface` (dds) | +| `--device` | `perception.device` | +| `--model` | `perception.model_path` | +| `--source` | `camera.mock_source` | +| `--wav` | `greet.wav_path` | +| `--audio` | `audio.backend` (`host`/`go2`/`null`) | +| `--audio-device`| `audio.output_device` (PulseAudio sink) | +| `--no-avoidance`| `safety.use_lidar_avoidance = False` | +| `--headless` | `headless` (no cv2 window) | +| `--dry-run` | `dry_run` (decide but never move) | +| `--conf` | `perception.person_conf` | +| `--web` | `web.enabled` (control dashboard) | +| `--web-port` | `web.port` (default 8080) | +| `--geofence` | enable GPS geofence (default vision-only) | +| `--gps` | `gps.source` (`auto`/`gpsd`/`serial`/`mock`) | +| `--radius` | `geofence.radius_m` (metres) | +| `--play` | `play.mode` (`calm`/`moderate`/`playful`) | + +A live cv2 window (unless `--headless`) draws **green** person boxes, +**red** danger boxes, the road-coverage percentage, and the current state. +Press **ESC** in the window (or **Ctrl-C** in the terminal) to quit. + +### Control dashboard (HTTP) + +Add `--web` and open **`http://:8080/`** from any laptop/phone on the +network — ideal headless on the dog. The page shows the live camera (with the +detection/state overlay) **plus controls**: change **play mode** +(calm/moderate/playful), **pause/resume**, **E-STOP**, and **"set fence centre +here"**, with a live status panel (state, GPS fix, in/out of fence). + +```bash +./welcome.sh --robot-ip 192.168.1.50 --headless --web # dog, browse to its IP:8080 +./welcome.sh --mock --source 0 --web --web-port 9000 # off-robot demo +``` + +Endpoints: `/` (dashboard), `/stream.mjpg` (raw MJPEG), `/snapshot.jpg`, +`/status.json` (status), `POST /control` (commands), `/healthz`. Stdlib-only +(`http.server` + `cv2.imencode`) — no extra dependency, multiple viewers. + +> ⚠️ **Security:** the viewer binds `0.0.0.0` with **no authentication** — anyone +> on the same network can watch the camera at `http://:/`. That's +> intended for a trusted home LAN. On an untrusted network, set +> `web.host = "127.0.0.1"` in `config.py` (view only via an SSH tunnel), or +> leave `--web` off. + +--- + +## Tuning + +Everything is in **`config.py`**, grouped by subsystem. Common knobs: + +- **Detection** -- `perception.person_conf` (default `0.80`), `perception.device`, + `perception.model_path`, `perception.danger_classes`, + `perception.danger_min_height_ratio`. +- **Road mask** -- `perception.road_hsv_lower/upper`, `road_crop_frac`, + `road_trigger_coverage`. +- **Approach feel** -- `servo.kp_yaw`, `servo.max_yaw_rate`, `servo.yaw_deadband`, + `servo.kp_forward`, `servo.max_forward`, `servo.stop_height_ratio`, + `servo.yaw_sign` (flip if your camera mounting inverts left/right). +- **Wander** -- `wander.forward_speed`, `wander.yaw_sweep_rate/period`. +- **Greeting** -- `greet.gestures`, `greet.gesture_gap`, `greet.cooldown`. +- **Safety caps** -- `safety.max_vx/max_vy/max_vyaw`, `safety.perception_timeout`, + `safety.command_timeout`. + +--- + +## Visual-servoing math + +The servo turns a single person bounding box into a `(vx, vyaw)` command each +tick. Let `frame_w`, `frame_h` be the frame size and the box have centre `cx` +and height `h`. + +**Horizontal (yaw) error** -- normalised to `[-1, 1]`, `+` = right of centre: + +``` +err = (cx - frame_w/2) / (frame_w/2) # Detection.horizontal_offset +``` + +**Yaw command** -- a P(ID) controller on `err`, with a deadband and clamp: + +``` +vyaw = yaw_sign * PID(err) clamped to +/- servo.max_yaw_rate + |err| < servo.yaw_deadband -> vyaw = 0 +``` + +With the default `yaw_sign = -1`: a target to the **right** (`err > 0`) yields +`vyaw < 0` (a clockwise/right turn) -- the robot turns toward the person. Flip +`yaw_sign` if your mounting inverts this. + +**Distance proxy** -- how much of the frame height the box fills: + +``` +height_ratio = h / frame_h # Detection.height_ratio +arrived = height_ratio >= servo.stop_height_ratio (default 0.50) +``` + +**Forward command** -- proportional to remaining distance, throttled when the +heading error is large so the dog squares up before charging, and zeroed on +arrival: + +``` +vx = kp_forward * (stop_height_ratio - height_ratio) +vx *= exp(-forward_heading_falloff * |err|) # slow down off-axis +vx = clamp(vx, 0, servo.max_forward) # never reverse to approach +arrived -> vx = 0 +``` + +All commands then pass the global safety caps (`safety.max_*`) before reaching +`drive()`. + +--- + +## Safety notes + +- **E-stop = Ctrl-C.** SIGINT/SIGTERM set a stop flag; the loop then stops the + robot and runs a clean `shutdown()` (zero velocity, release avoidance, close + camera/audio). ESC in the debug window does the same. +- **Test suspended first.** Always run a new build with the Go2 hung off the + ground and a hand on Ctrl-C before letting it walk. +- **LiDAR firmware hard-stop.** On real hardware, `drive()` routes through the + Go2 `ObstaclesAvoidClient` when `safety.use_lidar_avoidance` is on (default). + This is a firmware-level last line of defence on top of GoWelcome's own + `AVOID_DANGER` logic. `--no-avoidance` disables it (use with care). +- **Velocity caps.** Every command is clamped by `safety.max_vx/vy/vyaw` after + the controllers run, so a controller bug can't command an unsafe speed. +- **Stale perception.** If no fresh frame arrives within + `safety.perception_timeout`, the robot stops. +- **`--dry-run`** runs the full perception + decision pipeline but never sends a + non-zero velocity -- handy for validating behaviour safely. diff --git a/assets/greeting.README.md b/assets/greeting.README.md new file mode 100644 index 0000000..6f78eba --- /dev/null +++ b/assets/greeting.README.md @@ -0,0 +1,43 @@ +# Greeting audio asset + +GoWelcome plays a single greeting clip when it reaches the `GREET` state. + +## What to drop here + +Put your greeting file at: + + assets/greeting.wav + +(This path is the default `config.GreetConfig.wav_path`; override at runtime +with `--wav /path/to/other.wav`.) + +## Required format + +For maximum compatibility across the host audio backends (`simpleaudio` and the +`aplay` fallback) and the experimental Go2 `audiohub` path, the clip should be: + +- **WAV / PCM** container +- **16 kHz** sample rate +- **mono** (1 channel) +- **16-bit** signed little-endian samples (`pcm_s16le`) + +## Converting an existing file + +Use `ffmpeg` to convert any source audio (mp3, m4a, a stereo/48 kHz wav, ...) +into the expected format: + + ffmpeg -i your_source.mp3 -ar 16000 -ac 1 -c:a pcm_s16le assets/greeting.wav + +Breakdown: +- `-ar 16000` -> resample to 16 kHz +- `-ac 1` -> downmix to mono +- `-c:a pcm_s16le` -> 16-bit signed little-endian PCM + +## Notes + +- This file (`greeting.README.md`) is documentation only and is never played. +- `greeting.wav` itself is intentionally **not** committed; add your own. +- If `greeting.wav` is missing, the audio backend logs a warning and the greet + proceeds without sound (gestures still run) -- it never crashes the loop. +- Keep the clip short (a few seconds); `GreetConfig.gesture_gap` / + `GreetConfig.cooldown` govern timing around it. diff --git a/config.py b/config.py new file mode 100644 index 0000000..e155081 --- /dev/null +++ b/config.py @@ -0,0 +1,357 @@ +"""Central configuration for GoWelcome. + +Every tunable lives here so field tuning never means hunting through modules. +Grouped by subsystem; all values are plain Python (no heavy imports) so this +file is safe to import anywhere, including unit tests. + +Paths are derived from this file's location (``PROJECT_ROOT``) -- never +hardcode an absolute project path. Override at runtime via ``main.py`` CLI +flags or by editing the dataclass defaults below. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from pathlib import Path +from typing import Tuple + +# Resolve the project root dynamically (this file lives at the repo root). +PROJECT_ROOT: Path = Path(__file__).resolve().parent +ASSETS_DIR: Path = PROJECT_ROOT / "assets" + + +@dataclass +class NetworkConfig: + """How to reach the Go2 over CycloneDDS (official SDK, the ``dds`` transport).""" + + interface: str = "eth0" # host NIC on the robot LAN (e.g. eth0 / wlan0 / enpXs0) + domain_id: int = 0 # DDS domain; 0 for a physical Go2 + + +@dataclass +class WebRTCConfig: + """How to reach the Go2 over WebRTC (the default ``webrtc`` transport). + + Uses ``unitree_webrtc_connect`` -- the same protocol as the Unitree app, so + it works on Go2 AIR/PRO/EDU over wifi and (uniquely) can play the greeting + from the dog's own speaker via AudioHub. No jailbreak required. + """ + + connection_method: str = "localsta" # localsta | localap | remote + ip: str = "" # robot IP for localsta (e.g. 192.168.x.x); "" -> discover by serial + serial_number: str = "" # for localsta discovery / remote signaling + aes_128_key: str = "" # 32-hex per-device key; REQUIRED on Go2 firmware >= 1.1.15 + username: str = "" # remote (cloud) signaling only + password: str = "" # remote (cloud) signaling only + region: str = "global" # global | cn + # How to play the greeting clip on the dog: + # "audiohub" -> upload greeting.wav to the robot once, play by uuid (default) + # "stream" -> stream the file live each greeting via an aiortc MediaPlayer + audio_method: str = "audiohub" + + +@dataclass +class PerceptionConfig: + """YOLO detection + HSV road masking.""" + + # --- YOLO --- + # Resolve a bundled weights file relative to the project so GoWelcome runs + # from ANY working directory; fall back to the bare name (ultralytics then + # auto-downloads it) when no local copy is present. + model_path: str = ( + str(PROJECT_ROOT / "yolov8n.pt") + if (PROJECT_ROOT / "yolov8n.pt").exists() + else "yolov8n.pt" + ) + device: str = "cpu" # "cpu", "cuda", "cuda:0" ... ("cuda" on Jetson) + use_half: bool = False # FP16 (discrete-GPU only; leave off on Jetson/CPU) + use_tracking: bool = False # model.track(persist=True) vs model.predict + tracker: str = "bytetrack.yaml" + infer_imgsz: int = 640 + person_conf: float = 0.80 # spec: APPROACH only when person conf > 0.80 + danger_conf: float = 0.40 # lower bar for vehicles (safety-biased) + person_classes: Tuple[str, ...] = ("person",) + danger_classes: Tuple[str, ...] = ("car", "truck", "bus", "motorcycle", "train") + # A danger box is only "near enough to matter" once it fills this much of + # the frame height (ignore tiny far-away cars on the horizon). + danger_min_height_ratio: float = 0.25 + + # --- HSV road / asphalt mask --- + road_enabled: bool = True + road_crop_frac: float = 0.30 # analyse the bottom 30% of the frame + # Asphalt/concrete is low-saturation grey. HSV ranges (OpenCV: H 0-179). + road_hsv_lower: Tuple[int, int, int] = (0, 0, 40) + road_hsv_upper: Tuple[int, int, int] = (179, 60, 200) + road_morph_kernel: int = 5 # opening kernel to de-speckle the mask + # Coverage of the crop above which we consider "road ahead". + road_trigger_coverage: float = 0.35 + + +@dataclass +class CameraConfig: + """Frame source sizing (mock backend / resize).""" + + width: int = 640 + height: int = 480 + # Mock backend frame source: integer webcam index, or a path to a video file. + mock_source: str = "0" + target_fps: float = 15.0 + + +@dataclass +class ServoConfig: + """Visual-servoing P(ID) controller: bbox centroid -> velocity.""" + + # Yaw: turn to centre the target horizontally. + kp_yaw: float = 1.2 # gain on normalised horizontal error (-1..1) + ki_yaw: float = 0.0 + kd_yaw: float = 0.05 + max_yaw_rate: float = 0.9 # rad/s clamp + yaw_deadband: float = 0.06 # |norm err| below this -> no turn + # Sign: with default Unitree convention (+vyaw = CCW/left), a target to the + # RIGHT (err>0) needs a right turn (vyaw<0), so vyaw = -kp*err. Flip if your + # robot/camera mounting inverts this. + yaw_sign: float = -1.0 + + # Forward: approach until the box fills the frame vertically. + kp_forward: float = 1.0 + max_forward: float = 0.45 # m/s (gentle backyard pace) + min_forward: float = 0.06 # below this, treat as stopped + # spec: trigger stop when the box fills ~50% of the vertical frame. + stop_height_ratio: float = 0.50 + # Throttle forward speed when heading error is large (don't charge sideways): + # scale = exp(-forward_heading_falloff * |norm_yaw_err|). + forward_heading_falloff: float = 2.0 + + +@dataclass +class WanderConfig: + """Open-loop roaming when no person is in view.""" + + forward_speed: float = 0.30 # m/s cruise + # Gentle sinusoidal yaw sweep so the dog scans the yard. + yaw_sweep_rate: float = 0.35 # rad/s peak + yaw_sweep_period: float = 6.0 # s per full left-right cycle + + +@dataclass +class AvoidConfig: + """Road/car avoidance: a soft 'keep away' steer in WANDER plus the hard + AVOID_DANGER reaction up close.""" + + # --- hard reaction (AVOID_DANGER) --- + backup_speed: float = 0.15 # m/s reverse while turning away + turn_rate: float = 0.7 # rad/s turn-away + # Stay in AVOID until the road/danger has been clear this many frames. + clear_frames: int = 8 + backup_duration: float = 0.8 # s of reverse before pivoting in place + + # --- soft 'stay away from the road/cars' steer (applied during WANDER) --- + # This is the vision-only area-containment: the dog veers away from pavement + # and slows down BEFORE it reaches the hard AVOID_DANGER trigger, so it keeps + # its distance from the road/cars rather than only reacting at the edge. + soft_avoid_enabled: bool = True + # Road coverage (in the bottom-crop thirds) above which the soft steer kicks + # in. Must be below perception.road_trigger_coverage (the hard trigger). + soft_road_coverage: float = 0.12 + road_repulsion_gain: float = 1.4 # rad/s of yaw bias per unit (left-right) road imbalance + road_slowdown_gain: float = 1.5 # forward-speed reduction per unit centre-road coverage + # Steer away from any detected vehicle (even far/small ones) by this much + # yaw per unit of its normalised horizontal offset; near cars still hard-avoid. + car_repulsion_gain: float = 0.7 + + +@dataclass +class GreetConfig: + """The greeting payload + re-greet gating.""" + + wav_path: str = str(ASSETS_DIR / "greeting.wav") + # Gestures performed during a greeting, in order. + gestures: Tuple[str, ...] = ("hello", "heart") + gesture_gap: float = 2.5 # s between gestures (let each finish) + audio_volume: int = 85 # 0-100 (applied where the backend supports it) + # After greeting, ignore people for this long so we don't loop on one person. + cooldown: float = 12.0 # s + settle_time: float = 0.6 # s to come to a full stop before gesturing + + +@dataclass +class AudioConfig: + """Greeting audio backend selection. + + backend: + "host" -> play on the machine running GoWelcome (aplay/simpleaudio). + Reliable; works regardless of robot model. DEFAULT. + "go2" -> EXPERIMENTAL: stream to the Go2 'audiohub' over DDS. Unverified + against firmware; may be a no-op. Test on hardware first. + "null" -> log only, no sound (used by the mock backend / CI). + """ + + backend: str = "host" + # Host backend: external player command (used if simpleaudio is absent or + # when output_device is set). Default is aplay (ALSA); paplay is used + # automatically when output_device names a PulseAudio sink. + host_player_cmd: Tuple[str, ...] = ("aplay", "-q") + # Optional output device to pin a *specific* speaker (e.g. a USB/Bluetooth + # speaker mounted on the Go2). When set, HostSpeakerAudio plays via + # `paplay --device=` so playback always lands on that sink + # regardless of the system default. This mirrors the proven pattern from + # the team's G1 "Sanad" stack (a USB Anker PowerConf speaker targeted by + # its PulseAudio sink name). Empty -> use the system default sink. + # Example: "alsa_output.usb-Anker_PowerConf_A3321-DEV-SN1-01.analog-stereo" + output_device: str = "" + + +@dataclass +class SafetyConfig: + """Cross-cutting safety limits + the firmware avoidance toggle.""" + + use_lidar_avoidance: bool = True # route drive() through ObstaclesAvoidClient + # Hard caps applied to *every* velocity command, after the controllers. + max_vx: float = 0.5 + max_vy: float = 0.3 + max_vyaw: float = 1.0 + # If perception goes stale (no new frame) longer than this, stop the robot. + perception_timeout: float = 0.8 # s + # Watchdog: if drive() isn't called within this period the robot decays to + # stop on its own (Unitree firmware behaviour); we also explicitly stop. + command_timeout: float = 0.5 # s + + +@dataclass +class WebConfig: + """Optional MJPEG web viewer -- watch the live annotated camera in a browser. + + When enabled, GoWelcome serves the latest frame (with YOLO person/danger + boxes, road coverage and the current state drawn on it) as an MJPEG stream + over plain HTTP. Open ``http://:/`` in any browser. Useful + for headless operation on the dog (no local display needed). + + Stdlib-only (``http.server`` + ``cv2.imencode``); no extra dependency. + """ + + enabled: bool = False + host: str = "0.0.0.0" # bind all interfaces so other machines can view + port: int = 8080 + fps: float = 10.0 # stream pacing (cap; never exceeds perception rate) + jpeg_quality: int = 80 # 1-100; lower = less bandwidth + + +@dataclass +class GpsConfig: + """External GPS receiver used for the area geofence. + + NOTE: the Go2 has NO built-in GPS -- this requires a USB/serial GPS receiver + on the onboard computer (read via gpsd or NMEA over serial). Standard GPS is + accurate to ~2-5 m, so keep the geofence well inside the real edge; use RTK + for tight bounds near roads. + """ + + enabled: bool = False # OFF by default -- GoWelcome is vision-only unless GPS is added + source: str = "auto" # auto | gpsd | serial | mock + serial_port: str = "/dev/ttyACM0" + serial_baud: int = 9600 + gpsd_host: str = "127.0.0.1" + gpsd_port: int = 2947 + min_satellites: int = 4 # reject fixes with fewer sats + max_hdop: float = 5.0 # reject fixes worse (larger) than this + stale_after: float = 3.0 # s; a fix older than this counts as "no fix" + # Mock source (off-robot testing): a virtual receiver that integrates the + # commanded velocity into a fake lat/lon so the geofence can be exercised. + mock_start_lat: float = 25.2048 + mock_start_lon: float = 55.2708 + + +@dataclass +class GeoFenceConfig: + """Optional GPS keep-in-area geofence + the BOUNDARY return behaviour. + + OFF by default: GoWelcome keeps the dog in the area with vision (road/car + avoidance). Enable this only when an external GPS receiver is fitted. + """ + + enabled: bool = False + center_mode: str = "onstart" # onstart (first good fix) | fixed + center_lat: float = 0.0 # used when center_mode == "fixed" + center_lon: float = 0.0 + radius_m: float = 15.0 # keep within this radius of the centre + margin_m: float = 3.0 # start homing back this far before the edge + release_m: float = 2.0 # extra hysteresis: only resume once this far back inside + return_speed: float = 0.35 # m/s while homing toward centre + homing_kp: float = 1.2 # vyaw per rad of (bearing-to-centre - course) + max_homing_yaw: float = 0.9 # rad/s clamp while homing + min_speed_for_course: float = 0.15 # m/s; below this GPS course is unreliable + # What to do when GPS is unavailable/stale while the fence is enabled: + # "stop" -> halt (safe; don't roam blind near a boundary). DEFAULT. + # "ignore" -> keep operating on perception-only avoidance (riskier). + no_fix_behavior: str = "stop" + + +@dataclass +class PlayConfig: + """Idle 'act like a dog' behaviour scheduler (runs during WANDER). + + ``mode`` is runtime-settable from the dashboard. Each mode sets the mean + seconds between random dog actions; actions are gestures the active robot + backend supports (unsupported ones are skipped). + """ + + mode: str = "moderate" # calm | moderate | playful + calm_interval: float = 75.0 + moderate_interval: float = 30.0 + playful_interval: float = 15.0 + jitter: float = 0.4 # +/- fraction randomisation of the interval + action_hold: float = 3.0 # s the dog pauses (stops roaming) to perform a trick + actions: Tuple[str, ...] = ("stretch", "wiggle", "scrape", "dance1", "wallow") + + def interval_for(self, mode: str) -> float: + return { + "calm": self.calm_interval, + "moderate": self.moderate_interval, + "playful": self.playful_interval, + }.get(mode, self.moderate_interval) + + +@dataclass +class LoopConfig: + """Main control-loop cadence + lost-target handling.""" + + rate_hz: float = 15.0 + # Frames a locked person may be missing before APPROACH falls back to WANDER. + person_lost_frames: int = 12 + + +@dataclass +class GoWelcomeConfig: + """Top-level config aggregating every subsystem.""" + + # Which robot transport to use on real hardware: + # "webrtc" -> Go2WebRTCRobot (default; app protocol, wifi, dog-speaker audio) + # "dds" -> Go2Robot (official unitree_sdk2py over CycloneDDS, wired/EDU) + transport: str = "webrtc" + + network: NetworkConfig = field(default_factory=NetworkConfig) + webrtc: WebRTCConfig = field(default_factory=WebRTCConfig) + perception: PerceptionConfig = field(default_factory=PerceptionConfig) + camera: CameraConfig = field(default_factory=CameraConfig) + servo: ServoConfig = field(default_factory=ServoConfig) + wander: WanderConfig = field(default_factory=WanderConfig) + avoid: AvoidConfig = field(default_factory=AvoidConfig) + greet: GreetConfig = field(default_factory=GreetConfig) + audio: AudioConfig = field(default_factory=AudioConfig) + safety: SafetyConfig = field(default_factory=SafetyConfig) + loop: LoopConfig = field(default_factory=LoopConfig) + web: WebConfig = field(default_factory=WebConfig) + gps: GpsConfig = field(default_factory=GpsConfig) + geofence: GeoFenceConfig = field(default_factory=GeoFenceConfig) + play: PlayConfig = field(default_factory=PlayConfig) + + # Runtime mode flags (set by main.py). + mock: bool = False # True -> MockRobot (no hardware) + headless: bool = False # True -> no cv2 debug window + dry_run: bool = False # True -> perception + decisions, but never move + + +def default_config() -> GoWelcomeConfig: + """Return a fresh fully-defaulted config.""" + return GoWelcomeConfig() diff --git a/gowelcome/__init__.py b/gowelcome/__init__.py new file mode 100644 index 0000000..02f2aaa --- /dev/null +++ b/gowelcome/__init__.py @@ -0,0 +1,10 @@ +"""GoWelcome - autonomous Go2 backyard greeter. + +A mapless, reactive Python state machine for the Unitree Go2 that wanders a +backyard, detects and approaches people with YOLO + visual servoing, plays a +greeting, and avoids roads/vehicles. Built on the official ``unitree_sdk2py``. + +See ``README.md`` for architecture and run instructions. +""" + +__version__ = "0.1.0" diff --git a/gowelcome/control/__init__.py b/gowelcome/control/__init__.py new file mode 100644 index 0000000..15b666b --- /dev/null +++ b/gowelcome/control/__init__.py @@ -0,0 +1,6 @@ +"""Control subpackage: PID primitives and bbox-only visual servoing. + +Pure-Python (numpy-free, SDK-free, cv2-free) so it imports and unit-tests +off-robot. See :mod:`gowelcome.control.pid` and +:mod:`gowelcome.control.servoing`. +""" diff --git a/gowelcome/control/pid.py b/gowelcome/control/pid.py new file mode 100644 index 0000000..7133bc8 --- /dev/null +++ b/gowelcome/control/pid.py @@ -0,0 +1,113 @@ +"""Minimal PID controller + angle helper. + +Adapted from ``Robot/go2_dog_behavior/simple_controllers.py``: keeps the +anti-windup integral clamp, the deadband-zeroes-integral-and-derivative +behaviour, and output saturation. The output-deadband compensation feature of +the original is intentionally dropped -- the visual servo does its own +min-forward gating, so it is not needed here. + +Pure module: only depends on :mod:`math`, so it imports and unit-tests with no +heavy/optional dependencies. +""" + +from __future__ import annotations + +import math +from typing import Optional, Tuple + + +def normalize_angle(angle: float) -> float: + """Wrap ``angle`` (radians) into ``[-pi, pi]``. + + Args: + angle: Angle in radians, any magnitude. + + Returns: + The equivalent angle in ``[-pi, pi]`` via ``atan2(sin, cos)``. + """ + return math.atan2(math.sin(angle), math.cos(angle)) + + +class PIDController: + """A scalar PID controller with anti-windup, deadband, and output clamp. + + The controller is stateful (it accumulates an integral and remembers the + previous error for the derivative term); call :meth:`reset` to clear that + state, e.g. when re-acquiring a target. + """ + + def __init__( + self, + kp: float, + ki: float = 0.0, + kd: float = 0.0, + output_limits: Tuple[Optional[float], Optional[float]] = (None, None), + integral_limit: Optional[float] = None, + deadband: float = 0.0, + ) -> None: + """Configure the gains and limits. + + Args: + kp: Proportional gain. + ki: Integral gain. + kd: Derivative gain. + output_limits: ``(min_output, max_output)``; use ``None`` on either + side to leave that bound unclamped. + integral_limit: Symmetric clamp on the accumulated integral + (anti-windup); ``None`` disables it. + deadband: When ``abs(error)`` is below this, the integral and + derivative terms are zeroed (the proportional term still acts). + """ + self.kp = kp + self.ki = ki + self.kd = kd + self.min_output, self.max_output = output_limits + self.integral_limit = integral_limit + self.deadband = deadband + self.integral: float = 0.0 + self.prev_error: float = 0.0 + + def update(self, error: float, dt: float) -> float: + """Advance the controller one step and return the control output. + + Args: + error: The current error (setpoint - measurement, or any signed + error convention the caller prefers). + dt: Timestep in seconds. Non-positive ``dt`` is guarded: the + derivative term is taken as zero and the integral is left + unchanged for that step. + + Returns: + The (optionally clamped) PID output. + """ + # Integrate (skip when dt is non-positive to avoid bogus accumulation). + if dt > 0: + self.integral += error * dt + if self.integral_limit is not None: + self.integral = max( + -self.integral_limit, min(self.integral, self.integral_limit) + ) + derivative = (error - self.prev_error) / dt + else: + derivative = 0.0 + + # Inside the deadband, suppress integral build-up and derivative kick. + if abs(error) < self.deadband: + self.integral = 0.0 + derivative = 0.0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + + # Saturate to the configured output limits. + if self.max_output is not None: + output = min(self.max_output, output) + if self.min_output is not None: + output = max(self.min_output, output) + + self.prev_error = error + return output + + def reset(self) -> None: + """Clear the integral accumulator and the remembered previous error.""" + self.integral = 0.0 + self.prev_error = 0.0 diff --git a/gowelcome/control/servoing.py b/gowelcome/control/servoing.py new file mode 100644 index 0000000..29641ca --- /dev/null +++ b/gowelcome/control/servoing.py @@ -0,0 +1,111 @@ +"""Bbox-only visual servoing: turn a person ``Detection`` into a velocity. + +The Go2 has no metric depth in this stack, so :class:`VisualServo` servos +purely on the detection bounding box: + +* **Yaw** is a P(ID) law on the *normalised horizontal offset* of the box + centroid (centre the target). +* **Forward** is a proportional law on how much shorter the box is than the + ``stop_height_ratio`` setpoint (a box that fills more of the frame is + closer), throttled down when the heading error is large so the dog turns to + face the person before charging. + +Velocity convention (matches the rest of GoWelcome / Unitree +``SportClient.Move``): ``vx`` forward+, ``vyaw`` CCW/left+ (rad/s). This module +only produces ``vx`` and ``vyaw``; lateral ``vy`` stays zero in APPROACH. + +Pure module: imports only :mod:`math`, :mod:`config`, and +:mod:`gowelcome.types` -- no cv2, no SDK -- so it imports and unit-tests +off-robot. +""" + +from __future__ import annotations + +import math +from typing import Tuple + +from config import ServoConfig +from gowelcome.types import Detection + +from gowelcome.control.pid import PIDController + + +class VisualServo: + """Pixel-offset visual servo producing ``(vx, vyaw, arrived)``. + + Holds a single yaw :class:`~gowelcome.control.pid.PIDController` built from + the :class:`~config.ServoConfig`; the forward channel is a stateless + proportional law. Call :meth:`reset` when re-acquiring a target so stale + integral/derivative state does not leak across approaches. + """ + + def __init__(self, cfg: ServoConfig) -> None: + """Build the yaw PID from ``cfg``. + + Args: + cfg: Servo tuning block (gains, deadband, sign, limits, stop size). + """ + self.cfg = cfg + # The PID also carries the deadband so that, on the tick a target + # crosses *into* the band, its derivative term is suppressed (the input + # is pre-zeroed below, so the PID sees error 0 -> inside its own + # deadband -> no derivative kick). The pre-zeroing of the input is what + # additionally suppresses the proportional term inside the band. + self.yaw_pid = PIDController( + kp=cfg.kp_yaw, + ki=cfg.ki_yaw, + kd=cfg.kd_yaw, + output_limits=(-cfg.max_yaw_rate, cfg.max_yaw_rate), + deadband=cfg.yaw_deadband, + ) + + def reset(self) -> None: + """Reset the internal yaw PID state (integral + previous error).""" + self.yaw_pid.reset() + + def compute( + self, target: Detection, frame_w: int, frame_h: int, dt: float + ) -> Tuple[float, float, bool]: + """Compute one servoing step toward ``target``. + + Pixel-offset P(ID) controller, bbox-only (no depth). + + Args: + target: The person bounding box to servo toward. + frame_w: Frame width in pixels (for the horizontal offset). + frame_h: Frame height in pixels (for the height-ratio distance + proxy). + dt: Timestep in seconds (fed to the yaw PID derivative/integral). + + Returns: + ``(vx, vyaw, arrived)`` where ``vx`` is forward velocity (m/s, + never negative in APPROACH), ``vyaw`` is yaw rate (rad/s, CCW+), + and ``arrived`` is ``True`` once the box fills at least + ``cfg.stop_height_ratio`` of the frame height. + """ + cfg = self.cfg + + # --- Yaw: centre the target horizontally. ------------------------- + norm_err = target.horizontal_offset(frame_w) # -1..1, + = right of centre + norm_err_eff = 0.0 if abs(norm_err) < cfg.yaw_deadband else norm_err + vyaw = cfg.yaw_sign * self.yaw_pid.update(norm_err_eff, dt) + + # --- Forward: approach until the box fills the frame vertically. --- + height_ratio = target.height_ratio(frame_h) + arrived = height_ratio >= cfg.stop_height_ratio + + if arrived: + vx = 0.0 + else: + # Box smaller than the stop size -> positive forward command. + forward_raw = cfg.kp_forward * (cfg.stop_height_ratio - height_ratio) + # Throttle forward when heading error is large (don't charge sideways). + scaling = math.exp(-cfg.forward_heading_falloff * abs(norm_err)) # 0..1 + vx = forward_raw * scaling + # Never reverse in APPROACH; clamp to the configured max. + vx = max(0.0, min(cfg.max_forward, vx)) + # Below min_forward, treat as stopped-forward (turning still allowed). + if vx < cfg.min_forward: + vx = 0.0 + + return (vx, vyaw, arrived) diff --git a/gowelcome/geo/__init__.py b/gowelcome/geo/__init__.py new file mode 100644 index 0000000..bce8695 --- /dev/null +++ b/gowelcome/geo/__init__.py @@ -0,0 +1,14 @@ +"""GPS + geofence subsystem for keeping GoWelcome inside its area.""" + +from gowelcome.geo.geofence import GeoFence, bearing_deg, haversine_m, normalize_deg +from gowelcome.geo.gps import GpsFix, GpsSource, build_gps_source + +__all__ = [ + "GeoFence", + "bearing_deg", + "haversine_m", + "normalize_deg", + "GpsFix", + "GpsSource", + "build_gps_source", +] diff --git a/gowelcome/geo/geofence.py b/gowelcome/geo/geofence.py new file mode 100644 index 0000000..fff8ad4 --- /dev/null +++ b/gowelcome/geo/geofence.py @@ -0,0 +1,126 @@ +"""Pure geofence geometry -- haversine distance + bearings + the keep-in fence. + +No I/O, no threads, no heavy deps (stdlib ``math`` only), so it unit-tests +off-robot. The :class:`GeoFence` holds a centre (lat/lon) and a radius, decides +when the dog has strayed too close to the edge, and computes the yaw needed to +home back toward the centre using GPS course-over-ground (no compass required). +""" + +from __future__ import annotations + +import math +from typing import Optional, Tuple + +from config import GeoFenceConfig + +_EARTH_RADIUS_M = 6_371_000.0 + + +def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """Great-circle distance between two lat/lon points, in metres.""" + p1 = math.radians(lat1) + p2 = math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlam = math.radians(lon2 - lon1) + a = ( + math.sin(dphi / 2.0) ** 2 + + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2 + ) + return 2.0 * _EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a))) + + +def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """Initial bearing from point 1 to point 2, degrees clockwise from north [0,360).""" + p1 = math.radians(lat1) + p2 = math.radians(lat2) + dlam = math.radians(lon2 - lon1) + y = math.sin(dlam) * math.cos(p2) + x = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dlam) + return (math.degrees(math.atan2(y, x)) + 360.0) % 360.0 + + +def normalize_deg(angle: float) -> float: + """Wrap an angle to ``[-180, 180)`` degrees.""" + return (angle + 180.0) % 360.0 - 180.0 + + +class GeoFence: + """A circular keep-in fence around a centre point. + + Distances use the haversine metric; the homing yaw is derived from the GPS + course-over-ground so no magnetometer/compass is needed. + """ + + def __init__(self, cfg: GeoFenceConfig) -> None: + self.cfg = cfg + self._center: Optional[Tuple[float, float]] = None + if cfg.center_mode == "fixed" and (cfg.center_lat or cfg.center_lon): + self._center = (cfg.center_lat, cfg.center_lon) + + # --- centre management ------------------------------------------------- + @property + def has_center(self) -> bool: + return self._center is not None + + @property + def center(self) -> Optional[Tuple[float, float]]: + return self._center + + def set_center(self, lat: float, lon: float) -> None: + """Explicitly (re)set the fence centre (e.g. dashboard 'set here').""" + self._center = (float(lat), float(lon)) + + def maybe_capture_center(self, lat: float, lon: float) -> None: + """In ``onstart`` mode, capture the first good fix as the centre.""" + if self._center is None and self.cfg.center_mode == "onstart": + self._center = (float(lat), float(lon)) + + # --- queries ----------------------------------------------------------- + def distance_from_center(self, lat: float, lon: float) -> Optional[float]: + if self._center is None: + return None + return haversine_m(self._center[0], self._center[1], lat, lon) + + def distance_to_edge(self, lat: float, lon: float) -> Optional[float]: + """Metres from the current position to the fence edge (negative = outside).""" + d = self.distance_from_center(lat, lon) + return None if d is None else (self.cfg.radius_m - d) + + def breached(self, lat: float, lon: float) -> bool: + """True once within ``margin_m`` of the edge (or beyond) -> start homing.""" + d = self.distance_from_center(lat, lon) + if d is None: + return False + return d >= (self.cfg.radius_m - self.cfg.margin_m) + + def cleared(self, lat: float, lon: float) -> bool: + """True once safely back inside (hysteresis) -> may resume normal behaviour.""" + d = self.distance_from_center(lat, lon) + if d is None: + return True + return d <= (self.cfg.radius_m - self.cfg.margin_m - self.cfg.release_m) + + def bearing_to_center(self, lat: float, lon: float) -> Optional[float]: + if self._center is None: + return None + return bearing_deg(lat, lon, self._center[0], self._center[1]) + + def homing_yaw(self, lat: float, lon: float, course_deg: Optional[float]) -> float: + """Yaw rate (rad/s) to steer toward the centre given current GPS course. + + ``vyaw = homing_kp * heading_error_rad``, clamped, where ``heading_error`` + is ``normalize(bearing_to_centre - course)``. ``+`` yaw = turn left/CCW. + When ``course_deg`` is unknown (too slow for a valid GPS track), returns + a gentle constant turn so the dog moves and re-acquires a course. + """ + target = self.bearing_to_center(lat, lon) + if target is None: + return 0.0 + if course_deg is None: + # No reliable course yet: turn gently (one direction) to gain a track. + return min(self.cfg.max_homing_yaw, 0.4) + # Compass bearings are clockwise-from-north; a positive (clockwise) error + # means "turn right" = negative yaw in the body convention, hence the sign. + err_deg = normalize_deg(target - course_deg) + vyaw = -math.radians(err_deg) * self.cfg.homing_kp + return max(-self.cfg.max_homing_yaw, min(self.cfg.max_homing_yaw, vyaw)) diff --git a/gowelcome/geo/gps.py b/gowelcome/geo/gps.py new file mode 100644 index 0000000..283a6c5 --- /dev/null +++ b/gowelcome/geo/gps.py @@ -0,0 +1,334 @@ +"""GPS sources for the geofence. + +The Go2 has no built-in GPS, so this reads an EXTERNAL receiver on the onboard +computer. Three sources, selected by ``cfg.gps.source``: + + * ``gpsd`` -- talk to a running ``gpsd`` over its JSON socket (no extra dep). + * ``serial`` -- parse NMEA (GGA/RMC) straight off a serial receiver (pyserial). + * ``mock`` -- a simulated receiver that integrates the commanded velocity into + a fake lat/lon, so the geofence + BOUNDARY behaviour can be + exercised entirely off-robot. + +Each source runs a background thread and exposes the most recent good fix via +:meth:`GpsSource.latest` (which returns ``None`` when the fix is stale). Quality +gating (min sats / max HDOP) is applied before a fix is published. +""" + +from __future__ import annotations + +import json +import logging +import math +import socket +import threading +import time +from abc import ABC, abstractmethod +from dataclasses import dataclass +from typing import Optional + +from config import GoWelcomeConfig, GpsConfig + +logger = logging.getLogger(__name__) + +_EARTH_RADIUS_M = 6_371_000.0 + + +@dataclass +class GpsFix: + """A single GPS reading. Angles in degrees, distances in metres/SI.""" + + lat: float + lon: float + ts: float # time.monotonic() when received + course_deg: Optional[float] = None # course over ground, CW from north + speed_mps: Optional[float] = None + num_sats: Optional[int] = None + hdop: Optional[float] = None + + +class GpsSource(ABC): + """Background GPS reader exposing the latest (non-stale) fix.""" + + @abstractmethod + def start(self) -> None: ... + + @abstractmethod + def stop(self) -> None: ... + + @abstractmethod + def latest(self) -> Optional[GpsFix]: + """Most recent good fix, or ``None`` if unavailable/stale.""" + + def on_command(self, vx: float, vy: float, vyaw: float) -> None: + """Hook so simulated sources can integrate the robot's motion. No-op + for real receivers.""" + + +class _ThreadGpsSource(GpsSource): + """Shared thread + quality-gating + staleness machinery for real sources.""" + + def __init__(self, cfg: GpsConfig) -> None: + self.cfg = cfg + self._fix: Optional[GpsFix] = None + self._lock = threading.Lock() + self._stop = threading.Event() + self._thread: Optional[threading.Thread] = None + + def start(self) -> None: + self._thread = threading.Thread(target=self._loop, name="GpsSource", daemon=True) + self._thread.start() + + def stop(self) -> None: + self._stop.set() + + def latest(self) -> Optional[GpsFix]: + with self._lock: + fix = self._fix + if fix is None: + return None + if (time.monotonic() - fix.ts) > self.cfg.stale_after: + return None + return fix + + def _publish(self, fix: GpsFix) -> None: + if fix.num_sats is not None and fix.num_sats < self.cfg.min_satellites: + return + if fix.hdop is not None and fix.hdop > self.cfg.max_hdop: + return + with self._lock: + self._fix = fix + + @abstractmethod + def _loop(self) -> None: ... + + +class GpsdSource(_ThreadGpsSource): + """Read fixes from a running ``gpsd`` over its JSON protocol (stdlib socket).""" + + def _loop(self) -> None: + while not self._stop.is_set(): + try: + with socket.create_connection( + (self.cfg.gpsd_host, self.cfg.gpsd_port), timeout=5.0 + ) as sock: + sock.sendall(b'?WATCH={"enable":true,"json":true}\n') + buf = b"" + sock.settimeout(2.0) + while not self._stop.is_set(): + try: + chunk = sock.recv(4096) + except socket.timeout: + continue + if not chunk: + break + buf += chunk + while b"\n" in buf: + line, buf = buf.split(b"\n", 1) + self._handle_line(line) + except Exception as exc: # pragma: no cover - hardware path + logger.debug("gpsd connection error: %s; retrying", exc) + self._stop.wait(2.0) + + def _handle_line(self, line: bytes) -> None: + try: + obj = json.loads(line.decode("ascii", "replace")) + except Exception: + return + if obj.get("class") != "TPV": + return + if obj.get("mode", 0) < 2: # need at least a 2D fix + return + lat, lon = obj.get("lat"), obj.get("lon") + if lat is None or lon is None: + return + self._publish(GpsFix( + lat=float(lat), lon=float(lon), ts=time.monotonic(), + course_deg=_opt_float(obj.get("track")), + speed_mps=_opt_float(obj.get("speed")), + num_sats=None, # gpsd reports sats in SKY reports; not gated here + hdop=_opt_float(obj.get("hdop")), + )) + + +class SerialNmeaSource(_ThreadGpsSource): + """Parse NMEA GGA/RMC off a serial GPS receiver (lazy pyserial).""" + + def _loop(self) -> None: + try: + import serial # type: ignore + except ImportError: # pragma: no cover - off-robot + logger.error("SerialNmeaSource needs pyserial: pip install pyserial") + return + while not self._stop.is_set(): + try: + with serial.Serial(self.cfg.serial_port, self.cfg.serial_baud, timeout=1.0) as ser: + last_course = None + last_speed = None + while not self._stop.is_set(): + raw = ser.readline().decode("ascii", "replace").strip() + if not raw or not raw.startswith("$"): + continue + kind = raw[3:6] + if kind == "RMC": + c, s = _parse_rmc(raw) + if c is not None: + last_course = c + if s is not None: + last_speed = s + elif kind == "GGA": + parsed = _parse_gga(raw) + if parsed is not None: + lat, lon, sats, hdop = parsed + self._publish(GpsFix( + lat=lat, lon=lon, ts=time.monotonic(), + course_deg=last_course, speed_mps=last_speed, + num_sats=sats, hdop=hdop, + )) + except Exception as exc: # pragma: no cover - hardware path + logger.debug("serial GPS error: %s; retrying", exc) + self._stop.wait(2.0) + + +class MockGpsSource(GpsSource): + """Simulated receiver: integrates the commanded velocity into a fake fix. + + Lets the geofence + BOUNDARY homing be demonstrated/tested with no hardware. + Heading is tracked CW-from-north so it matches a real GPS course-over-ground. + """ + + def __init__(self, cfg: GpsConfig, rate_hz: float = 10.0) -> None: + self.cfg = cfg + self._lat0 = cfg.mock_start_lat + self._lon0 = cfg.mock_start_lon + self._north_m = 0.0 + self._east_m = 0.0 + self._heading_cw = 0.0 # degrees, CW from north + self._vx = 0.0 + self._vyaw = 0.0 + self._period = 1.0 / max(1.0, rate_hz) + self._lock = threading.Lock() + self._stop = threading.Event() + self._thread: Optional[threading.Thread] = None + self._fix: Optional[GpsFix] = None + + def start(self) -> None: + self._thread = threading.Thread(target=self._loop, name="MockGps", daemon=True) + self._thread.start() + + def stop(self) -> None: + self._stop.set() + + def on_command(self, vx: float, vy: float, vyaw: float) -> None: + with self._lock: + self._vx = vx + self._vyaw = vyaw + + def latest(self) -> Optional[GpsFix]: + with self._lock: + return self._fix + + def _loop(self) -> None: + dt = self._period + while not self._stop.is_set(): + with self._lock: + vx, vyaw = self._vx, self._vyaw + # +vyaw is CCW (left); CW-from-north heading decreases on a left turn. + self._heading_cw = (self._heading_cw - math.degrees(vyaw) * dt) % 360.0 + h = math.radians(self._heading_cw) + self._north_m += vx * math.cos(h) * dt + self._east_m += vx * math.sin(h) * dt + lat = self._lat0 + math.degrees(self._north_m / _EARTH_RADIUS_M) + lon = self._lon0 + math.degrees( + self._east_m / (_EARTH_RADIUS_M * math.cos(math.radians(self._lat0))) + ) + speed = abs(vx) + fix = GpsFix( + lat=lat, lon=lon, ts=time.monotonic(), + course_deg=self._heading_cw if speed >= 0.05 else None, + speed_mps=speed, num_sats=12, hdop=0.8, + ) + with self._lock: + self._fix = fix + self._stop.wait(dt) + + +# --------------------------------------------------------------------------- # +# Helpers +# --------------------------------------------------------------------------- # +def _opt_float(v) -> Optional[float]: + try: + return float(v) + except (TypeError, ValueError): + return None + + +def _nmea_deg(value: str, hemi: str) -> Optional[float]: + """Convert an NMEA ddmm.mmmm + hemisphere field to signed decimal degrees.""" + if not value: + return None + try: + dot = value.index(".") + deg = int(value[: dot - 2]) + minutes = float(value[dot - 2 :]) + dec = deg + minutes / 60.0 + if hemi in ("S", "W"): + dec = -dec + return dec + except (ValueError, IndexError): + return None + + +def _parse_gga(sentence: str): + """Return ``(lat, lon, num_sats, hdop)`` from a GGA sentence, or ``None``.""" + sentence = sentence.split("*", 1)[0] # drop the trailing *checksum + f = sentence.split(",") + if len(f) < 10: + return None + if f[6] in ("", "0"): # fix quality 0 == no fix + return None + lat = _nmea_deg(f[2], f[3]) + lon = _nmea_deg(f[4], f[5]) + if lat is None or lon is None: + return None + sats = int(f[7]) if f[7].isdigit() else None + hdop = _opt_float(f[8]) + return lat, lon, sats, hdop + + +def _parse_rmc(sentence: str): + """Return ``(course_deg, speed_mps)`` from an RMC sentence (or ``None``s).""" + sentence = sentence.split("*", 1)[0] # drop the trailing *checksum + f = sentence.split(",") + if len(f) < 9 or f[2] != "A": # status A == valid + return None, None + speed = _opt_float(f[7]) # knots + course = _opt_float(f[8]) + return course, (speed * 0.514444 if speed is not None else None) + + +def build_gps_source(cfg: GoWelcomeConfig) -> Optional[GpsSource]: + """Construct the GPS source named by ``cfg.gps`` (or ``None`` if disabled). + + ``auto`` probes gpsd first, then falls back to a serial NMEA receiver. + """ + g = cfg.gps + if not g.enabled: + return None + src = (g.source or "auto").strip().lower() + + if src == "mock" or cfg.mock: + logger.info("GPS: using simulated MockGpsSource") + return MockGpsSource(g) + if src == "gpsd": + return GpsdSource(g) + if src == "serial": + return SerialNmeaSource(g) + + # auto: prefer gpsd if it answers, else serial. + try: + with socket.create_connection((g.gpsd_host, g.gpsd_port), timeout=0.5): + logger.info("GPS: auto-detected gpsd at %s:%d", g.gpsd_host, g.gpsd_port) + return GpsdSource(g) + except OSError: + logger.info("GPS: gpsd not found; using serial NMEA on %s", g.serial_port) + return SerialNmeaSource(g) diff --git a/gowelcome/perception/__init__.py b/gowelcome/perception/__init__.py new file mode 100644 index 0000000..d2a9e92 --- /dev/null +++ b/gowelcome/perception/__init__.py @@ -0,0 +1,15 @@ +"""Perception layer: YOLO person/vehicle detection + HSV road masking. + +This package owns everything that turns a raw BGR camera frame into a +:class:`gowelcome.types.PerceptionResult`. Heavy/optional dependencies +(``ultralytics``, ``cv2``) are imported lazily inside the classes below so this +package stays importable on machines without them (e.g. off-robot unit tests). +""" + +from __future__ import annotations + +from gowelcome.perception.detector import YoloDetector +from gowelcome.perception.road_mask import RoadDetector +from gowelcome.perception.vision_thread import PerceptionThread + +__all__ = ["YoloDetector", "RoadDetector", "PerceptionThread"] diff --git a/gowelcome/perception/detector.py b/gowelcome/perception/detector.py new file mode 100644 index 0000000..b732f01 --- /dev/null +++ b/gowelcome/perception/detector.py @@ -0,0 +1,116 @@ +"""YOLO object detector wrapper (Ultralytics). + +Thin adapter that runs an Ultralytics YOLO model on raw BGR frames and emits +:class:`gowelcome.types.Detection` boxes. Classification of those boxes into +*persons* vs *dangers* is intentionally **not** done here -- that is the +perception thread's job (it knows the per-class confidence/size gates). We just +return everything at or above the lowest confidence floor we care about. + +The ``ultralytics`` import is lazy (inside ``__init__``) so the package imports +fine on a machine without it; off-robot unit tests can still import this module. +""" + +from __future__ import annotations + +from typing import List + +from config import PerceptionConfig +from gowelcome.types import Detection + + +class YoloDetector: + """Run an Ultralytics YOLO model and return raw :class:`Detection` boxes.""" + + def __init__(self, cfg: PerceptionConfig) -> None: + """Load the YOLO model described by ``cfg``. + + Args: + cfg: Perception configuration (model path, device, conf floors, + tracking toggle, inference image size). + + Raises: + ImportError: if ``ultralytics`` is not installed, with a hint to + ``pip install ultralytics``. + """ + try: + from ultralytics import YOLO # lazy: heavy/optional dep + except ImportError as exc: # pragma: no cover - exercised off-robot + raise ImportError( + "ultralytics is required for YoloDetector. " + "Install it with: pip install ultralytics" + ) from exc + + self.cfg = cfg + # Lowest confidence we ever keep; the thread applies the stricter + # per-class gates (person_conf / danger_conf) afterwards. + self.conf_floor: float = min(cfg.person_conf, cfg.danger_conf) + + self.model = YOLO(cfg.model_path) + + # Move to the requested device when possible (CPU/cuda/cuda:0...). + if cfg.device: + try: + self.model.to(cfg.device) + except Exception: # pragma: no cover - device/driver dependent + pass + + # Optional FP16 inference (discrete GPU only). Guarded: a no-op or + # failure here must never break detection. + if cfg.use_half: + try: + self.model.model.half() + except Exception: # pragma: no cover - hardware dependent + pass + + # ``names`` may be a dict {id: label} or a list; both are handled in + # detect() via the ``get`` probe. + self.names = self.model.names + + def detect(self, frame) -> List[Detection]: + """Run inference on one BGR frame and return all kept detections. + + Args: + frame: ``HxWx3`` BGR ``uint8`` numpy array. ``None`` or empty + frames yield an empty list. + + Returns: + Every detection with confidence ``>= conf_floor``, as + :class:`Detection` objects (pixel coords, optional track id). + """ + if frame is None or getattr(frame, "size", 0) == 0: + return [] + + cfg = self.cfg + if cfg.use_tracking: + results = self.model.track( + frame, + persist=True, + conf=self.conf_floor, + tracker=cfg.tracker, + verbose=False, + imgsz=cfg.infer_imgsz, + )[0] + else: + results = self.model( + frame, + conf=self.conf_floor, + verbose=False, + imgsz=cfg.infer_imgsz, + )[0] + + names = self.names + detections: List[Detection] = [] + for box in results.boxes: + cls_id = int(box.cls[0]) + conf = float(box.conf[0]) + x1, y1, x2, y2 = map(int, box.xyxy[0]) + tid = int(box.id[0]) if getattr(box, "id", None) is not None else -1 + label = ( + names[cls_id] + if not hasattr(names, "get") + else names.get(cls_id, str(cls_id)) + ) + detections.append( + Detection(label, conf, x1, y1, x2, y2, track_id=tid) + ) + return detections diff --git a/gowelcome/perception/road_mask.py b/gowelcome/perception/road_mask.py new file mode 100644 index 0000000..7b053c4 --- /dev/null +++ b/gowelcome/perception/road_mask.py @@ -0,0 +1,91 @@ +"""HSV asphalt/road mask over the bottom crop of the frame. + +Backyards border roads and driveways. We don't have a map, so we cheaply flag +"road ahead" by thresholding low-saturation grey (asphalt/concrete) in the +bottom strip of the camera frame and reporting how much of the left/centre/right +thirds is covered. The control layer uses :class:`RoadInfo.clearer_side` to +steer away from the road. + +``cv2`` is imported lazily so the package imports without OpenCV; when +``road_enabled`` is False we skip OpenCV entirely and return an empty result. +""" + +from __future__ import annotations + +import numpy as np + +from config import PerceptionConfig +from gowelcome.types import RoadInfo + + +class RoadDetector: + """Threshold the bottom crop of a frame for road-coloured pixels.""" + + def __init__(self, cfg: PerceptionConfig) -> None: + """Prepare the road detector. + + When ``cfg.road_enabled`` is False this is a no-op shell: ``analyze`` + returns an empty :class:`RoadInfo` and OpenCV is never imported. + + Args: + cfg: Perception configuration (crop fraction, HSV bounds, kernel). + + Raises: + ImportError: if road detection is enabled but ``cv2`` is missing, + with a hint to ``pip install opencv-python``. + """ + self.cfg = cfg + self._cv2 = None + if cfg.road_enabled: + try: + import cv2 # lazy: heavy/optional dep + except ImportError as exc: # pragma: no cover - exercised off-robot + raise ImportError( + "opencv-python (cv2) is required for RoadDetector when " + "road_enabled is True. Install it with: " + "pip install opencv-python" + ) from exc + self._cv2 = cv2 + + def analyze(self, frame) -> RoadInfo: + """Estimate road coverage in the bottom crop of ``frame``. + + Args: + frame: ``HxWx3`` BGR ``uint8`` numpy array. + + Returns: + A :class:`RoadInfo` with overall coverage and per-third coverage + (each in ``[0, 1]``) plus the binary crop mask. If road detection + is disabled, returns ``RoadInfo(0, 0, 0, 0, None)``. + """ + cfg = self.cfg + if not cfg.road_enabled or self._cv2 is None: + return RoadInfo(0.0, 0.0, 0.0, 0.0, None) + + cv2 = self._cv2 + h, w = frame.shape[:2] + # Bottom crop: keep the lowest ``road_crop_frac`` of the frame. + crop = frame[int(h * (1.0 - cfg.road_crop_frac)):, :] + + hsv = cv2.cvtColor(crop, cv2.COLOR_BGR2HSV) + mask = cv2.inRange( + hsv, + np.array(cfg.road_hsv_lower), + np.array(cfg.road_hsv_upper), + ) + + # Morphological opening to de-speckle the binary mask. + kernel = np.ones((cfg.road_morph_kernel, cfg.road_morph_kernel), np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + + # Mean over a {0,255} mask -> fraction of road pixels once /255. + coverage = float(mask.mean()) / 255.0 + + # Split the crop width into equal thirds; per-third coverage. + cw = mask.shape[1] + third = max(1, cw // 3) + left = float(mask[:, :third].mean()) / 255.0 + center = float(mask[:, third:2 * third].mean()) / 255.0 + right = float(mask[:, 2 * third:].mean()) / 255.0 + + return RoadInfo(coverage, left, center, right, mask=mask) diff --git a/gowelcome/perception/vision_thread.py b/gowelcome/perception/vision_thread.py new file mode 100644 index 0000000..2cf7a36 --- /dev/null +++ b/gowelcome/perception/vision_thread.py @@ -0,0 +1,134 @@ +"""Background perception thread. + +Owns the camera->YOLO->road-mask pipeline on its own daemon thread so the main +control loop never blocks on inference. Each frame it publishes an immutable +:class:`gowelcome.types.PerceptionResult` snapshot that the control loop reads +via :meth:`PerceptionThread.latest`. + +Heavy/optional deps (``ultralytics``, ``cv2``) are only ever imported by the +:class:`YoloDetector` / :class:`RoadDetector` helpers (lazily), never here at +module top-level. +""" + +from __future__ import annotations + +import logging +import threading +import time +from typing import Optional + +from config import GoWelcomeConfig +from gowelcome.perception.detector import YoloDetector +from gowelcome.perception.road_mask import RoadDetector +from gowelcome.robot.interface import RobotInterface +from gowelcome.types import PerceptionResult, RoadInfo + +_LOG = logging.getLogger(__name__) + +# How long to nap when no frame is available yet (seconds). +_NO_FRAME_SLEEP = 0.02 + + +class PerceptionThread(threading.Thread): + """Run perception in the background and publish the latest result.""" + + def __init__(self, robot: RobotInterface, cfg: GoWelcomeConfig) -> None: + """Build the detector/road-mask pipeline and prepare the thread. + + Args: + robot: Robot interface to pull frames from (``get_frame``). + cfg: Full GoWelcome configuration; ``cfg.perception`` configures + the detectors and ``cfg.camera.target_fps`` paces the loop. + """ + super().__init__(daemon=True, name="PerceptionThread") + self.robot = robot + self.cfg = cfg + + self.detector = YoloDetector(cfg.perception) + self.road_detector = RoadDetector(cfg.perception) + + self._stop_event = threading.Event() + self._lock = threading.Lock() + self._latest: Optional[PerceptionResult] = None + self._seq = 0 + + fps = max(1e-3, cfg.camera.target_fps) + self._period = 1.0 / fps + + def run(self) -> None: + """Main loop: grab a frame, detect + analyse, publish a result. + + Runs until :meth:`stop` is called. The whole per-iteration body is + guarded so a single bad frame logs and continues rather than killing + the thread. The loop is throttled to roughly ``camera.target_fps``. + """ + pcfg = self.cfg.perception + while not self._stop_event.is_set(): + start = time.monotonic() + try: + frame = self.robot.get_frame() + if frame is None: + # No frame yet: nap briefly so we don't busy-spin. + time.sleep(_NO_FRAME_SLEEP) + continue + + dets = self.detector.detect(frame) + h, w = frame.shape[:2] + + persons = [ + d + for d in dets + if d.label in pcfg.person_classes + and d.conf >= pcfg.person_conf + ] + dangers = [ + d + for d in dets + if d.label in pcfg.danger_classes + and d.conf >= pcfg.danger_conf + and d.height_ratio(h) >= pcfg.danger_min_height_ratio + ] + + if pcfg.road_enabled: + road = self.road_detector.analyze(frame) + else: + road = RoadInfo(0.0, 0.0, 0.0, 0.0, None) + + with self._lock: + self._seq += 1 + seq = self._seq + result = PerceptionResult( + frame_w=w, + frame_h=h, + detections=dets, + persons=persons, + dangers=dangers, + road=road, + ts=time.monotonic(), + seq=seq, + frame=frame, + ) + with self._lock: + self._latest = result + except Exception: # noqa: BLE001 - one bad frame must not kill us + _LOG.exception("PerceptionThread iteration failed; continuing") + + # Throttle to roughly target_fps: sleep the remainder of the period. + elapsed = time.monotonic() - start + remaining = self._period - elapsed + if remaining > 0: + # Wait on the stop event so stop() wakes us promptly. + self._stop_event.wait(remaining) + + def stop(self) -> None: + """Signal the run loop to exit (thread-safe; idempotent).""" + self._stop_event.set() + + def latest(self) -> Optional[PerceptionResult]: + """Return the most recent :class:`PerceptionResult`, or ``None``. + + Thread-safe; the returned snapshot is the object published by the last + completed iteration. + """ + with self._lock: + return self._latest diff --git a/gowelcome/robot/__init__.py b/gowelcome/robot/__init__.py new file mode 100644 index 0000000..4e19386 --- /dev/null +++ b/gowelcome/robot/__init__.py @@ -0,0 +1 @@ +"""Robot abstraction layer: hardware-agnostic interface + concrete backends.""" diff --git a/gowelcome/robot/audio.py b/gowelcome/robot/audio.py new file mode 100644 index 0000000..9c9c54c --- /dev/null +++ b/gowelcome/robot/audio.py @@ -0,0 +1,504 @@ +"""Pluggable greeting-audio backends. + +The Go2 has **no** first-party SDK audio-playback path. The Unitree Python +SDK's :class:`AudioClient` targets the G1 ``voice`` service (TTS / PCM stream +play); on the Go2 only :class:`VuiClient` exists (volume / brightness control). +There is no documented, SDK-exposed way to play an arbitrary WAV through the +Go2's onboard speaker. The likely real path is the DDS topic +``rt/api/audiohub/request`` -- but that is **not** wrapped by the SDK, so we do +not implement it here. + +Consequently greeting audio is *pluggable*: + +* :class:`NullAudio` -- logs only (mock / CI). +* :class:`HostSpeakerAudio` -- plays on the machine running GoWelcome (laptop / + Jetson). Reliable, model-independent. The DEFAULT. +* :class:`Go2AudioHubAudio` -- EXPERIMENTAL / UNVERIFIED attempt to stream PCM + to the robot via the G1 ``AudioClient`` (almost certainly absent on Go2 + firmware -- it degrades gracefully to a logged no-op). + +Pick one with :func:`build_audio_backend`. + +All heavy/optional imports (``simpleaudio``, the Unitree SDK) are performed +lazily inside methods so this module imports cleanly off-robot. +""" + +from __future__ import annotations + +import logging +import struct +import subprocess +import threading +import time +from pathlib import Path +from typing import List, Optional, Tuple + +from config import GoWelcomeConfig +from gowelcome.robot.interface import AudioBackend + +logger = logging.getLogger(__name__) + + +# --------------------------------------------------------------------------- +# Vendored WAV/PCM helpers (copied verbatim-in-spirit from the Unitree SDK +# example ``example/g1/audio/wav.py``). Used only by Go2AudioHubAudio. Logging +# replaces the original ``print`` calls. +# --------------------------------------------------------------------------- + +def read_wav(filename: str) -> Tuple[List[int], int, int, bool]: + """Parse a PCM WAV file into a flat list of raw bytes. + + Vendored from the Unitree SDK ``g1/audio`` example. Only 16-bit PCM is + supported (the requirement for the onboard audio stream). + + Returns: + ``(raw_pcm_bytes, sample_rate, num_channels, ok)``. On any error + returns ``([], -1, -1, False)`` instead of raising. + """ + try: + with open(filename, "rb") as f: + def read(fmt: str): + return struct.unpack(fmt, f.read(struct.calcsize(fmt))) + + # === Chunk Header === + chunk_id, = read(" bool: + """Stream 16-bit little-endian PCM to a client with a ``PlayStream`` method. + + Vendored from the Unitree SDK ``g1/audio`` example. Sends the PCM in + ``chunk_size``-byte chunks (default 96000 = ~3 s at 16 kHz mono), pausing + ``sleep_time`` seconds between chunks. + + Returns: + ``True`` if every chunk was accepted (return code 0), else ``False``. + """ + pcm_data = bytes(pcm_list) + stream_id = str(int(time.time() * 1000)) # unique id from current time + offset = 0 + chunk_index = 0 + total_size = len(pcm_data) + + while offset < total_size: + remaining = total_size - offset + current_chunk_size = min(chunk_size, remaining) + chunk = pcm_data[offset:offset + current_chunk_size] + + if verbose: + preview = " ".join( + str(struct.unpack_from(" bool: + """Log the intended playback and return ``True`` (always 'succeeds').""" + logger.info("[NULL-AUDIO] would play %s (blocking=%s)", wav_path, blocking) + return True + + +class HostSpeakerAudio(AudioBackend): + """Play the greeting on the *host* running GoWelcome (laptop / Jetson). + + Prefers ``simpleaudio`` (pure-Python, no external process). If that import + fails it falls back to running ``cfg.audio.host_player_cmd`` + the WAV path + as a subprocess (e.g. ``aplay -q ``). Never raises on a missing file or + backend -- it logs and returns ``False``. + """ + + def __init__(self, cfg: GoWelcomeConfig) -> None: + self._cfg = cfg + self._player_cmd: List[str] = list(cfg.audio.host_player_cmd) + # Optional explicit PulseAudio sink to pin a specific speaker (e.g. a + # USB/BT speaker on the dog). When set we MUST go through paplay -- + # neither simpleaudio nor aplay targets a Pulse sink by name. + self._output_device: str = (cfg.audio.output_device or "").strip() + # Live handle to the in-flight playback (simpleaudio PlayObject or the + # subprocess.Popen), so close()/non-blocking calls can manage it. + self._sa_play = None + self._proc: Optional[subprocess.Popen] = None + # Resolve simpleaudio availability once, lazily. Skipped entirely when a + # specific output device is pinned (simpleaudio can't target a sink). + self._sa = None + if self._output_device: + logger.info( + "HostSpeakerAudio: pinning output to PulseAudio sink %r " + "(via paplay)", self._output_device, + ) + else: + try: + import simpleaudio # type: ignore + self._sa = simpleaudio + logger.debug("HostSpeakerAudio: using simpleaudio") + except ImportError: + logger.info( + "HostSpeakerAudio: simpleaudio not available " + "(pip install simpleaudio) -- falling back to subprocess %s", + self._player_cmd, + ) + + def play(self, wav_path: str, blocking: bool = False) -> bool: + """Play ``wav_path`` on the host speaker; ``True`` if dispatched.""" + if not Path(wav_path).is_file(): + logger.warning("HostSpeakerAudio: wav not found: %s", wav_path) + return False + + if self._output_device: + return self._play_subprocess(wav_path, blocking) + if self._sa is not None: + return self._play_simpleaudio(wav_path, blocking) + return self._play_subprocess(wav_path, blocking) + + def _play_simpleaudio(self, wav_path: str, blocking: bool) -> bool: + """Play via simpleaudio; honour ``blocking`` with ``wait_done()``.""" + try: + wave_obj = self._sa.WaveObject.from_wave_file(wav_path) + self._sa_play = wave_obj.play() + logger.info("HostSpeakerAudio: playing %s (blocking=%s)", wav_path, blocking) + if blocking: + self._sa_play.wait_done() + self._sa_play = None + return True + except Exception as exc: # noqa: BLE001 -- never raise out of audio + logger.warning("HostSpeakerAudio: simpleaudio playback failed: %s", exc) + return False + + def _play_subprocess(self, wav_path: str, blocking: bool) -> bool: + """Play via an external player. + + When an explicit ``output_device`` sink is pinned, use + ``paplay --device= `` (the only path that targets a specific + PulseAudio sink -- mirrors the team's Sanad G1 stack). Otherwise spawn + the configured ``host_player_cmd`` (default ``aplay -q``). + """ + if self._output_device: + cmd = ["paplay", f"--device={self._output_device}", wav_path] + elif self._player_cmd: + cmd = self._player_cmd + [wav_path] + else: + logger.warning("HostSpeakerAudio: empty host_player_cmd; cannot play") + return False + try: + self._proc = subprocess.Popen( + cmd, + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + ) + logger.info( + "HostSpeakerAudio: spawned %s (blocking=%s)", cmd, blocking, + ) + if blocking: + _, stderr = self._proc.communicate() + rc = self._proc.returncode + self._proc = None + if rc != 0: + logger.warning( + "HostSpeakerAudio: player exited %s: %s", + rc, (stderr or b"").decode(errors="replace").strip(), + ) + return False + return True + except FileNotFoundError: + logger.warning( + "HostSpeakerAudio: player not found: %r " + "(install it or set cfg.audio.host_player_cmd)", cmd[0], + ) + return False + except Exception as exc: # noqa: BLE001 -- never raise out of audio + logger.warning("HostSpeakerAudio: subprocess playback failed: %s", exc) + return False + + def close(self) -> None: + """Stop any in-flight playback and release resources.""" + if self._sa_play is not None: + try: + self._sa_play.stop() + except Exception: # noqa: BLE001 + pass + self._sa_play = None + if self._proc is not None and self._proc.poll() is None: + try: + self._proc.terminate() + except Exception: # noqa: BLE001 + pass + self._proc = None + + +class Go2AudioHubAudio(AudioBackend): + """EXPERIMENTAL / UNVERIFIED onboard-speaker backend for the Go2. + + .. warning:: + This is **not verified to work on Go2 firmware**. The Unitree Python + SDK exposes :class:`AudioClient` only for the **G1** ``voice`` service; + the Go2 ships :class:`VuiClient` (volume / brightness) and has no + documented SDK path to play an arbitrary clip. The real path is almost + certainly the DDS topic ``rt/api/audiohub/request``, which the SDK does + **not** wrap -- so it is *not* implemented here. + + This class optimistically tries the G1 :class:`AudioClient` (which + chunks 16 kHz mono 16-bit PCM via ``PlayStream``). On a Go2 the service + Init will typically fail; we log a clear "onboard audio + unsupported/unverified -- falling back" message and return ``False``. + Always test on hardware before relying on it. + + Requires the WAV to be **16 kHz mono 16-bit PCM**. + """ + + def __init__(self, cfg: GoWelcomeConfig) -> None: + self._cfg = cfg + self._client = None + self._init_ok = False + self._play_thread: Optional[threading.Thread] = None + try: + from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient # type: ignore + except ImportError as exc: + logger.warning( + "Go2AudioHubAudio: unitree_sdk2py not available (%s) -- " + "install the Unitree Python SDK. Onboard audio disabled.", exc, + ) + return + except Exception as exc: # noqa: BLE001 + logger.warning( + "Go2AudioHubAudio: failed to import AudioClient (%s); " + "onboard audio unsupported/unverified.", exc, + ) + return + + try: + client = AudioClient() + # The DDS channel factory must already be initialised by the robot + # backend (Go2Robot.__init__) before this is used on hardware. + client.SetTimeout(10.0) + client.Init() + self._client = client + self._init_ok = True + # Best-effort volume; ignored if the firmware lacks the API. + try: + self._client.SetVolume(int(self._cfg.greet.audio_volume)) + except Exception as exc: # noqa: BLE001 + logger.debug("Go2AudioHubAudio: SetVolume not supported: %s", exc) + logger.info("Go2AudioHubAudio: AudioClient init OK (UNVERIFIED on Go2)") + except Exception as exc: # noqa: BLE001 + logger.warning( + "Go2AudioHubAudio: AudioClient Init failed (%s) -- Go2 onboard " + "audio unsupported/unverified; falling back.", exc, + ) + self._client = None + self._init_ok = False + + def play(self, wav_path: str, blocking: bool = False) -> bool: + """Stream the WAV to the robot speaker. ``True`` only if it succeeded. + + ``blocking`` is effectively always honoured: :func:`play_pcm_stream` + sends synchronously (sleeping between chunks), so playback completes + before this returns regardless of the flag. + """ + if not self._init_ok or self._client is None: + logger.warning( + "Go2AudioHubAudio: client not initialised -- Go2 onboard audio " + "unsupported/unverified; falling back. (path=%s)", wav_path, + ) + return False + + if not Path(wav_path).is_file(): + logger.warning("Go2AudioHubAudio: wav not found: %s", wav_path) + return False + + pcm, sample_rate, num_channels, ok = read_wav(wav_path) + if not ok: + logger.warning("Go2AudioHubAudio: failed to parse wav: %s", wav_path) + return False + if sample_rate != 16000 or num_channels != 1: + logger.warning( + "Go2AudioHubAudio: expected 16kHz mono 16-bit PCM, got " + "%sHz / %s channel(s): %s", + sample_rate, num_channels, wav_path, + ) + return False + + if blocking: + return self._stream(pcm) + + # Non-blocking: play_greeting() is called from the state machine's + # step() on the control-loop thread, which must NEVER block (it gates + # the perception-staleness safety stop). play_pcm_stream sleeps between + # chunks and makes synchronous PlayStream RPCs, so run it on a daemon + # worker thread instead and return immediately. + if self._play_thread is not None and self._play_thread.is_alive(): + logger.debug("Go2AudioHubAudio: a clip is already playing; skipping") + return True + self._play_thread = threading.Thread( + target=self._stream, args=(pcm,), name="Go2AudioStream", daemon=True, + ) + self._play_thread.start() + return True + + def _stream(self, pcm: List[int]) -> bool: + """Stream PCM to the robot synchronously (on a worker thread when called + non-blocking). Never raises.""" + try: + ok = play_pcm_stream(self._client, pcm, stream_name="gowelcome") + if not ok: + logger.warning( + "Go2AudioHubAudio: PlayStream failed -- Go2 onboard audio " + "unsupported/unverified; falling back.", + ) + return ok + except Exception as exc: # noqa: BLE001 -- never raise out of audio + logger.warning( + "Go2AudioHubAudio: playback error (%s) -- Go2 onboard audio " + "unsupported/unverified; falling back.", exc, + ) + return False + + def close(self) -> None: + """Best-effort stop of any in-flight stream.""" + if self._client is not None: + try: + self._client.PlayStop("gowelcome") + except Exception: # noqa: BLE001 + pass + + +def build_audio_backend(cfg: GoWelcomeConfig) -> AudioBackend: + """Construct the audio backend named by ``cfg.audio.backend``. + + Recognised values: + * ``"host"`` -> :class:`HostSpeakerAudio` (default for unrecognised-but- + host-like intent). + * ``"go2"`` -> :class:`Go2AudioHubAudio` (experimental). + * ``"null"`` -> :class:`NullAudio`. + + Anything else logs a warning and falls back to :class:`NullAudio` (silent, + safe). + """ + backend = (cfg.audio.backend or "").strip().lower() + if backend == "host": + return HostSpeakerAudio(cfg) + if backend == "go2": + return Go2AudioHubAudio(cfg) + if backend == "null": + return NullAudio() + logger.warning( + "build_audio_backend: unknown audio backend %r -- using NullAudio " + "(no sound). Valid: 'host', 'go2', 'null'.", cfg.audio.backend, + ) + return NullAudio() diff --git a/gowelcome/robot/factory.py b/gowelcome/robot/factory.py new file mode 100644 index 0000000..661ecf7 --- /dev/null +++ b/gowelcome/robot/factory.py @@ -0,0 +1,59 @@ +"""Robot backend factory. + +Selects and constructs the concrete :class:`RobotInterface` implementation +based on the runtime config. The chosen backend is *lazy-imported* so the +unused backend's heavy dependencies are never pulled in: + + * ``cfg.mock`` is ``True`` -> :class:`~gowelcome.robot.mock_robot.MockRobot` + (webcam / video file; no robot SDK required). + * ``cfg.transport == "webrtc"`` -> :class:`~gowelcome.robot.webrtc_robot.Go2WebRTCRobot` + (default; app WebRTC protocol via ``unitree_webrtc_connect``; wifi + audio). + * ``cfg.transport == "dds"`` -> :class:`~gowelcome.robot.go2_robot.Go2Robot` + (official ``unitree_sdk2py`` over CycloneDDS; wired / EDU). +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from gowelcome.robot.interface import RobotInterface + +if TYPE_CHECKING: # pragma: no cover - type hints only + from config import GoWelcomeConfig + + +def build_robot(cfg: "GoWelcomeConfig") -> RobotInterface: + """Build the appropriate robot backend for ``cfg``. + + ``cfg.mock`` -> MockRobot; else ``cfg.transport`` selects the real backend + (``"webrtc"`` default, ``"dds"`` for the official SDK). The unselected + backends are never imported, so their heavy/optional dependencies need not + be installed. + + Args: + cfg: The fully-populated :class:`~config.GoWelcomeConfig`. + + Returns: + A ready-to-use :class:`RobotInterface` instance. + + Raises: + ValueError: if ``cfg.transport`` is not a known value. + """ + if cfg.mock: + from gowelcome.robot.mock_robot import MockRobot + + return MockRobot(cfg) + + transport = (cfg.transport or "webrtc").strip().lower() + if transport == "webrtc": + from gowelcome.robot.webrtc_robot import Go2WebRTCRobot + + return Go2WebRTCRobot(cfg) + if transport == "dds": + from gowelcome.robot.go2_robot import Go2Robot + + return Go2Robot(cfg) + + raise ValueError( + f"Unknown transport {cfg.transport!r} (expected 'webrtc' or 'dds')." + ) diff --git a/gowelcome/robot/go2_robot.py b/gowelcome/robot/go2_robot.py new file mode 100644 index 0000000..ac0f0fa --- /dev/null +++ b/gowelcome/robot/go2_robot.py @@ -0,0 +1,440 @@ +"""Concrete :class:`RobotInterface` backed by the official Unitree Go2 SDK. + +This module talks to a physical Unitree Go2 over CycloneDDS using the +``unitree_sdk2py`` package. All heavy/optional dependencies (``unitree_sdk2py``, +``cv2``) are imported *lazily* inside :class:`Go2Robot.__init__`, so importing +this module never fails on an off-robot machine that lacks the SDK -- only +*instantiating* :class:`Go2Robot` requires it. + +Velocity convention (matches ``SportClient.Move``): + vx forward (+) / backward (-) m/s, body frame + vy left (+) / right (-) m/s, body frame + vyaw turn-left/CCW (+) / right/CW (-) rad/s + +Driving is routed through one of two paths: + * ``ObstaclesAvoidClient.Move`` -- the firmware LiDAR avoidance layer filters + and hard-stops these commands (the *safe* path), used when + ``cfg.safety.use_lidar_avoidance`` is enabled. + * ``SportClient.Move`` -- the raw locomotion path, used otherwise. + +A background daemon thread continuously pulls + decodes camera frames so that +:meth:`get_frame` is non-blocking and decoupled from the control rate. +""" + +from __future__ import annotations + +import logging +import threading +import time +from typing import TYPE_CHECKING, Callable, Dict, Optional, Tuple + +import numpy as np + +from gowelcome.robot.interface import GESTURES, RobotInterface + +if TYPE_CHECKING: # pragma: no cover - type hints only + from config import GoWelcomeConfig + +logger = logging.getLogger(__name__) + + +class Go2Robot(RobotInterface): + """Drive a physical Unitree Go2 via the official ``unitree_sdk2py`` SDK. + + Args: + cfg: The fully-populated :class:`~config.GoWelcomeConfig`. + + Raises: + ImportError: If ``unitree_sdk2py`` (or ``cv2``) is not installed. + """ + + def __init__(self, cfg: "GoWelcomeConfig") -> None: + # --- Lazy heavy imports (kept out of module scope on purpose) -------- + try: + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ( + ObstaclesAvoidClient, + ) + from unitree_sdk2py.go2.sport.sport_client import SportClient + from unitree_sdk2py.go2.video.video_client import VideoClient + except ImportError as exc: # pragma: no cover - off-robot path + raise ImportError( + "Go2Robot requires the official Unitree SDK. Install it with: " + "pip install unitree_sdk2py (or build from " + "github.com/unitreerobotics/unitree_sdk2_python)." + ) from exc + + try: + import cv2 # noqa: F401 (imported for an early, clear failure) + except ImportError as exc: # pragma: no cover - off-robot path + raise ImportError( + "Go2Robot requires OpenCV to decode camera frames. Install it " + "with: pip install opencv-python" + ) from exc + + self.cfg = cfg + self._cv2 = cv2 # cache the module for the camera thread + + # --- DDS channel: initialise EXACTLY ONCE before any client --------- + ChannelFactoryInitialize(cfg.network.domain_id, cfg.network.interface) + + # --- Sport (locomotion / posture / gestures) ------------------------ + self._sport = SportClient() + self._sport.SetTimeout(10.0) + self._sport.Init() + + # --- Video (front camera) ------------------------------------------- + self._video = VideoClient() + self._video.SetTimeout(3.0) + self._video.Init() + + # --- Obstacle avoidance (firmware LiDAR hard-stop layer) ------------ + self._oa = ObstaclesAvoidClient() + self._oa.SetTimeout(3.0) + self._oa.Init() + + # --- Vui (Go2 on-board volume only) --------------------------------- + self._vui = None + try: + from unitree_sdk2py.go2.vui.vui_client import VuiClient + + vui = VuiClient() + vui.SetTimeout(3.0) + vui.Init() + self._vui = vui + except Exception as exc: # pragma: no cover - firmware-dependent + logger.warning("VuiClient unavailable (volume control disabled): %s", exc) + + # --- Audio backend (greeting playback) ------------------------------ + from gowelcome.robot.audio import build_audio_backend + + self.audio = build_audio_backend(cfg) + self._greet_volume_set = False + + # --- Gesture name -> SportClient method map ------------------------- + self._gestures: Dict[str, Callable[[], int]] = { + "hello": self._sport.Hello, + "heart": self._sport.Heart, + "stretch": self._sport.Stretch, + "dance1": self._sport.Dance1, + "dance2": self._sport.Dance2, + "scrape": self._sport.Scrape, + "content": self._sport.Content, + "sit": self._sport.Sit, + "rise_sit": self._sport.RiseSit, + } + + # --- Camera frame buffer (written by the camera thread) ------------- + self._frame: Optional[np.ndarray] = None + self._frame_size: Tuple[int, int] = (0, 0) + self._frame_lock = threading.Lock() + + # --- Avoidance routing state ---------------------------------------- + self._avoidance_on = False + if cfg.safety.use_lidar_avoidance: + self.set_avoidance(True) + + # --- Shutdown guard (idempotent) ------------------------------------ + self._shutdown = False + self._shutdown_lock = threading.Lock() + + # --- Start the camera thread last, once everything is wired up ------ + self._cam_stop = threading.Event() + self._cam_thread = threading.Thread( + target=self._camera_loop, name="Go2CameraThread", daemon=True + ) + self._cam_thread.start() + + logger.info( + "Go2Robot ready (iface=%s domain=%d avoidance=%s dry_run=%s)", + cfg.network.interface, + cfg.network.domain_id, + self._avoidance_on, + cfg.dry_run, + ) + + # ------------------------------------------------------------------ # + # Camera thread # + # ------------------------------------------------------------------ # + def _camera_loop(self) -> None: + """Continuously pull + decode camera frames into ``self._frame``. + + Runs on a daemon thread; failures are logged and retried so a transient + decode error never kills the stream. Throttled to ~camera target_fps + when frames are unavailable to avoid a busy-spin. + """ + cv2 = self._cv2 + min_period = 1.0 / max(1.0, self.cfg.camera.target_fps) + while not self._cam_stop.is_set(): + start = time.monotonic() + try: + code, data = self._video.GetImageSample() + except Exception as exc: # pragma: no cover - hardware path + logger.debug("GetImageSample raised: %s", exc) + self._cam_stop.wait(min_period) + continue + + if code != 0 or not data: + logger.debug("GetImageSample non-zero code=%s", code) + self._cam_stop.wait(min_period) + continue + + try: + buf = np.frombuffer(bytes(data), dtype=np.uint8) + image = cv2.imdecode(buf, cv2.IMREAD_COLOR) + except Exception as exc: # pragma: no cover - hardware path + logger.debug("Frame decode failed: %s", exc) + self._cam_stop.wait(min_period) + continue + + if image is None: + self._cam_stop.wait(min_period) + continue + + h, w = image.shape[:2] + with self._frame_lock: + self._frame = image + self._frame_size = (int(w), int(h)) + + # Gentle pace so we don't outrun the camera / burn CPU. + elapsed = time.monotonic() - start + if elapsed < min_period: + self._cam_stop.wait(min_period - elapsed) + + # ------------------------------------------------------------------ # + # Perception input # + # ------------------------------------------------------------------ # + def get_frame(self) -> "Optional[np.ndarray]": + """Return the latest BGR camera frame, or ``None`` if none yet.""" + with self._frame_lock: + return self._frame + + def frame_size(self) -> "tuple[int, int]": + """Current ``(width, height)``; ``(0, 0)`` until the first frame.""" + with self._frame_lock: + return self._frame_size + + # ------------------------------------------------------------------ # + # Locomotion # + # ------------------------------------------------------------------ # + def drive(self, vx: float, vy: float, vyaw: float) -> None: + """Command a body-frame velocity for one tick. + + The :class:`~config.SafetyConfig` hard caps are applied first, then the + command is routed through the active path (avoidance ``Move`` if + enabled, else sport ``Move``). When ``cfg.dry_run`` is set the intended + command is logged but only a zero velocity is actually sent. + """ + safety = self.cfg.safety + cvx = _clamp(vx, -safety.max_vx, safety.max_vx) + cvy = _clamp(vy, -safety.max_vy, safety.max_vy) + cvyaw = _clamp(vyaw, -safety.max_vyaw, safety.max_vyaw) + + if self.cfg.dry_run: + logger.info( + "[dry_run] drive intent vx=%.3f vy=%.3f vyaw=%.3f (sending 0,0,0)", + cvx, + cvy, + cvyaw, + ) + self._send_move(0.0, 0.0, 0.0) + return + + self._send_move(cvx, cvy, cvyaw) + + def _send_move(self, vx: float, vy: float, vyaw: float) -> None: + """Dispatch a velocity through the currently-active drive path.""" + if self._avoidance_on: + self._call("oa.Move", lambda: self._oa.Move(vx, vy, vyaw)) + else: + self._call("sport.Move", lambda: self._sport.Move(vx, vy, vyaw)) + + def stop(self) -> None: + """Command zero velocity through the active path (stay standing).""" + self._send_move(0.0, 0.0, 0.0) + + def set_avoidance(self, on: bool) -> None: + """Enable/disable the on-robot LiDAR hard-stop avoidance layer. + + When enabling: poll ``SwitchSet/SwitchGet`` until the firmware reports + the switch as on, then claim remote control via + ``UseRemoteCommandFromApi(True)``. When disabling: release remote + control and turn the switch off. ``drive`` routes through ``oa.Move`` + only while enabled. + """ + if on: + ok = self._enable_avoidance() + self._avoidance_on = ok + if not ok: + logger.warning( + "Avoidance could not be enabled; falling back to sport.Move path" + ) + else: + self._call( + "oa.UseRemoteCommandFromApi(False)", + lambda: self._oa.UseRemoteCommandFromApi(False), + ) + self._call("oa.SwitchSet(False)", lambda: self._oa.SwitchSet(False)) + self._avoidance_on = False + logger.info("LiDAR avoidance disabled") + + def _enable_avoidance(self) -> bool: + """Turn the firmware avoidance switch on and claim API control. + + Returns: + ``True`` if the switch is confirmed on and API control was claimed. + """ + deadline = time.monotonic() + 5.0 + enabled = False + while time.monotonic() < deadline: + try: + self._oa.SwitchSet(True) + code, enable = self._oa.SwitchGet() + except Exception as exc: # pragma: no cover - hardware path + logger.debug("Avoidance SwitchGet/Set raised: %s", exc) + time.sleep(0.1) + continue + if code == 0 and enable: + enabled = True + break + time.sleep(0.1) + + if not enabled: + return False + + rc = self._call( + "oa.UseRemoteCommandFromApi(True)", + lambda: self._oa.UseRemoteCommandFromApi(True), + ) + if rc != 0: + return False + logger.info("LiDAR avoidance enabled (API remote control claimed)") + return True + + # ------------------------------------------------------------------ # + # Posture / expression # + # ------------------------------------------------------------------ # + def balance_stand(self) -> None: + """Enter balanced standing (ready-to-move).""" + self._call("sport.BalanceStand", self._sport.BalanceStand) + + def stand_up(self) -> None: + """Stiff stand up.""" + self._call("sport.StandUp", self._sport.StandUp) + + def damp(self) -> None: + """Emergency soft-stop: limp the motors (safe failure posture).""" + self._call("sport.Damp", self._sport.Damp) + + def gesture(self, name: str) -> None: + """Perform an expressive gesture (see :data:`GESTURES`). + + Unknown names are logged and ignored (no-op). + """ + fn = self._gestures.get(name) + if fn is None: + logger.warning( + "Unknown gesture %r (known: %s) -- ignoring", + name, + ", ".join(GESTURES), + ) + return + self._call(f"sport.{name}", fn) + + # ------------------------------------------------------------------ # + # Greeting payload # + # ------------------------------------------------------------------ # + def play_greeting(self) -> None: + """Play the configured greeting clip (non-blocking). + + Best-effort sets the Go2 on-board volume once via ``VuiClient`` (the + gesture sequence itself is driven by the state machine calling + :meth:`gesture`). + """ + if self._vui is not None and not self._greet_volume_set: + try: + self._vui.SetVolume(int(self.cfg.greet.audio_volume)) + except Exception as exc: # pragma: no cover - firmware-dependent + logger.debug("VuiClient.SetVolume failed: %s", exc) + finally: + self._greet_volume_set = True + + try: + self.audio.play(self.cfg.greet.wav_path, blocking=False) + except Exception as exc: + logger.warning("Greeting playback failed: %s", exc) + + # ------------------------------------------------------------------ # + # Lifecycle # + # ------------------------------------------------------------------ # + def shutdown(self) -> None: + """Stop motion, release the avoidance API, close camera + audio. + + Idempotent and safe to call from a signal handler / ``finally`` block. + Every step is wrapped so one failure does not block the rest. + """ + with self._shutdown_lock: + if self._shutdown: + return + self._shutdown = True + + # 1. Stop the camera thread. + try: + self._cam_stop.set() + if self._cam_thread.is_alive(): + self._cam_thread.join(timeout=2.0) + except Exception as exc: + logger.warning("Camera thread shutdown failed: %s", exc) + + # 2. Release the firmware avoidance path (zero velocity + drop API). + if self._avoidance_on: + try: + self._oa.Move(0.0, 0.0, 0.0) + except Exception as exc: + logger.warning("oa.Move(0,0,0) on shutdown failed: %s", exc) + try: + self._oa.UseRemoteCommandFromApi(False) + except Exception as exc: + logger.warning("oa.UseRemoteCommandFromApi(False) failed: %s", exc) + self._avoidance_on = False + + # 3. Stop locomotion. + try: + self._sport.StopMove() + except Exception as exc: + logger.warning("sport.StopMove() on shutdown failed: %s", exc) + + # 4. Close audio. + try: + self.audio.close() + except Exception as exc: + logger.warning("audio.close() on shutdown failed: %s", exc) + + logger.info("Go2Robot shutdown complete") + + # ------------------------------------------------------------------ # + # Helpers # + # ------------------------------------------------------------------ # + def _call(self, what: str, fn: Callable[[], int]) -> int: + """Invoke an SDK call, logging (never raising) on a non-zero/error. + + Args: + what: Human-readable label for log messages. + fn: A zero-arg callable returning an SDK int code (0 == ok). + + Returns: + The SDK return code, or ``-1`` if the call raised. + """ + try: + code = fn() + except Exception as exc: # pragma: no cover - hardware path + logger.error("%s raised: %s", what, exc) + return -1 + if code is not None and code != 0: + logger.warning("%s returned non-zero code=%s", what, code) + return code if code is not None else 0 + + +def _clamp(value: float, lo: float, hi: float) -> float: + """Clamp ``value`` into the inclusive ``[lo, hi]`` range.""" + return lo if value < lo else hi if value > hi else value diff --git a/gowelcome/robot/interface.py b/gowelcome/robot/interface.py new file mode 100644 index 0000000..f139d1e --- /dev/null +++ b/gowelcome/robot/interface.py @@ -0,0 +1,123 @@ +"""Abstract robot + audio contracts. + +The state machine and main loop talk *only* to ``RobotInterface``; concrete +implementations are :class:`gowelcome.robot.go2_robot.Go2Robot` (real hardware +via the official Unitree SDK) and :class:`gowelcome.robot.mock_robot.MockRobot` +(webcam / video file, for off-robot development). + +Velocity convention (matches Unitree ``SportClient.Move``): + vx forward (+) / backward (-) metres/second, body frame + vy left (+) / right (-) metres/second, body frame + vyaw turn-left/CCW (+) / right/CW (-) radians/second + +Gesture names accepted by :meth:`RobotInterface.gesture` (mapped to the SDK on +real hardware): ``hello``, ``heart``, ``stretch``, ``dance1``, ``dance2``, +``scrape``, ``content``, ``sit``, ``rise_sit``. +""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import Optional, TYPE_CHECKING + +if TYPE_CHECKING: + import numpy as np + + +# Canonical gesture vocabulary the state machine may request. Backends map +# these to whatever the underlying hardware supports (unknown -> no-op + warn). +GESTURES = ( + "hello", + "heart", + "stretch", + "dance1", + "dance2", + "scrape", + "content", + "sit", + "rise_sit", + # dog-play actions (supported by the WebRTC backend; no-op where unsupported) + "wiggle", + "wallow", + "pounce", +) + + +class AudioBackend(ABC): + """Plays the greeting clip. Pluggable because the Go2 has no first-party + SDK audio path (see ``gowelcome/robot/audio.py`` for the gory details).""" + + @abstractmethod + def play(self, wav_path: str, blocking: bool = False) -> bool: + """Play ``wav_path``. Returns ``True`` if playback was dispatched. + + ``blocking=True`` waits for the clip to finish; ``False`` returns + immediately (fire-and-forget). Implementations must never raise on a + missing file or backend error -- log and return ``False`` instead. + """ + + def close(self) -> None: # optional cleanup hook + """Release any audio resources. Default: no-op.""" + + +class RobotInterface(ABC): + """Everything the behaviour layer needs from a robot.""" + + # --- perception input ------------------------------------------------- + @abstractmethod + def get_frame(self) -> "Optional[np.ndarray]": + """Return the latest BGR camera frame, or ``None`` if unavailable.""" + + @abstractmethod + def frame_size(self) -> "tuple[int, int]": + """Current ``(width, height)``; ``(0, 0)`` until the first frame.""" + + # --- locomotion ------------------------------------------------------- + @abstractmethod + def drive(self, vx: float, vy: float, vyaw: float) -> None: + """Command a body-frame velocity (one tick). Call repeatedly to keep + moving. On real hardware this goes through the firmware LiDAR + obstacle-avoidance layer when avoidance is enabled.""" + + @abstractmethod + def stop(self) -> None: + """Command zero velocity (stay standing).""" + + @abstractmethod + def set_avoidance(self, on: bool) -> None: + """Enable/disable the on-robot LiDAR hard-stop avoidance layer.""" + + # --- posture / expression -------------------------------------------- + @abstractmethod + def balance_stand(self) -> None: + """Enter balanced standing (ready-to-move).""" + + @abstractmethod + def stand_up(self) -> None: + """Stiff stand up.""" + + @abstractmethod + def damp(self) -> None: + """Emergency soft-stop: limp motors. Safe failure posture.""" + + @abstractmethod + def gesture(self, name: str) -> None: + """Perform an expressive gesture (see ``GESTURES``).""" + + # --- greeting payload ------------------------------------------------- + @abstractmethod + def play_greeting(self) -> None: + """Play the configured greeting audio clip (non-blocking).""" + + # --- lifecycle -------------------------------------------------------- + @abstractmethod + def shutdown(self) -> None: + """Stop motion, release avoidance API, close camera/audio. Idempotent + and safe to call from a signal handler / ``finally`` block.""" + + # context-manager sugar so callers can ``with build_robot(...) as bot:`` + def __enter__(self) -> "RobotInterface": + return self + + def __exit__(self, exc_type, exc, tb) -> None: + self.shutdown() diff --git a/gowelcome/robot/mock_robot.py b/gowelcome/robot/mock_robot.py new file mode 100644 index 0000000..23280d9 --- /dev/null +++ b/gowelcome/robot/mock_robot.py @@ -0,0 +1,183 @@ +"""Off-robot robot backend for development and testing. + +:class:`MockRobot` implements :class:`RobotInterface` against a laptop webcam or +a video file (via OpenCV) instead of a real Go2. Locomotion / posture / gesture +commands are merely logged -- nothing moves. This lets you exercise the +perception pipeline and state machine on a workstation with no hardware. + +All optional/heavy imports (``cv2``) are performed lazily inside ``__init__`` so +the package stays importable on a machine without OpenCV. +""" + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING, Optional, Tuple + +from config import GoWelcomeConfig +from gowelcome.robot.audio import build_audio_backend +from gowelcome.robot.interface import GESTURES, RobotInterface + +if TYPE_CHECKING: + import numpy as np + +logger = logging.getLogger(__name__) + + +class MockRobot(RobotInterface): + """A :class:`RobotInterface` backed by a webcam / video file, no hardware. + + Frames come from ``cv2.VideoCapture(cfg.camera.mock_source)`` (an integer + webcam index when the source is all digits, otherwise a file path). If the + capture cannot be opened, the robot operates *frameless* -- :meth:`get_frame` + returns ``None`` -- so the rest of the stack can still run. + + Locomotion / posture / gesture calls are logged with a ``[MOCK]`` prefix. + Greeting audio is dispatched through whatever backend + :func:`gowelcome.robot.audio.build_audio_backend` selects for ``cfg``. + """ + + def __init__(self, cfg: GoWelcomeConfig) -> None: + self._cfg = cfg + self._cap = None + self._is_file = False + self._frame_size: Tuple[int, int] = (0, 0) + self._avoidance_on = cfg.safety.use_lidar_avoidance + + # Lazy OpenCV import -- keep the package importable without cv2. + try: + import cv2 # type: ignore + except ImportError: + logger.warning( + "MockRobot: OpenCV (cv2) not available " + "(pip install opencv-python) -- running frameless.", + ) + cv2 = None + self._cv2 = cv2 + + # Audio backend (NullAudio for mock by default, but honour cfg.audio). + self.audio = build_audio_backend(cfg) + + if self._cv2 is not None: + self._open_capture() + + # --- capture helpers -------------------------------------------------- + def _open_capture(self) -> None: + """Open the configured camera/video source, logging on failure.""" + cv2 = self._cv2 + src_str = str(self._cfg.camera.mock_source) + if src_str.isdigit(): + source = int(src_str) + self._is_file = False + else: + source = src_str + self._is_file = True + + try: + cap = cv2.VideoCapture(source) + except Exception as exc: # noqa: BLE001 + logger.warning("MockRobot: failed to create VideoCapture(%r): %s", source, exc) + self._cap = None + return + + if not cap.isOpened(): + logger.warning( + "MockRobot: could not open camera source %r -- running frameless.", + source, + ) + self._cap = None + return + + self._cap = cap + logger.info("MockRobot: opened frame source %r (is_file=%s)", source, self._is_file) + + # --- perception input ------------------------------------------------- + def get_frame(self) -> "Optional[np.ndarray]": + """Return the latest BGR frame resized to the configured size. + + Video files loop (seek to frame 0 on EOF). Returns ``None`` when no + capture is available or a read genuinely fails. + """ + if self._cap is None or self._cv2 is None: + return None + cv2 = self._cv2 + + ok, frame = self._cap.read() + if not ok or frame is None: + # End of a video file -> loop back to the start and retry once. + if self._is_file: + self._cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + ok, frame = self._cap.read() + if not ok or frame is None: + logger.debug("MockRobot: frame read failed") + return None + + w, h = self._cfg.camera.width, self._cfg.camera.height + try: + frame = cv2.resize(frame, (w, h)) + except Exception as exc: # noqa: BLE001 + logger.warning("MockRobot: resize failed: %s", exc) + return None + + self._frame_size = (w, h) + return frame + + def frame_size(self) -> Tuple[int, int]: + """Current ``(width, height)``; ``(0, 0)`` until the first frame.""" + return self._frame_size + + # --- locomotion ------------------------------------------------------- + def drive(self, vx: float, vy: float, vyaw: float) -> None: + """Log a body-frame velocity command (no hardware motion).""" + logger.info("[MOCK] drive vx=%.3f vy=%.3f vyaw=%.3f", vx, vy, vyaw) + + def stop(self) -> None: + """Log a zero-velocity (stay-standing) command.""" + logger.info("[MOCK] stop") + + def set_avoidance(self, on: bool) -> None: + """Log a change to the (virtual) LiDAR avoidance toggle.""" + self._avoidance_on = on + logger.info("[MOCK] set_avoidance on=%s", on) + + # --- posture / expression -------------------------------------------- + def balance_stand(self) -> None: + """Log entering balanced standing.""" + logger.info("[MOCK] balance_stand") + + def stand_up(self) -> None: + """Log a stiff stand-up.""" + logger.info("[MOCK] stand_up") + + def damp(self) -> None: + """Log an emergency soft-stop (limp).""" + logger.info("[MOCK] damp") + + def gesture(self, name: str) -> None: + """Log an expressive gesture; warn if the name is unknown.""" + if name not in GESTURES: + logger.warning("[MOCK] gesture: unknown gesture %r (known: %s)", name, GESTURES) + return + logger.info("[MOCK] gesture %s", name) + + # --- greeting payload ------------------------------------------------- + def play_greeting(self) -> None: + """Dispatch the configured greeting clip through the audio backend.""" + wav_path = self._cfg.greet.wav_path + logger.info("[MOCK] play_greeting %s", wav_path) + self.audio.play(wav_path) + + # --- lifecycle -------------------------------------------------------- + def shutdown(self) -> None: + """Release the capture and audio backend. Idempotent.""" + logger.info("[MOCK] shutdown") + if self._cap is not None: + try: + self._cap.release() + except Exception as exc: # noqa: BLE001 + logger.debug("MockRobot: capture release failed: %s", exc) + self._cap = None + try: + self.audio.close() + except Exception as exc: # noqa: BLE001 + logger.debug("MockRobot: audio close failed: %s", exc) diff --git a/gowelcome/robot/webrtc_robot.py b/gowelcome/robot/webrtc_robot.py new file mode 100644 index 0000000..1eb51f4 --- /dev/null +++ b/gowelcome/robot/webrtc_robot.py @@ -0,0 +1,514 @@ +"""WebRTC robot backend for the Go2 (the default ``webrtc`` transport). + +Drives a Unitree Go2 over WebRTC using ``unitree_webrtc_connect`` -- the same +protocol the Unitree Go / Explore apps use, so it works on **AIR / PRO / EDU +over wifi** and can play the greeting **from the dog's own speaker** (AudioHub), +which the official DDS SDK cannot do on a Go2. + +The library is fully asynchronous (asyncio + aiortc); GoWelcome is threaded and +synchronous. This class bridges the two: it runs the WebRTC connection on a +dedicated event-loop thread and exposes the synchronous :class:`RobotInterface`. + + * High-rate commands (drive/gesture/posture) are sent **fire-and-forget** via + ``publish_without_callback`` scheduled with ``loop.call_soon_threadsafe`` -- + the control loop never blocks and no response-future accumulates. + * Setup steps that need confirmation (connect, mode switch, avoidance enable, + AudioHub upload) are awaited via ``run_coroutine_threadsafe(...).result()``. + +Velocity convention (matches the rest of GoWelcome): vx forward+, vy left+, +vyaw CCW/left+ (rad/s). + +All heavy deps (``unitree_webrtc_connect``, ``aiortc``, ``cv2``) are imported +lazily so this module imports fine off-robot. +""" + +from __future__ import annotations + +import asyncio +import json +import logging +import os +import threading +from typing import TYPE_CHECKING, Optional, Tuple + +import numpy as np + +from gowelcome.robot.interface import GESTURES, RobotInterface + +if TYPE_CHECKING: # pragma: no cover - type hints only + from config import GoWelcomeConfig + +logger = logging.getLogger(__name__) + +# GoWelcome gesture name -> Unitree SPORT_CMD key. (The Go2's heart gesture is +# the "FingerHeart" action; the rest map by name.) +_GESTURE_TO_CMD = { + "hello": "Hello", + "heart": "FingerHeart", + "stretch": "Stretch", + "dance1": "Dance1", + "dance2": "Dance2", + "scrape": "Scrape", + "content": "Content", + "sit": "Sit", + "rise_sit": "RiseSit", + # dog-play actions + "wiggle": "WiggleHips", + "wallow": "Wallow", + "pounce": "FrontPounce", +} + + +def _clamp(value: float, lo: float, hi: float) -> float: + return lo if value < lo else hi if value > hi else value + + +class Go2WebRTCRobot(RobotInterface): + """Drive a Go2 over WebRTC (app protocol) via ``unitree_webrtc_connect``. + + Args: + cfg: Fully-populated :class:`~config.GoWelcomeConfig`. + + Raises: + ImportError: if ``unitree_webrtc_connect`` / ``cv2`` are not installed. + ValueError: if the WebRTC connection parameters are incomplete. + """ + + def __init__(self, cfg: "GoWelcomeConfig") -> None: + try: + from unitree_webrtc_connect.webrtc_driver import UnitreeWebRTCConnection + from unitree_webrtc_connect.constants import ( + DATA_CHANNEL_TYPE, + OBSTACLES_AVOID_API, + RTC_TOPIC, + SPORT_CMD, + WebRTCConnectionMethod, + ) + from unitree_webrtc_connect.webrtc_audiohub import WebRTCAudioHub + except ImportError as exc: # pragma: no cover - off-robot path + raise ImportError( + "Go2WebRTCRobot requires 'unitree_webrtc_connect'. Install with: " + "pip install unitree_webrtc_connect (plus: sudo apt install " + "portaudio19-dev). For Go2 firmware >= 1.1.15 you also need the " + "per-device AES key -- see the repo's examples/fetch_aes_key.py." + ) from exc + try: + import cv2 + except ImportError as exc: # pragma: no cover - off-robot path + raise ImportError( + "Go2WebRTCRobot requires OpenCV: pip install opencv-python" + ) from exc + + self.cfg = cfg + self._cv2 = cv2 + self._RTC_TOPIC = RTC_TOPIC + self._SPORT_CMD = SPORT_CMD + self._OA = OBSTACLES_AVOID_API + self._REQUEST = DATA_CHANNEL_TYPE["REQUEST"] + + # --- camera frame buffer (written by the video callback) ------------ + self._frame: Optional[np.ndarray] = None + self._frame_size: Tuple[int, int] = (0, 0) + self._frame_lock = threading.Lock() + + # --- state ---------------------------------------------------------- + self._closing = False + self._shutdown = False + self._shutdown_lock = threading.Lock() + self._avoidance_on = False + self._audiohub = None + self._greeting_uuid: Optional[str] = None + self._stream_player = None # keep a ref so the aiortc track isn't GC'd + self._cmd_id = 0 + self._audio_method = (cfg.webrtc.audio_method or "audiohub").strip().lower() + + # --- build the connection object ------------------------------------ + self._conn = self._build_connection(cfg, UnitreeWebRTCConnection, WebRTCConnectionMethod) + + # --- start the asyncio loop on its own daemon thread ---------------- + self._loop = asyncio.new_event_loop() + self._loop_thread = threading.Thread( + target=self._run_loop, name="Go2WebRTCLoop", daemon=True + ) + self._loop_thread.start() + + # --- connect (blocking) --------------------------------------------- + self._run(self._conn.connect(), timeout=30.0) + logger.info("WebRTC connected (%s)", cfg.webrtc.connection_method) + + # --- ensure normal motion mode -------------------------------------- + try: + self._run( + self._conn.datachannel.pub_sub.publish_request_new( + RTC_TOPIC["MOTION_SWITCHER"], + {"api_id": 1002, "parameter": {"name": "normal"}}, + ), + timeout=10.0, + ) + except Exception as exc: # pragma: no cover - hardware path + logger.warning("motion-switcher to 'normal' failed: %s", exc) + + # --- camera on + frame callback (run on the loop thread; switching the + # video channel sends on the data channel) ------------------------- + async def _setup_video(): + self._conn.video.switchVideoChannel(True) + self._conn.video.add_track_callback(self._on_video_track) + + try: + self._run(_setup_video(), timeout=8.0) + except Exception as exc: # pragma: no cover - hardware path + logger.warning("video channel setup failed: %s", exc) + + # --- audio backend (AudioHub) --------------------------------------- + try: + self._audiohub = WebRTCAudioHub(self._conn) + except Exception as exc: # pragma: no cover - hardware path + logger.warning("AudioHub init failed: %s", exc) + self._audiohub = None + if self._audio_method == "audiohub": + self._prepare_audiohub_greeting() + + # --- obstacle avoidance --------------------------------------------- + if cfg.safety.use_lidar_avoidance: + self.set_avoidance(True) + + # --- ready posture -------------------------------------------------- + self.balance_stand() + logger.info( + "Go2WebRTCRobot ready (audio=%s, avoidance=%s, dry_run=%s)", + self._audio_method, self._avoidance_on, cfg.dry_run, + ) + + # ------------------------------------------------------------------ # + # Connection construction + # ------------------------------------------------------------------ # + def _build_connection(self, cfg, UnitreeWebRTCConnection, WebRTCConnectionMethod): + wc = cfg.webrtc + method = (wc.connection_method or "localsta").strip().lower() + kwargs = {"region": wc.region or "global", "device_type": "Go2"} + if wc.aes_128_key: + kwargs["aes_128_key"] = wc.aes_128_key + if wc.serial_number: + kwargs["serialNumber"] = wc.serial_number + + if method == "localsta": + cm = WebRTCConnectionMethod.LocalSTA + if wc.ip: + kwargs["ip"] = wc.ip + elif not wc.serial_number: + raise ValueError( + "WebRTC localsta needs webrtc.ip or webrtc.serial_number " + "(pass --robot-ip or --serial )." + ) + elif method == "localap": + cm = WebRTCConnectionMethod.LocalAP + elif method == "remote": + cm = WebRTCConnectionMethod.Remote + if not (wc.serial_number and wc.username and wc.password): + raise ValueError( + "WebRTC remote needs webrtc.serial_number + username + password." + ) + kwargs["username"] = wc.username + kwargs["password"] = wc.password + else: + raise ValueError(f"Unknown webrtc.connection_method: {wc.connection_method!r}") + + return UnitreeWebRTCConnection(cm, **kwargs) + + # ------------------------------------------------------------------ # + # asyncio bridge + # ------------------------------------------------------------------ # + def _run_loop(self) -> None: + asyncio.set_event_loop(self._loop) + self._loop.run_forever() + + def _run(self, coro, timeout: float = 10.0): + """Run ``coro`` on the loop thread and block for its result.""" + fut = asyncio.run_coroutine_threadsafe(coro, self._loop) + return fut.result(timeout) + + def _run_bg(self, coro) -> None: + """Fire-and-forget a coroutine on the loop thread (non-blocking).""" + try: + fut = asyncio.run_coroutine_threadsafe(coro, self._loop) + fut.add_done_callback( + lambda f: f.exception() and logger.debug("bg coro error: %s", f.exception()) + ) + except Exception as exc: # pragma: no cover + logger.debug("schedule bg coro failed: %s", exc) + + def _next_id(self) -> int: + self._cmd_id = (self._cmd_id + 1) % 2147483000 + return self._cmd_id + + def _fire(self, topic: str, api_id: int, parameter=None) -> None: + """Fire-and-forget a request command from any thread (no reply wait). + + Builds the same request envelope ``publish_request_new`` uses, then + sends it via the no-future ``publish_without_callback`` scheduled on the + loop thread -- so the control loop never blocks and no response-future + leaks (critical for the high-rate Move path). + """ + payload = { + "header": {"identity": {"id": self._next_id(), "api_id": api_id}}, + "parameter": "", + } + if parameter is not None: + payload["parameter"] = ( + parameter if isinstance(parameter, str) else json.dumps(parameter) + ) + + def _send(): + try: + self._conn.datachannel.pub_sub.publish_without_callback( + topic, payload, self._REQUEST + ) + except Exception as exc: # pragma: no cover - hardware path + logger.debug("publish failed (api_id=%s): %s", api_id, exc) + + try: + self._loop.call_soon_threadsafe(_send) + except Exception as exc: # pragma: no cover + logger.debug("call_soon_threadsafe failed: %s", exc) + + # ------------------------------------------------------------------ # + # Camera + # ------------------------------------------------------------------ # + async def _on_video_track(self, track) -> None: + """Receive video frames into the shared buffer until shutdown.""" + cv2 = self._cv2 + target = (self.cfg.camera.width, self.cfg.camera.height) + while not self._closing: + try: + frame = await track.recv() + except Exception as exc: # pragma: no cover - track ended + logger.debug("video recv ended: %s", exc) + break + try: + img = frame.to_ndarray(format="bgr24") + if (img.shape[1], img.shape[0]) != target: + img = cv2.resize(img, target) + with self._frame_lock: + self._frame = img + self._frame_size = (img.shape[1], img.shape[0]) + except Exception as exc: # pragma: no cover + logger.debug("video decode failed: %s", exc) + + def get_frame(self) -> "Optional[np.ndarray]": + with self._frame_lock: + return self._frame + + def frame_size(self) -> "tuple[int, int]": + with self._frame_lock: + return self._frame_size + + # ------------------------------------------------------------------ # + # Locomotion + # ------------------------------------------------------------------ # + def drive(self, vx: float, vy: float, vyaw: float) -> None: + s = self.cfg.safety + vx = _clamp(vx, -s.max_vx, s.max_vx) + vy = _clamp(vy, -s.max_vy, s.max_vy) + vyaw = _clamp(vyaw, -s.max_vyaw, s.max_vyaw) + if self.cfg.dry_run: + logger.info( + "[dry_run] drive intent vx=%.3f vy=%.3f vyaw=%.3f (sending 0)", + vx, vy, vyaw, + ) + vx = vy = vyaw = 0.0 + if self._avoidance_on: + self._fire(self._RTC_TOPIC["OBSTACLES_AVOID"], self._OA["MOVE"], + {"x": vx, "y": vy, "yaw": vyaw, "mode": 0}) + else: + self._fire(self._RTC_TOPIC["SPORT_MOD"], self._SPORT_CMD["Move"], + {"x": vx, "y": vy, "z": vyaw}) + + def stop(self) -> None: + if self._avoidance_on: + self._fire(self._RTC_TOPIC["OBSTACLES_AVOID"], self._OA["MOVE"], + {"x": 0.0, "y": 0.0, "yaw": 0.0, "mode": 0}) + else: + self._fire(self._RTC_TOPIC["SPORT_MOD"], self._SPORT_CMD["StopMove"]) + + def set_avoidance(self, on: bool) -> None: + oa = self._RTC_TOPIC["OBSTACLES_AVOID"] + try: + if on: + self._run(self._conn.datachannel.pub_sub.publish_request_new( + oa, {"api_id": self._OA["SWITCH_SET"], + "parameter": {"enable": True}}), timeout=8.0) + self._run(self._conn.datachannel.pub_sub.publish_request_new( + oa, {"api_id": self._OA["USE_REMOTE_COMMAND_FROM_API"], + "parameter": {"is_remote_commands_from_api": True}}), timeout=8.0) + self._avoidance_on = True + logger.info("LiDAR avoidance enabled (WebRTC)") + else: + self._run(self._conn.datachannel.pub_sub.publish_request_new( + oa, {"api_id": self._OA["USE_REMOTE_COMMAND_FROM_API"], + "parameter": {"is_remote_commands_from_api": False}}), timeout=8.0) + self._run(self._conn.datachannel.pub_sub.publish_request_new( + oa, {"api_id": self._OA["SWITCH_SET"], + "parameter": {"enable": False}}), timeout=8.0) + self._avoidance_on = False + logger.info("LiDAR avoidance disabled (WebRTC)") + except Exception as exc: # pragma: no cover - hardware path + logger.warning( + "set_avoidance(%s) failed: %s -- falling back to sport Move path", + on, exc, + ) + self._avoidance_on = False + + # ------------------------------------------------------------------ # + # Posture / gestures + # ------------------------------------------------------------------ # + def _sport(self, cmd_name: str) -> None: + api = self._SPORT_CMD.get(cmd_name) + if api is None: + logger.warning("unknown sport command %r", cmd_name) + return + self._fire(self._RTC_TOPIC["SPORT_MOD"], api) + + def balance_stand(self) -> None: + self._sport("BalanceStand") + + def stand_up(self) -> None: + self._sport("StandUp") + + def damp(self) -> None: + self._sport("Damp") + + def gesture(self, name: str) -> None: + cmd = _GESTURE_TO_CMD.get(name) + if cmd is None: + logger.warning("unknown gesture %r (known: %s)", name, ", ".join(GESTURES)) + return + self._sport(cmd) + + # ------------------------------------------------------------------ # + # Greeting audio + # ------------------------------------------------------------------ # + def _prepare_audiohub_greeting(self) -> None: + """Upload greeting.wav to the robot and resolve its AudioHub uuid.""" + if self._audiohub is None: + return + path = self.cfg.greet.wav_path + if not os.path.isfile(path): + logger.warning("greeting wav not found for AudioHub upload: %s", path) + return + name = os.path.splitext(os.path.basename(path))[0] + try: + self._run(self._audiohub.upload_audio_file(path), timeout=60.0) + resp = self._run(self._audiohub.get_audio_list(), timeout=10.0) + self._greeting_uuid = self._find_uuid(resp, name) + if self._greeting_uuid: + logger.info("AudioHub greeting ready (uuid=%s)", self._greeting_uuid) + else: + logger.warning( + "uploaded greeting but could not resolve its uuid from the list" + ) + except Exception as exc: # pragma: no cover - hardware path + logger.warning("AudioHub greeting prepare failed: %s", exc) + + @staticmethod + def _find_uuid(resp, name: str) -> Optional[str]: + """Best-effort extraction of an audio entry's uuid from a list response. + + The exact shape of the AudioHub list response is firmware-dependent, so + this walks common shapes: ``resp['data']['data']`` (often a JSON string) + containing a list of ``{name/file_name, unique_id/uuid/id}`` records. + """ + try: + data = resp.get("data") if isinstance(resp, dict) else None + blob = data.get("data") if isinstance(data, dict) else data + if isinstance(blob, str): + blob = json.loads(blob) + items = [] + if isinstance(blob, dict): + for v in blob.values(): + if isinstance(v, list): + items = v + break + elif isinstance(blob, list): + items = blob + for it in items: + if not isinstance(it, dict): + continue + nm = it.get("name") or it.get("file_name") or it.get("custom_name") + uid = it.get("unique_id") or it.get("uuid") or it.get("id") + if uid and (nm == name or (isinstance(nm, str) and name in nm)): + return uid + if items and isinstance(items[-1], dict): + last = items[-1] + return last.get("unique_id") or last.get("uuid") or last.get("id") + except Exception as exc: # pragma: no cover + logger.debug("uuid parse failed: %s", exc) + return None + + def play_greeting(self) -> None: + if self._audio_method == "stream": + self._play_stream() + return + if self._audiohub is not None and self._greeting_uuid: + self._run_bg(self._audiohub.play_by_uuid(self._greeting_uuid)) + else: + logger.warning( + "AudioHub greeting unavailable (uuid=%s); no audio played", + self._greeting_uuid, + ) + + def _play_stream(self) -> None: + """Stream the greeting wav live via an aiortc MediaPlayer track.""" + path = self.cfg.greet.wav_path + if not os.path.isfile(path): + logger.warning("greeting wav not found: %s", path) + return + + def _add(): + try: + from aiortc.contrib.media import MediaPlayer + player = MediaPlayer(path) + self._stream_player = player # keep a reference alive + self._conn.pc.addTrack(player.audio) + except Exception as exc: # pragma: no cover - hardware path + logger.warning("stream greeting failed: %s", exc) + + self._loop.call_soon_threadsafe(_add) + + # ------------------------------------------------------------------ # + # Lifecycle + # ------------------------------------------------------------------ # + def shutdown(self) -> None: + """Stop motion, release avoidance, disconnect, stop the loop. Idempotent.""" + with self._shutdown_lock: + if self._shutdown: + return + self._shutdown = True + self._closing = True + + try: + self.stop() + except Exception as exc: # pragma: no cover + logger.debug("stop() on shutdown failed: %s", exc) + + if self._avoidance_on: + try: + self.set_avoidance(False) + except Exception as exc: # pragma: no cover + logger.warning("avoidance release on shutdown failed: %s", exc) + + try: + self._run(self._conn.disconnect(), timeout=8.0) + except Exception as exc: # pragma: no cover + logger.warning("WebRTC disconnect failed: %s", exc) + + try: + self._loop.call_soon_threadsafe(self._loop.stop) + except Exception: # pragma: no cover + pass + try: + if self._loop_thread.is_alive(): + self._loop_thread.join(timeout=3.0) + except Exception: # pragma: no cover + pass + + logger.info("Go2WebRTCRobot shutdown complete") diff --git a/gowelcome/statemachine.py b/gowelcome/statemachine.py new file mode 100644 index 0000000..dc5ac88 --- /dev/null +++ b/gowelcome/statemachine.py @@ -0,0 +1,527 @@ +"""The reactive behaviour brain for GoWelcome. + +Owns high-level arbitration: it reads the latest perception snapshot and GPS +fix, decides which :class:`~gowelcome.types.State` should be active, and drives +the robot. Reactive and mapless -- every decision is a function of the most +recent perception + GPS plus a few monotonic timers. + +Arbitration priority inside :meth:`step` (highest first): + e-stop / pause (dashboard) -- hard override, halt + stale perception -- halt until fresh frames + AVOID_DANGER (cars / road) -- immediate collision safety wins + GPS hold (no fix while geofencing) -- don't roam blind + finish an in-progress GREET -- brief, stationary + BOUNDARY (geofence) -- home back; never chase out of the area + APPROACH a person + WANDER + idle dog-play + +The single public entry point is :meth:`step`, which runs one control tick and +never blocks. Dashboard control methods (``set_play_mode`` / ``set_paused`` / +``request_estop`` / ``set_geofence_center`` / ``status``) are thread-safe. + +Velocity convention: vx forward+, vy left+, vyaw CCW/left+ (rad/s). +""" + +from __future__ import annotations + +import logging +import math +import random +import threading +import time +from typing import Optional + +from config import GoWelcomeConfig +from gowelcome.types import Detection, PerceptionResult, State +from gowelcome.robot.interface import RobotInterface +from gowelcome.perception.vision_thread import PerceptionThread +from gowelcome.control.servoing import VisualServo +from gowelcome.geo import GeoFence, GpsSource + +logger = logging.getLogger(__name__) + +_PLAY_MODES = ("calm", "moderate", "playful") + + +class GoWelcomeStateMachine: + """Reactive five-state behaviour arbiter for the Go2 backyard greeter. + + Args: + robot: Locomotion / posture / audio backend (real or mock). + perception: Source of the latest :class:`PerceptionResult` snapshot. + cfg: Fully-populated :class:`GoWelcomeConfig`. + gps_source: Optional GPS reader; when present and ``cfg.geofence.enabled``, + the geofence BOUNDARY behaviour is active. + """ + + def __init__( + self, + robot: RobotInterface, + perception: PerceptionThread, + cfg: GoWelcomeConfig, + gps_source: Optional[GpsSource] = None, + ) -> None: + self._robot = robot + self._perception = perception + self._cfg = cfg + self._servo = VisualServo(cfg.servo) + + self._state: State = State.WANDER + + # --- geofence (optional) ----------------------------------------- + self._gps = gps_source + self._fence: Optional[GeoFence] = ( + GeoFence(cfg.geofence) + if (cfg.geofence.enabled and gps_source is not None) + else None + ) + self._fix = None # latest valid GpsFix (or None) + self._gps_warned = False + + # --- cross-state bookkeeping ------------------------------------- + self._wander_t = 0.0 + self._last_greet_time = float("-inf") + self._last_cmd = (0.0, 0.0, 0.0) + + # --- APPROACH ---------------------------------------------------- + self._lost_frames = 0 + + # --- GREET ------------------------------------------------------- + self._greet_start = 0.0 + self._greet_audio_played = False + self._greet_gestures_done = 0 + + # --- AVOID_DANGER ------------------------------------------------ + self._avoid_start = 0.0 + self._avoid_side = 1 + self._avoid_clear_count = 0 + + # --- dog-play ---------------------------------------------------- + self._play_until = 0.0 # while now < this, hold still and perform a trick + self._next_play_t = 0.0 # when to trigger the next trick + + # --- runtime controls (dashboard) ------------------------------- + self._ctrl_lock = threading.Lock() + self._play_mode = cfg.play.mode if cfg.play.mode in _PLAY_MODES else "moderate" + self._paused = False + self._estop = False + self._damped = False + + self._schedule_next_play(time.monotonic()) + logger.info( + "GoWelcomeStateMachine initialised (geofence=%s, play=%s)", + self._fence is not None, self._play_mode, + ) + + # ================================================================== # + # Public API + # ================================================================== # + @property + def state(self) -> State: + return self._state + + def step(self, dt: float) -> State: + """Run one control tick; returns the (possibly new) current state.""" + now = time.monotonic() + + # 0. Dashboard hard overrides. + if self._is_estopped(): + if not self._damped: + self._robot.damp() + self._damped = True + self._command_stop() + return self._state + self._damped = False + if self._is_paused(): + self._command_stop() + return self._state + + perc = self._perception.latest() + + # 1. Stale / missing perception -> halt, hold state. + if perc is None or (now - perc.ts) > self._cfg.safety.perception_timeout: + self._command_stop() + return self._state + + # 2. Update the GPS fix + capture the geofence centre. + self._update_gps() + + # 3. DANGER (cars / road) -- immediate collision safety, always wins. + danger = self._is_danger(perc) + if danger and self._state is not State.AVOID_DANGER: + self._set_state(State.AVOID_DANGER, perc=perc) + if self._state is State.AVOID_DANGER: + self._run_avoid(perc, now, dt) + return self._state + + # 4. GPS gating: if geofencing but we don't know where we are, hold. + if self._fence is not None: + if not self._fence.has_center: + self._hold("waiting for GPS fix to set geofence centre") + return self._state + if self._fix is None and self._cfg.geofence.no_fix_behavior == "stop": + self._hold("GPS fix lost -> holding (geofence safety)") + return self._state + + # 5. Finish an in-progress GREET (stationary, brief). + if self._state is State.GREET: + self._run_greet(now) + return self._state + + # 6. BOUNDARY: near/over the geofence edge -> home back to centre. + if self._fence is not None and self._fix is not None and \ + self._fence.breached(self._fix.lat, self._fix.lon): + if self._state is not State.BOUNDARY: + self._set_state(State.BOUNDARY, perc=perc) + if self._state is State.BOUNDARY: + self._run_boundary(now, dt) + return self._state + + # 7. A qualifying person -> APPROACH (respecting the re-greet cooldown). + cooled = (now - self._last_greet_time) >= self._cfg.greet.cooldown + if perc.persons and cooled: + if self._state is not State.APPROACH: + self._set_state(State.APPROACH, perc=perc) + self._run_approach(perc, now, dt) + return self._state + + # 8. Fallback -> WANDER (+ idle dog-play). + if self._state is not State.WANDER: + self._set_state(State.WANDER, perc=perc) + self._run_wander(now, dt, perc) + return self._state + + # --- dashboard controls (thread-safe) ----------------------------- + def set_play_mode(self, mode: str) -> bool: + """Set the idle dog-play intensity (calm/moderate/playful).""" + mode = (mode or "").strip().lower() + if mode not in _PLAY_MODES: + return False + with self._ctrl_lock: + self._play_mode = mode + logger.info("play mode -> %s", mode) + return True + + def play_mode(self) -> str: + with self._ctrl_lock: + return self._play_mode + + def set_paused(self, paused: bool) -> None: + with self._ctrl_lock: + self._paused = bool(paused) + logger.info("paused -> %s", paused) + + def request_estop(self) -> None: + with self._ctrl_lock: + self._estop = True + logger.warning("E-STOP requested") + + def clear_estop(self) -> None: + with self._ctrl_lock: + self._estop = False + logger.info("E-STOP cleared") + + def set_geofence_center(self, lat: Optional[float] = None, + lon: Optional[float] = None) -> bool: + """Re-centre the geofence here (uses the current fix if lat/lon omitted).""" + if self._fence is None: + return False + if lat is None or lon is None: + if self._fix is None: + return False + lat, lon = self._fix.lat, self._fix.lon + self._fence.set_center(lat, lon) + logger.info("geofence centre set to (%.6f, %.6f)", lat, lon) + return True + + def status(self) -> dict: + """A JSON-serialisable snapshot for the dashboard (thread-safe reads).""" + fix = self._fix + snap = { + "state": self._state.value, + "play_mode": self.play_mode(), + "paused": self._is_paused(), + "estop": self._is_estopped(), + "last_cmd": {"vx": round(self._last_cmd[0], 3), + "vy": round(self._last_cmd[1], 3), + "vyaw": round(self._last_cmd[2], 3)}, + "gps": None, + "geofence": None, + } + if fix is not None: + snap["gps"] = {"lat": fix.lat, "lon": fix.lon, + "sats": fix.num_sats, "hdop": fix.hdop, + "course_deg": fix.course_deg} + if self._fence is not None and self._fence.has_center: + g = {"center": self._fence.center, "radius_m": self._cfg.geofence.radius_m} + if fix is not None: + d = self._fence.distance_from_center(fix.lat, fix.lon) + g["distance_m"] = round(d, 1) if d is not None else None + g["inside"] = not self._fence.breached(fix.lat, fix.lon) + snap["geofence"] = g + return snap + + # ================================================================== # + # Transition management + # ================================================================== # + def _set_state(self, new: State, perc: Optional[PerceptionResult] = None) -> None: + if new is self._state: + return + old = self._state + self._state = new + logger.info("State transition: %s -> %s", old.value, new.value) + now = time.monotonic() + + if new is State.WANDER: + self._servo.reset() + self._lost_frames = 0 + self._schedule_next_play(now) + elif new is State.APPROACH: + self._servo.reset() + self._lost_frames = 0 + elif new is State.GREET: + self._greet_start = now + self._greet_audio_played = False + self._greet_gestures_done = 0 + self._command_stop() + self._robot.balance_stand() + elif new is State.AVOID_DANGER: + self._avoid_start = now + self._avoid_clear_count = 0 + if perc is not None and perc.road is not None: + self._avoid_side = -perc.road.clearer_side + else: + self._avoid_side = 1 + elif new is State.BOUNDARY: + self._servo.reset() + + # ================================================================== # + # Shared helpers + # ================================================================== # + def _is_danger(self, perc: PerceptionResult) -> bool: + if perc.dangers: + return True + road = perc.road + return road is not None and \ + road.center >= self._cfg.perception.road_trigger_coverage + + def _update_gps(self) -> None: + """Refresh the latest valid fix and capture the geofence centre.""" + if self._gps is None: + self._fix = None + return + fix = self._gps.latest() + self._fix = fix + if fix is not None and self._fence is not None: + self._fence.maybe_capture_center(fix.lat, fix.lon) + + def _command_drive(self, vx: float, vy: float, vyaw: float) -> None: + """Issue a locomotion command, honouring ``dry_run`` + feeding the GPS sim.""" + self._last_cmd = (vx, vy, vyaw) + if self._cfg.dry_run: + logger.info("[dry_run] drive(vx=%.3f, vy=%.3f, vyaw=%.3f) suppressed", + vx, vy, vyaw) + self._robot.stop() + eff = (0.0, 0.0, 0.0) + else: + self._robot.drive(vx, vy, vyaw) + eff = (vx, vy, vyaw) + if self._gps is not None: + try: + self._gps.on_command(*eff) + except Exception: # pragma: no cover + pass + + def _command_stop(self) -> None: + self._last_cmd = (0.0, 0.0, 0.0) + self._robot.stop() + if self._gps is not None: + try: + self._gps.on_command(0.0, 0.0, 0.0) + except Exception: # pragma: no cover + pass + + def _hold(self, why: str) -> None: + if not self._gps_warned: + logger.warning("%s", why) + self._gps_warned = True + self._command_stop() + + def _is_paused(self) -> bool: + with self._ctrl_lock: + return self._paused + + def _is_estopped(self) -> bool: + with self._ctrl_lock: + return self._estop + + # ================================================================== # + # WANDER (+ dog-play) + # ================================================================== # + def _run_wander(self, now: float, dt: float, + perc: Optional[PerceptionResult] = None) -> None: + # While performing a trick, hold still so the gesture completes. + if now < self._play_until: + self._command_stop() + return + self._wander_t += dt + wc = self._cfg.wander + vx = wc.forward_speed + phase = 2.0 * math.pi * self._wander_t / wc.yaw_sweep_period + vyaw = wc.yaw_sweep_rate * math.sin(phase) + + # Vision-only area containment: veer away from the road/cars and slow + # down BEFORE reaching the hard AVOID_DANGER trigger, so the dog keeps + # its distance from the road rather than only reacting at the edge. + yaw_bias, speed_scale = self._road_car_steer(perc) + vx *= speed_scale + vyaw += yaw_bias + + self._command_drive(vx, 0.0, vyaw) + # Only do idle tricks when not actively keeping away from road/cars. + if yaw_bias == 0.0 and speed_scale >= 0.99: + self._maybe_play(now) + + def _road_car_steer(self, perc: Optional[PerceptionResult]): + """Soft 'keep away from road/cars' steer for WANDER. + + Returns ``(yaw_bias_rad_s, forward_speed_scale)``. The road repulsion + veers toward the clearer side and slows as pavement fills the view; the + car repulsion veers away from the nearest detected vehicle (even far + ones the hard AVOID_DANGER ignores). ``+vyaw`` = turn left/CCW. + """ + ac = self._cfg.avoid + if not ac.soft_avoid_enabled or perc is None: + return 0.0, 1.0 + yaw_bias = 0.0 + speed_scale = 1.0 + + road = perc.road + if road is not None and \ + max(road.left, road.center, road.right) >= ac.soft_road_coverage: + # road heavier on the left -> (right-left) < 0 -> negative yaw = turn right. + yaw_bias += ac.road_repulsion_gain * (road.right - road.left) + speed_scale *= max(0.15, 1.0 - ac.road_slowdown_gain * road.center) + + cars = [d for d in perc.detections + if d.label in self._cfg.perception.danger_classes] + if cars: + car = max(cars, key=lambda d: d.area) # most prominent / nearest + # car on the right (offset>0) -> positive yaw = turn left, away from it. + yaw_bias += ac.car_repulsion_gain * car.horizontal_offset(perc.frame_w) + speed_scale *= max(0.3, 1.0 - car.height_ratio(perc.frame_h)) + + return yaw_bias, speed_scale + + def _maybe_play(self, now: float) -> None: + if self._cfg.dry_run or now < self._next_play_t: + return + actions = self._cfg.play.actions + if not actions: + return + action = actions[random.randrange(len(actions))] + logger.info("PLAY: %s", action) + self._robot.gesture(action) + self._play_until = now + self._cfg.play.action_hold + self._schedule_next_play(now + self._cfg.play.action_hold) + + def _schedule_next_play(self, after: float) -> None: + pc = self._cfg.play + base = pc.interval_for(self.play_mode()) + j = base * pc.jitter + self._next_play_t = after + base + random.uniform(-j, j) + + # ================================================================== # + # APPROACH + # ================================================================== # + def _run_approach(self, perc: PerceptionResult, now: float, dt: float) -> None: + target: Optional[Detection] = perc.biggest_person() + if target is None: + self._lost_frames += 1 + if self._lost_frames > self._cfg.loop.person_lost_frames: + logger.info("APPROACH lost person -> WANDER") + self._set_state(State.WANDER, perc=perc) + self._run_wander(now, dt, perc) + return + self._command_drive(0.0, 0.0, 0.0) + return + self._lost_frames = 0 + vx, vyaw, arrived = self._servo.compute(target, perc.frame_w, perc.frame_h, dt) + self._command_drive(vx, 0.0, vyaw) + if arrived: + logger.info("APPROACH arrived -> GREET") + self._set_state(State.GREET, perc=perc) + self._run_greet(time.monotonic()) + + # ================================================================== # + # GREET + # ================================================================== # + def _greet_total_duration(self) -> float: + gc = self._cfg.greet + return gc.settle_time + len(gc.gestures) * gc.gesture_gap + + def _run_greet(self, now: float) -> None: + gc = self._cfg.greet + elapsed = now - self._greet_start + if not self._greet_audio_played: + self._robot.play_greeting() + self._greet_audio_played = True + while self._greet_gestures_done < len(gc.gestures): + due_at = gc.settle_time + self._greet_gestures_done * gc.gesture_gap + if elapsed < due_at: + break + name = gc.gestures[self._greet_gestures_done] + logger.info("GREET gesture %r", name) + self._robot.gesture(name) + self._greet_gestures_done += 1 + if self._greet_gestures_done >= len(gc.gestures) and \ + elapsed >= self._greet_total_duration(): + self._last_greet_time = now + logger.info("GREET complete -> WANDER (cooldown %.1fs)", gc.cooldown) + self._set_state(State.WANDER, perc=None) + + # ================================================================== # + # AVOID_DANGER + # ================================================================== # + def _run_avoid(self, perc: PerceptionResult, now: float, dt: float) -> None: + ac = self._cfg.avoid + elapsed = now - self._avoid_start + if elapsed < ac.backup_duration: + self._command_drive(-ac.backup_speed, 0.0, self._avoid_side * ac.turn_rate) + else: + self._command_drive(0.0, 0.0, self._avoid_side * ac.turn_rate) + if self._is_danger(perc): + self._avoid_clear_count = 0 + else: + self._avoid_clear_count += 1 + if self._avoid_clear_count >= ac.clear_frames: + logger.info("AVOID_DANGER clear -> WANDER") + self._set_state(State.WANDER, perc=perc) + self._run_wander(now, dt, perc) + + # ================================================================== # + # BOUNDARY (geofence homing) + # ================================================================== # + def _run_boundary(self, now: float, dt: float) -> None: + fix = self._fix + if fix is None or self._fence is None: + # Lost GPS mid-return: hold (don't roam blind near the edge). + self._command_stop() + return + if self._fence.cleared(fix.lat, fix.lon): + logger.info("BOUNDARY cleared -> WANDER") + self._set_state(State.WANDER, perc=None) + self._run_wander(now, dt) + return + gc = self._cfg.geofence + # GPS course-over-ground is pure noise at low ground speed, so a stale/ + # garbage course could momentarily steer the wrong way. Below the speed + # threshold, hand homing_yaw course=None -> its gentle re-acquire turn, + # which drives forward to regain a valid track instead of trusting junk. + course = ( + fix.course_deg + if (fix.speed_mps is not None and fix.speed_mps >= gc.min_speed_for_course) + else None + ) + vyaw = self._fence.homing_yaw(fix.lat, fix.lon, course) + self._command_drive(gc.return_speed, 0.0, vyaw) diff --git a/gowelcome/types.py b/gowelcome/types.py new file mode 100644 index 0000000..b611d25 --- /dev/null +++ b/gowelcome/types.py @@ -0,0 +1,119 @@ +"""Shared data contracts for GoWelcome. + +These types are the *frozen interface* between the perception, control, robot, +and state-machine layers. Every module imports from here; nothing here imports +heavy/optional deps (ultralytics, cv2, the Unitree SDK), so this module is +always importable for off-robot testing. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from enum import Enum +from typing import List, Optional, Tuple, TYPE_CHECKING + +if TYPE_CHECKING: # numpy is only a type hint here; keep import light at runtime + import numpy as np + NDArray = np.ndarray # real reference -> static checkers + linters see it used +else: # pragma: no cover + NDArray = object + + +class State(Enum): + """The four core behaviour states of the GoWelcome machine.""" + + WANDER = "WANDER" # roam + idle dog-play, looking for a person + APPROACH = "APPROACH" # visual-servo toward a detected person + GREET = "GREET" # stop, play audio + gesture + AVOID_DANGER = "AVOID_DANGER" # road / vehicle detected -> steer to safety + BOUNDARY = "BOUNDARY" # near the geofence edge -> home back toward centre + + +@dataclass +class Detection: + """A single YOLO bounding box (pixel coords, top-left / bottom-right).""" + + label: str + conf: float + x1: int + y1: int + x2: int + y2: int + track_id: int = -1 # -1 when tracking is disabled / id unavailable + + @property + def cx(self) -> float: + return (self.x1 + self.x2) / 2.0 + + @property + def cy(self) -> float: + return (self.y1 + self.y2) / 2.0 + + @property + def w(self) -> float: + return float(self.x2 - self.x1) + + @property + def h(self) -> float: + return float(self.y2 - self.y1) + + @property + def area(self) -> float: + return self.w * self.h + + def height_ratio(self, frame_h: int) -> float: + """Fraction of the frame height the box fills (distance proxy).""" + return self.h / max(1, frame_h) + + def horizontal_offset(self, frame_w: int) -> float: + """Normalised horizontal error in ``[-1, 1]``; ``+`` = right of centre.""" + half = max(1.0, frame_w / 2.0) + return (self.cx - frame_w / 2.0) / half + + +@dataclass +class RoadInfo: + """Output of the HSV asphalt/road mask over the bottom crop of the frame.""" + + coverage: float # fraction of crop flagged as road (0..1) + left: float # road coverage in the left third (0..1) + center: float # road coverage in the centre third (0..1) + right: float # road coverage in the right third (0..1) + mask: Optional[NDArray] = None # binary uint8 mask of the crop (debug only) + + @property + def clearer_side(self) -> int: + """+1 if the right third is clearer (less road), -1 if the left is. + + Use to pick a turn direction that steers *away* from the road. + """ + return 1 if self.right <= self.left else -1 + + +@dataclass +class PerceptionResult: + """Immutable snapshot published by the perception thread each frame.""" + + frame_w: int + frame_h: int + detections: List[Detection] = field(default_factory=list) + persons: List[Detection] = field(default_factory=list) + dangers: List[Detection] = field(default_factory=list) # car/truck/bus/... + road: Optional[RoadInfo] = None + ts: float = 0.0 + seq: int = 0 + frame: Optional[NDArray] = None # BGR frame the result was computed from + + def best_person(self) -> Optional[Detection]: + """Highest-confidence person, or ``None``.""" + return max(self.persons, key=lambda d: d.conf, default=None) + + def biggest_person(self) -> Optional[Detection]: + """Largest (nearest) person by box area, or ``None``.""" + return max(self.persons, key=lambda d: d.area, default=None) + + +# --- convenience ---------------------------------------------------------- + +FrameSize = Tuple[int, int] # (width, height) +Velocity = Tuple[float, float, float] # (vx, vy, vyaw) diff --git a/gowelcome/web/__init__.py b/gowelcome/web/__init__.py new file mode 100644 index 0000000..4c3ff7e --- /dev/null +++ b/gowelcome/web/__init__.py @@ -0,0 +1,5 @@ +"""Optional HTTP/MJPEG camera viewer for GoWelcome.""" + +from gowelcome.web.stream import MjpegServer + +__all__ = ["MjpegServer"] diff --git a/gowelcome/web/stream.py b/gowelcome/web/stream.py new file mode 100644 index 0000000..f873992 --- /dev/null +++ b/gowelcome/web/stream.py @@ -0,0 +1,291 @@ +"""Tiny stdlib MJPEG + control dashboard over HTTP. + +Serves a live JPEG stream (the Go2 camera with GoWelcome's detection/state +overlay) plus an optional control dashboard -- ideal for headless operation on +the dog. Stdlib only; the caller supplies a ``jpeg_provider`` that returns +already-encoded JPEG bytes (so this module needs no cv2), and optionally a +``status_provider`` (-> dict) and ``control_handler`` (dict -> dict) to power +the dashboard panel + buttons. + +Routes: + GET / -> dashboard page (or bare viewer if no controls) + GET /stream.mjpg -> multipart/x-mixed-replace MJPEG stream + GET /snapshot.jpg -> a single current JPEG frame + GET /status.json -> current status dict (if status_provider given) + POST /control -> apply a control command (if control_handler given) + GET /healthz -> "ok" (liveness) +""" + +from __future__ import annotations + +import json +import logging +import threading +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from typing import Callable, Optional + +logger = logging.getLogger(__name__) + +# A callable returning the current frame as encoded JPEG bytes, or None if no +# frame is available yet. +JpegProvider = Callable[[], Optional[bytes]] +StatusProvider = Callable[[], dict] +ControlHandler = Callable[[dict], dict] + +_BOUNDARY = "gowelcomeframe" + + +def _index_html(title: str, controls: bool) -> bytes: + head = ( + "" + "" + f"{title}" + "
" + ) + if not controls: + body = ( + f"

{title} — live camera

" + "camera stream" + ) + else: + body = ( + f"

{title} — control dashboard

" + "camera stream" + "
Play:" + "" + "" + "" + "
" + "
" + "" + "" + "" + "
" + "
" + "" + "" + "
" + "
loading...
" + "" + ) + return (head + body + "
").encode("utf-8") + + +class MjpegServer: + """Serve a JPEG stream over HTTP on a background daemon thread. + + Args: + jpeg_provider: Returns the current frame as JPEG bytes (or ``None``). + host: Bind address (``0.0.0.0`` to allow remote viewers). + port: TCP port. + fps: Maximum stream frame rate (paces the multipart writer). + title: Page/title shown in the browser. + """ + + def __init__( + self, + jpeg_provider: JpegProvider, + host: str = "0.0.0.0", + port: int = 8080, + fps: float = 10.0, + title: str = "GoWelcome", + status_provider: Optional[StatusProvider] = None, + control_handler: Optional[ControlHandler] = None, + ) -> None: + self._provider = jpeg_provider + self._status_provider = status_provider + self._control_handler = control_handler + self._host = host + self._port = port + self._period = 1.0 / max(1e-3, fps) + self._title = title + self._httpd: Optional[ThreadingHTTPServer] = None + self._thread: Optional[threading.Thread] = None + self._stop = threading.Event() + + @property + def has_controls(self) -> bool: + return self._control_handler is not None or self._status_provider is not None + + @property + def port(self) -> int: + """The bound TCP port (resolved after :meth:`start`).""" + return self._port + + # ------------------------------------------------------------------ # + def start(self) -> None: + """Bind the socket and start serving on a daemon thread. + + Raises: + OSError: if the port cannot be bound (e.g. already in use). + """ + server = self + + class Handler(BaseHTTPRequestHandler): + # Per-connection socket timeout so a stalled/non-reading client + # cannot pin a handler thread forever inside wfile.write once the + # TCP send buffer fills: a timed-out write raises socket.timeout, + # caught by _send_stream's except -> the handler exits cleanly. + # (StreamRequestHandler.setup applies this via socket.settimeout.) + timeout = max(10.0, server._period * 4) + + # Silence the default noisy stderr logging. + def log_message(self, fmt, *args): # noqa: N802, D401 + logger.debug("web: " + fmt, *args) + + def do_GET(self): # noqa: N802 + if self.path in ("/", "/index.html"): + self._send_index() + elif self.path.startswith("/stream.mjpg"): + self._send_stream() + elif self.path.startswith("/snapshot.jpg"): + self._send_snapshot() + elif self.path.startswith("/status.json"): + self._send_json(server._status()) + elif self.path.startswith("/healthz"): + self._send_text("ok") + else: + self.send_error(404) + + def do_POST(self): # noqa: N802 + if not self.path.startswith("/control"): + self.send_error(404) + return + try: + n = int(self.headers.get("Content-Length", 0) or 0) + if n > 64 * 1024: # control commands are tiny; reject floods + self.send_error(413, "body too large") + return + raw = self.rfile.read(n) if n > 0 else b"{}" + cmd = json.loads(raw.decode("utf-8") or "{}") + except Exception: + self.send_error(400, "bad JSON") + return + self._send_json(server._control(cmd)) + + def _send_index(self): + body = _index_html(server._title, server.has_controls) + self.send_response(200) + self.send_header("Content-Type", "text/html; charset=utf-8") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def _send_text(self, text): + body = text.encode("utf-8") + self.send_response(200) + self.send_header("Content-Type", "text/plain") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def _send_json(self, obj): + body = json.dumps(obj).encode("utf-8") + self.send_response(200) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def _send_snapshot(self): + jpeg = server._safe_provide() + if jpeg is None: + self.send_error(503, "no frame yet") + return + self.send_response(200) + self.send_header("Content-Type", "image/jpeg") + self.send_header("Content-Length", str(len(jpeg))) + self.end_headers() + self.wfile.write(jpeg) + + def _send_stream(self): + self.send_response(200) + self.send_header("Age", "0") + self.send_header("Cache-Control", "no-cache, private") + self.send_header("Pragma", "no-cache") + self.send_header( + "Content-Type", + f"multipart/x-mixed-replace; boundary={_BOUNDARY}", + ) + self.end_headers() + try: + while not server._stop.is_set(): + jpeg = server._safe_provide() + if jpeg: + self.wfile.write(f"--{_BOUNDARY}\r\n".encode()) + self.send_header("Content-Type", "image/jpeg") + self.send_header("Content-Length", str(len(jpeg))) + self.end_headers() + self.wfile.write(jpeg) + self.wfile.write(b"\r\n") + server._stop.wait(server._period) + except (BrokenPipeError, ConnectionResetError): + logger.debug("web: client disconnected from stream") + except Exception: # noqa: BLE001 - a viewer error must not crash us + logger.debug("web: stream handler error", exc_info=True) + + self._httpd = ThreadingHTTPServer((self._host, self._port), Handler) + self._httpd.daemon_threads = True + # Reflect the actually-bound port (supports port=0 -> ephemeral). + self._port = self._httpd.server_address[1] + self._thread = threading.Thread( + target=self._httpd.serve_forever, name="MjpegServer", daemon=True + ) + self._thread.start() + logger.info( + "MJPEG viewer at http://%s:%d/ (stream: /stream.mjpg)", + "" if self._host in ("0.0.0.0", "") else self._host, + self._port, + ) + + def _safe_provide(self) -> Optional[bytes]: + try: + return self._provider() + except Exception: # noqa: BLE001 + logger.debug("web: jpeg_provider raised", exc_info=True) + return None + + def _status(self) -> dict: + if self._status_provider is None: + return {} + try: + return self._status_provider() + except Exception: # noqa: BLE001 + logger.debug("web: status_provider raised", exc_info=True) + return {"error": "status unavailable"} + + def _control(self, cmd: dict) -> dict: + if self._control_handler is None: + return {"ok": False, "error": "controls disabled"} + try: + return self._control_handler(cmd) or {"ok": True} + except Exception as exc: # noqa: BLE001 + logger.warning("web: control_handler error: %s", exc) + return {"ok": False, "error": str(exc)} + + def stop(self) -> None: + """Stop serving and release the socket. Idempotent.""" + self._stop.set() + if self._httpd is not None: + try: + self._httpd.shutdown() + self._httpd.server_close() + except Exception: # noqa: BLE001 + logger.debug("web: shutdown error", exc_info=True) + self._httpd = None + logger.info("MJPEG viewer stopped") diff --git a/main.py b/main.py new file mode 100755 index 0000000..e0fd8aa --- /dev/null +++ b/main.py @@ -0,0 +1,497 @@ +#!/usr/bin/env python3 +"""GoWelcome entrypoint. + +Wires the perception thread, behaviour state machine, and robot interface +together and runs the fixed-rate control loop. Parses CLI flags into a +:class:`config.GoWelcomeConfig`, then: + + build_robot -> balance_stand -> PerceptionThread.start -> + GoWelcomeStateMachine.step() at ``cfg.loop.rate_hz`` until Ctrl-C. + +Heavy / optional dependencies (``cv2`` for the debug window) are imported +lazily so the module stays importable on a bare machine. Run ``--mock`` for a +webcam/video dry-run with no robot present. + +Examples:: + + python main.py --mock --audio null --source 0 # off-robot + python main.py --interface eth0 # real Go2 +""" + +from __future__ import annotations + +import argparse +import logging +import os +import signal +import sys +import time +from types import FrameType +from typing import Optional + +# Make GoWelcome runnable from ANY working directory: ensure the project root +# (this file's directory) is importable so `import config` / `gowelcome` resolve +# regardless of where `python .../main.py` (or welcome.sh) is invoked from. +_PROJECT_ROOT = os.path.dirname(os.path.abspath(__file__)) +if _PROJECT_ROOT not in sys.path: + sys.path.insert(0, _PROJECT_ROOT) + +from config import GoWelcomeConfig, default_config + +log = logging.getLogger("gowelcome.main") + + +def build_arg_parser() -> argparse.ArgumentParser: + """Construct the CLI parser for GoWelcome.""" + p = argparse.ArgumentParser( + prog="gowelcome", + description="Autonomous Unitree Go2 backyard greeter (mapless, reactive).", + ) + p.add_argument( + "--mock", + action="store_true", + help="Use the MockRobot backend (webcam/video, no hardware).", + ) + p.add_argument( + "--transport", + type=str, + choices=("webrtc", "dds"), + default=None, + help="Robot transport: webrtc (default; app protocol, wifi, dog audio) " + "or dds (official unitree_sdk2py, wired/EDU).", + ) + p.add_argument( + "--interface", + type=str, + default=None, + help="[dds] Host network interface to the Go2 (e.g. eth0, wlan0).", + ) + p.add_argument( + "--robot-ip", + type=str, + default=None, + help="[webrtc] Robot IP for localsta (e.g. 192.168.1.50).", + ) + p.add_argument( + "--serial", + type=str, + default=None, + help="[webrtc] Robot serial number (for discovery / remote).", + ) + p.add_argument( + "--aes-key", + type=str, + default=None, + help="[webrtc] Per-device AES-128 key (32 hex), required on Go2 fw >= 1.1.15.", + ) + p.add_argument( + "--connection", + type=str, + choices=("localsta", "localap", "remote"), + default=None, + help="[webrtc] Connection method (default localsta).", + ) + p.add_argument( + "--audio-method", + type=str, + choices=("audiohub", "stream"), + default=None, + help="[webrtc] Greeting playback: audiohub (upload+play, default) or stream (live).", + ) + p.add_argument( + "--device", + type=str, + default=None, + help="Inference device for YOLO (cpu, cuda, cuda:0).", + ) + p.add_argument( + "--model", + type=str, + default=None, + help="Path to / name of the YOLO weights (ultralytics auto-downloads).", + ) + p.add_argument( + "--source", + type=str, + default=None, + help="Mock frame source: webcam index or video file path.", + ) + p.add_argument( + "--wav", + type=str, + default=None, + help="Path to the greeting .wav clip.", + ) + p.add_argument( + "--audio", + type=str, + choices=("host", "go2", "null"), + default=None, + help="Audio backend: host (default), go2 (experimental), null (silent).", + ) + p.add_argument( + "--audio-device", + type=str, + default=None, + help="Pin a PulseAudio sink for host audio (e.g. a USB/BT speaker on " + "the dog); plays via 'paplay --device='. Empty -> default sink.", + ) + p.add_argument( + "--no-avoidance", + action="store_true", + help="Disable the on-robot LiDAR firmware obstacle-avoidance layer.", + ) + p.add_argument( + "--headless", + action="store_true", + help="Run without the cv2 debug window.", + ) + p.add_argument( + "--dry-run", + action="store_true", + help="Perceive and decide, but never send a non-zero velocity.", + ) + p.add_argument( + "--conf", + type=float, + default=None, + help="Person detection confidence threshold for APPROACH.", + ) + p.add_argument( + "--web", + action="store_true", + help="Serve the live annotated camera as MJPEG over HTTP " + "(open http://:/ in a browser).", + ) + p.add_argument( + "--web-port", + type=int, + default=None, + help="Port for the --web dashboard (default 8080).", + ) + p.add_argument( + "--geofence", + action="store_true", + help="Enable the OPTIONAL GPS keep-in-area geofence (needs an external " + "GPS receiver). Default is vision-only containment (road/car avoidance).", + ) + p.add_argument( + "--gps", + type=str, + choices=("auto", "gpsd", "serial", "mock"), + default=None, + help="GPS source for --geofence (default auto; mock simulates one). " + "Implies --geofence.", + ) + p.add_argument( + "--radius", + type=float, + default=None, + help="Geofence keep-in radius in metres (default 15). Implies --geofence.", + ) + p.add_argument( + "--play", + type=str, + choices=("calm", "moderate", "playful"), + default=None, + help="Idle dog-play intensity (default moderate; changeable via dashboard).", + ) + return p + + +def config_from_args(args: argparse.Namespace) -> GoWelcomeConfig: + """Fold parsed CLI ``args`` into a fresh default config. + + Only flags the user actually supplied override the dataclass defaults, so + ``config.py`` remains the single source of truth for everything else. + """ + cfg = default_config() + + if args.mock: + cfg.mock = True + if args.transport is not None: + cfg.transport = args.transport + if args.interface is not None: + cfg.network.interface = args.interface + if args.robot_ip is not None: + cfg.webrtc.ip = args.robot_ip + if args.serial is not None: + cfg.webrtc.serial_number = args.serial + if args.aes_key is not None: + cfg.webrtc.aes_128_key = args.aes_key + if args.connection is not None: + cfg.webrtc.connection_method = args.connection + if args.audio_method is not None: + cfg.webrtc.audio_method = args.audio_method + if args.device is not None: + cfg.perception.device = args.device + if args.model is not None: + cfg.perception.model_path = args.model + if args.source is not None: + cfg.camera.mock_source = args.source + if args.wav is not None: + cfg.greet.wav_path = args.wav + if args.audio is not None: + cfg.audio.backend = args.audio + if args.audio_device is not None: + cfg.audio.output_device = args.audio_device + if args.no_avoidance: + cfg.safety.use_lidar_avoidance = False + if args.headless: + cfg.headless = True + if args.dry_run: + cfg.dry_run = True + if args.conf is not None: + cfg.perception.person_conf = args.conf + if args.web: + cfg.web.enabled = True + if args.web_port is not None: + cfg.web.port = args.web_port + # GPS geofence is opt-in (default: vision-only). Any of these flags enable it. + if args.geofence or args.gps is not None or args.radius is not None: + cfg.gps.enabled = True + cfg.geofence.enabled = True + if args.gps is not None: + cfg.gps.source = args.gps + if args.radius is not None: + cfg.geofence.radius_m = args.radius + if args.play is not None: + cfg.play.mode = args.play + + return cfg + + +def _draw_overlay(frame, result, state) -> None: + """Annotate ``frame`` in place with detections, road coverage and state. + + Lazily imports ``cv2``. Person boxes are green, danger boxes red. Best + effort only -- any drawing error is swallowed so the loop never dies on a + rendering issue. + """ + import cv2 # lazy: optional + + try: + for det in getattr(result, "persons", []) or []: + cv2.rectangle( + frame, (int(det.x1), int(det.y1)), (int(det.x2), int(det.y2)), + (0, 255, 0), 2, + ) + cv2.putText( + frame, f"person {det.conf:.2f}", (int(det.x1), max(0, int(det.y1) - 5)), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA, + ) + for det in getattr(result, "dangers", []) or []: + cv2.rectangle( + frame, (int(det.x1), int(det.y1)), (int(det.x2), int(det.y2)), + (0, 0, 255), 2, + ) + cv2.putText( + frame, f"{det.label} {det.conf:.2f}", + (int(det.x1), max(0, int(det.y1) - 5)), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA, + ) + + road = getattr(result, "road", None) + coverage = getattr(road, "coverage", 0.0) if road is not None else 0.0 + cv2.putText( + frame, f"road: {coverage:.0%}", (10, 22), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2, cv2.LINE_AA, + ) + state_name = getattr(state, "name", str(state)) + cv2.putText( + frame, f"state: {state_name}", (10, 46), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA, + ) + except Exception: # pragma: no cover - drawing must never crash the loop + log.debug("overlay drawing failed", exc_info=True) + + +def run(cfg: GoWelcomeConfig) -> int: + """Run the GoWelcome control loop until interrupted. Returns an exit code.""" + # Imports deferred until after CLI parsing so ``--help`` works without the + # full (heavy) dependency tree being importable. + from gowelcome.perception import PerceptionThread + from gowelcome.robot.factory import build_robot + from gowelcome.statemachine import GoWelcomeStateMachine + from gowelcome.geo import build_gps_source + + log.info("GoWelcome starting (mock=%s, dry_run=%s, headless=%s)", + cfg.mock, cfg.dry_run, cfg.headless) + + # Pre-bind so the finally block can clean up whatever was acquired, even if + # setup raises partway through (never leave the dog standing with avoidance + # claimed and no shutdown). + robot = None + perception = None + gps_source = None + web_server = None + window_ready = False + + stop_flag = {"stop": False} + + def _handle_signal(signum: int, _frame: Optional[FrameType]) -> None: + # Only set the flag here; the actual shutdown happens in the finally + # block of the main loop (signal handlers must stay minimal). + log.warning("signal %s received -> stopping", signum) + stop_flag["stop"] = True + + # Register handlers up front (only possible on the main thread). + try: + signal.signal(signal.SIGINT, _handle_signal) + signal.signal(signal.SIGTERM, _handle_signal) + except ValueError: # pragma: no cover - run() invoked off the main thread + log.warning("signal handlers not installed (not on the main thread)") + + try: + # --- resource acquisition (inside the try so finally always runs) --- + robot = build_robot(cfg) + perception = PerceptionThread(robot, cfg) + perception.start() + + # GPS geofence (optional): build + start the source before the SM. + gps_source = build_gps_source(cfg) + if gps_source is not None: + gps_source.start() + log.info("GPS geofence active (source=%s, radius=%.1fm)", + cfg.gps.source, cfg.geofence.radius_m) + + robot.balance_stand() + sm = GoWelcomeStateMachine(robot, perception, cfg, gps_source=gps_source) + + def _control(cmd): + action = (cmd or {}).get("action") + if action == "play_mode": + ok = sm.set_play_mode(cmd.get("mode", "")) + return {"ok": ok, "play_mode": sm.play_mode()} + if action == "pause": + sm.set_paused(bool(cmd.get("paused", True))) + return {"ok": True} + if action == "resume": + sm.set_paused(False) + return {"ok": True} + if action == "estop": + sm.request_estop() + return {"ok": True} + if action == "clear_estop": + sm.clear_estop() + return {"ok": True} + if action == "set_center": + return {"ok": sm.set_geofence_center()} + return {"ok": False, "error": f"unknown action {action!r}"} + + # Optional MJPEG web viewer: stream the latest annotated frame over HTTP. + if cfg.web.enabled: + def _jpeg_provider(): + result = perception.latest() + frame = getattr(result, "frame", None) if result is not None else None + if frame is None: + return None + try: + import cv2 # lazy: optional + canvas = frame.copy() + _draw_overlay(canvas, result, sm.state) + ok, buf = cv2.imencode( + ".jpg", canvas, + [cv2.IMWRITE_JPEG_QUALITY, int(cfg.web.jpeg_quality)], + ) + return buf.tobytes() if ok else None + except Exception: # pragma: no cover - encode/display failure + return None + + try: + from gowelcome.web import MjpegServer + web_server = MjpegServer( + _jpeg_provider, host=cfg.web.host, port=cfg.web.port, + fps=cfg.web.fps, title="GoWelcome", + status_provider=sm.status, control_handler=_control, + ) + web_server.start() + log.info("dashboard: open http://:%d/ in a browser", cfg.web.port) + except Exception: + log.exception("failed to start dashboard; continuing without it") + web_server = None + + rate = max(1e-3, float(cfg.loop.rate_hz)) + period = 1.0 / rate + show_window = (not cfg.headless) + + last = time.monotonic() + while not stop_flag["stop"]: + now = time.monotonic() + dt = now - last + last = now + + state = sm.step(dt) + + if show_window: + try: + result = perception.latest() + frame = getattr(result, "frame", None) if result is not None else None + if frame is not None: + import cv2 # lazy: optional + canvas = frame.copy() + _draw_overlay(canvas, result, state) + cv2.imshow("GoWelcome", canvas) + window_ready = True + if (cv2.waitKey(1) & 0xFF) == 27: # ESC + log.info("ESC pressed -> stopping") + stop_flag["stop"] = True + except Exception: # pragma: no cover - no display / no cv2 + log.info("debug window unavailable; continuing headless", + exc_info=True) + show_window = False + + # Sleep the remainder of the loop period. + elapsed = time.monotonic() - now + remaining = period - elapsed + if remaining > 0: + time.sleep(remaining) + except KeyboardInterrupt: # pragma: no cover - belt-and-braces + log.warning("KeyboardInterrupt -> stopping") + finally: + log.info("shutting down") + if web_server is not None: + try: + web_server.stop() + except Exception: # pragma: no cover + log.exception("web_server.stop() failed") + if perception is not None: + try: + perception.stop() + except Exception: # pragma: no cover + log.exception("perception.stop() failed") + if gps_source is not None: + try: + gps_source.stop() + except Exception: # pragma: no cover + log.exception("gps_source.stop() failed") + if robot is not None: + try: + robot.shutdown() + except Exception: # pragma: no cover + log.exception("robot.shutdown() failed") + if window_ready: + try: + import cv2 # lazy: optional + cv2.destroyAllWindows() + except Exception: # pragma: no cover + pass + + log.info("GoWelcome stopped") + return 0 + + +def main(argv: Optional[list[str]] = None) -> int: + """Parse args, configure logging, and run. Returns a process exit code.""" + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)-7s %(name)s: %(message)s", + datefmt="%H:%M:%S", + ) + args = build_arg_parser().parse_args(argv) + cfg = config_from_args(args) + return run(cfg) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pytest.ini b/pytest.ini new file mode 100644 index 0000000..7235cd2 --- /dev/null +++ b/pytest.ini @@ -0,0 +1,6 @@ +[pytest] +# Only collect the unit-test suite. This keeps pytest away from +# scripts/smoke_test.py (an executable end-to-end script, not a unit test, +# whose name would otherwise match the default *_test.py pattern). +testpaths = tests +python_files = test_*.py diff --git a/requirement.sh b/requirement.sh new file mode 100755 index 0000000..97e877c --- /dev/null +++ b/requirement.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +# Create/refresh the 'welcome' conda env and install GoWelcome's Python deps. +# +# ./requirement.sh +# +# Installs the off-robot / mock dependencies (numpy, opencv, ultralytics, +# simpleaudio). The real Go2 additionally needs the Unitree SDK (hardware only) +# -- see the note printed at the end. +set -euo pipefail + +ENV_NAME="welcome" +PY_VERSION="3.10" +# Resolve this script's own directory so it works from any CWD. +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +# --- locate conda -------------------------------------------------------- +if ! command -v conda >/dev/null 2>&1; then + for c in "$HOME/miniconda3" "$HOME/anaconda3" "$HOME/miniforge3" "/opt/conda"; do + if [ -f "$c/etc/profile.d/conda.sh" ]; then + # shellcheck disable=SC1091 + source "$c/etc/profile.d/conda.sh" + break + fi + done +fi +command -v conda >/dev/null 2>&1 || { + echo "ERROR: conda not found. Install Miniconda/Anaconda first." >&2 + exit 1 +} +# shellcheck disable=SC1091 +source "$(conda info --base)/etc/profile.d/conda.sh" + +# --- create env if missing ---------------------------------------------- +if conda env list | awk '{print $1}' | grep -qx "$ENV_NAME"; then + echo "[requirement] conda env '$ENV_NAME' already exists -- reusing it." +else + echo "[requirement] creating conda env '$ENV_NAME' (python $PY_VERSION)..." + conda create -y -n "$ENV_NAME" "python=$PY_VERSION" +fi + +conda activate "$ENV_NAME" +echo "[requirement] using $(python --version) @ $(command -v python)" + +# --- install python deps ------------------------------------------------- +python -m pip install --upgrade pip +python -m pip install -r "$SCRIPT_DIR/requirements.txt" + +echo +echo "[requirement] done. Off-robot/mock deps installed in conda env '$ENV_NAME'." +echo "[requirement] For a REAL Go2, also install the Unitree SDK (hardware only):" +echo " git clone https://github.com/unitreerobotics/unitree_sdk2_python" +echo " cd unitree_sdk2_python && pip install -e . # pulls in cyclonedds" +echo +echo "[requirement] Verify the install (real YOLO, no robot needed):" +echo " python \"$SCRIPT_DIR/scripts/smoke_test.py\"" +echo +echo "[requirement] Start GoWelcome with:" +echo " ./welcome.sh # mock / webcam (no robot)" +echo " ./welcome.sh --interface eth0 # real Go2 over eth0" diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..8a35076 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,36 @@ +# GoWelcome runtime dependencies. +# +# Install for off-robot / mock development: +# pip install -r requirements.txt +# (The HARDWARE-ONLY block at the bottom is NOT needed for `python main.py --mock`.) + +# --- Perception ---------------------------------------------------------- +numpy # arrays / math (light, imported at top level everywhere) +opencv-python # camera capture, HSV road mask, debug overlay window (cv2) +ultralytics # YOLOv8 person/vehicle detection + +# --- Audio (optional) ---------------------------------------------------- +# Host-side greeting playback. If absent, the host audio backend falls back to +# an external player command (config.AudioConfig.host_player_cmd, e.g. `aplay`). +simpleaudio # optional; pure-host WAV playback + +# --- GPS geofence (external receiver on the onboard computer) ------------- +# The Go2 has NO built-in GPS. For `--gps serial` (NMEA off a USB receiver): +pyserial # optional; only for the serial NMEA GPS source +# For `--gps gpsd`, install the system gpsd daemon instead (sudo apt install gpsd); +# GoWelcome talks to it over its socket with no extra Python dependency. + +# --- HARDWARE: WebRTC transport (DEFAULT; real Go2, skip for --mock) ------ +# App-protocol WebRTC driver: camera + sport + obstacle-avoid + AudioHub audio, +# works on Go2 AIR/PRO/EDU over wifi. Pulls in aiortc. Also needs PortAudio: +# sudo apt install -y portaudio19-dev +unitree_webrtc_connect # default transport (--transport webrtc) + +# --- HARDWARE: DDS transport (alternative; --transport dds) --------------- +# Official Unitree SDK + DDS. NOT on PyPI in a usable form -- install from the +# official repo on the robot's host (pulls cyclonedds): +# https://github.com/unitreerobotics/unitree_sdk2_python +# unitree_sdk2py # SportClient / VideoClient / ObstaclesAvoidClient +# cyclonedds # DDS transport used by unitree_sdk2py +# +# NOTE: pyserial is NOT required by GoWelcome (no direct serial link to the Go2). diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 0000000..9172b20 --- /dev/null +++ b/ruff.toml @@ -0,0 +1,7 @@ +# Ruff config for GoWelcome. +# main.py and the scripts insert the project root onto sys.path *before* +# importing project modules so GoWelcome runs from any working directory; that +# legitimately places those imports after code (E402), so ignore it there only. +[lint.per-file-ignores] +"main.py" = ["E402"] +"scripts/*.py" = ["E402"] diff --git a/scripts/run_mock.sh b/scripts/run_mock.sh new file mode 100755 index 0000000..50d4d1a --- /dev/null +++ b/scripts/run_mock.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail +# +# run_mock.sh -- off-robot GoWelcome run. +# +# Uses the MockRobot backend (webcam index 0 by default), silent audio, and no +# hardware. Any extra arguments are forwarded to main.py, e.g.: +# +# ./scripts/run_mock.sh --source demo.mp4 --headless +# ./scripts/run_mock.sh --device cuda --conf 0.6 +# +# Resolve the repo root from this script's location so it runs from anywhere. +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)" +cd "${PROJECT_ROOT}" + +exec python3 main.py --mock --audio null --source 0 "$@" diff --git a/scripts/run_robot.sh b/scripts/run_robot.sh new file mode 100755 index 0000000..bc1f70a --- /dev/null +++ b/scripts/run_robot.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +set -euo pipefail +# +# run_robot.sh -- run GoWelcome against a REAL Unitree Go2. +# +# !!! SAFETY: ALWAYS test the robot SUSPENDED (hung off the ground) first, with +# !!! a hand on Ctrl-C. Ctrl-C is the e-stop: it stops motion and shuts down. +# +# Usage: +# ./scripts/run_robot.sh [INTERFACE] [extra main.py args...] +# +# The first positional argument is the host network interface to the Go2 +# (default: eth0). Anything after it is forwarded straight to main.py, e.g.: +# +# ./scripts/run_robot.sh eth0 --no-avoidance --headless +# ./scripts/run_robot.sh wlan0 --device cuda +# +# Resolve the repo root from this script's location so it runs from anywhere. +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)" +cd "${PROJECT_ROOT}" + +INTERFACE="${1:-eth0}" +# Drop the interface from $@ if it was supplied, so the rest are passthrough. +if [[ "$#" -gt 0 ]]; then + shift +fi + +exec python3 main.py --interface "${INTERFACE}" "$@" diff --git a/scripts/smoke_test.py b/scripts/smoke_test.py new file mode 100644 index 0000000..ed6eae3 --- /dev/null +++ b/scripts/smoke_test.py @@ -0,0 +1,185 @@ +"""End-to-end off-robot smoke test for GoWelcome (real YOLO + cv2 + threads). + +Run AFTER ``./requirement.sh`` (needs ultralytics + opencv installed), with NO +robot attached. It exercises the genuine perception + control stack on real +images so you catch any dependency/logic problem on the bench, not on the dog: + + conda activate welcome # or: source the env however you like + python scripts/smoke_test.py + +Checks: + 1. YoloDetector on a people image and a vehicle image (bundled with ultralytics). + 2. RoadDetector HSV mask: flags asphalt-grey, ignores saturated grass. + 3. PerceptionThread + GoWelcomeStateMachine: a detected person drives + WANDER -> APPROACH/GREET; a detected vehicle forces AVOID_DANGER. + 4. build_robot factory builds MockRobot; NullAudio + shutdown are clean. + +Exits non-zero on the first failed assertion. +""" + +from __future__ import annotations + +import logging +import os +import sys +import time + +# Dynamic: project root is this file's parent dir's parent (scripts/..). +_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +if _ROOT not in sys.path: + sys.path.insert(0, _ROOT) + +logging.basicConfig(level=logging.WARNING, format="%(levelname)s %(name)s: %(message)s") + +import numpy as np + +try: + import cv2 + from ultralytics.utils import ASSETS +except ImportError as exc: # pragma: no cover + print(f"ERROR: smoke test needs opencv + ultralytics installed ({exc}).") + print("Run ./requirement.sh first, then activate the 'welcome' env.") + sys.exit(2) + +from config import default_config +from gowelcome.perception.detector import YoloDetector +from gowelcome.perception.road_mask import RoadDetector +from gowelcome.perception.vision_thread import PerceptionThread +from gowelcome.statemachine import GoWelcomeStateMachine +from gowelcome.robot.interface import RobotInterface +from gowelcome.robot.factory import build_robot + + +def banner(t: str) -> None: + print(f"\n===== {t} =====") + + +class _StubRobot(RobotInterface): + """Serves one fixed frame; records commands. No hardware.""" + + def __init__(self, frame): + self._f = frame + self.drives, self.gestures, self.greets = [], [], 0 + + def get_frame(self): + return self._f + + def frame_size(self): + return (self._f.shape[1], self._f.shape[0]) + + def drive(self, vx, vy, vyaw): + self.drives.append((vx, vy, vyaw)) + + def stop(self): + pass + + def set_avoidance(self, on): + pass + + def balance_stand(self): + pass + + def stand_up(self): + pass + + def damp(self): + pass + + def gesture(self, name): + self.gestures.append(name) + + def play_greeting(self): + self.greets += 1 + + def shutdown(self): + pass + + +def main() -> int: + cfg = default_config() + cfg.mock = True + cfg.audio.backend = "null" + cfg.perception.device = "cpu" + w, h = cfg.camera.width, cfg.camera.height + + # ---------------------------------------------------------- 1. YOLO + banner("1. YoloDetector on real images") + det = YoloDetector(cfg.perception) + + zid = cv2.resize(cv2.imread(str(ASSETS / "zidane.jpg")), (w, h)) + zdets = det.detect(zid) + print("zidane.jpg ->", [(d.label, round(d.conf, 2)) for d in zdets]) + assert any(d.label == "person" for d in zdets), "expected person(s) in zidane.jpg" + + bus = cv2.resize(cv2.imread(str(ASSETS / "bus.jpg")), (w, h)) + bdets = det.detect(bus) + blabels = {d.label for d in bdets} + print("bus.jpg ->", [(d.label, round(d.conf, 2)) for d in bdets]) + assert "person" in blabels, "expected person in bus.jpg" + assert "bus" in blabels, "expected bus (danger class) in bus.jpg" + print("OK: real YOLO detects persons and a vehicle") + + # ---------------------------------------------------------- 2. ROAD + banner("2. RoadDetector HSV mask") + grey = np.full((h, w, 3), 95, np.uint8) + ri = RoadDetector(cfg.perception).analyze(grey) + print(f"grey -> coverage={ri.coverage:.2f} L/C/R={ri.left:.2f}/{ri.center:.2f}/{ri.right:.2f}") + assert ri.coverage > 0.5, "asphalt-grey frame should read as mostly road" + green = np.zeros((h, w, 3), np.uint8) + green[:] = (40, 200, 40) + rg = RoadDetector(cfg.perception).analyze(green) + print(f"green -> coverage={rg.coverage:.2f}") + assert rg.coverage < 0.2, "saturated green should not read as road" + print("OK: road mask flags asphalt, ignores grass") + + # ---------------------------------------------------------- 3. PIPELINE + banner("3. PerceptionThread + StateMachine (person -> APPROACH/GREET)") + cfg.perception.road_enabled = False + cfg.perception.person_conf = 0.5 + robot = _StubRobot(zid) + pt = PerceptionThread(robot, cfg) + pt.start() + time.sleep(2.0) + res = pt.latest() + print("perception ->", "persons:", len(res.persons), "dangers:", len(res.dangers)) + assert res and res.persons, "perception thread should report a person" + sm = GoWelcomeStateMachine(robot, pt, cfg) + seq = [sm.step(1 / 15).value for _ in range(6)] + pt.stop() + print("states:", seq) + assert any(s in ("APPROACH", "GREET") for s in seq), "should approach/greet a person" + print("OK: perception->decision reacts to a real detection") + + # ---------------------------------------------------------- 3b. DANGER + banner("3b. Danger override (vehicle -> AVOID_DANGER)") + robot2 = _StubRobot(bus) + pt2 = PerceptionThread(robot2, cfg) + pt2.start() + time.sleep(2.0) + res2 = pt2.latest() + print("perception ->", "persons:", len(res2.persons), "dangers:", len(res2.dangers)) + sm2 = GoWelcomeStateMachine(robot2, pt2, cfg) + seq2 = [sm2.step(1 / 15).value for _ in range(4)] + pt2.stop() + print("states:", seq2) + assert res2.dangers, "the bus should be classified as a danger" + assert "AVOID_DANGER" in seq2, "a nearby vehicle must force AVOID_DANGER" + print("OK: vehicle triggers the safety escape state") + + # ---------------------------------------------------------- 4. FACTORY + banner("4. build_robot factory + NullAudio") + bot = build_robot(cfg) + print("build_robot ->", type(bot).__name__) + assert type(bot).__name__ == "MockRobot" + bot.play_greeting() + bot.shutdown() + print("OK: factory builds MockRobot; null audio + shutdown clean") + + print("\n" + "=" * 40) + print("ALL SMOKE TESTS PASSED") + print("=" * 40) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e9b53ef --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1,6 @@ +"""GoWelcome off-robot unit tests. + +These tests import only the *light* modules (``config``, ``gowelcome.types``, +``gowelcome.control.*``, ``gowelcome.statemachine``) and never touch cv2, +ultralytics, the Unitree SDK, or any hardware -- so they run in plain CI. +""" diff --git a/tests/test_dashboard.py b/tests/test_dashboard.py new file mode 100644 index 0000000..4042377 --- /dev/null +++ b/tests/test_dashboard.py @@ -0,0 +1,108 @@ +"""Dashboard (status + control) tests for the web server. Stdlib only.""" + +from __future__ import annotations + +import json +import time +import urllib.request + +import pytest + +from gowelcome.web import MjpegServer + +_FAKE_JPEG = bytes.fromhex("ffd8ffe000104a46494600010100000100010000ffd9") + + +@pytest.fixture() +def dash(): + state = {"play_mode": "moderate", "paused": False} + + def status(): + return {"state": "WANDER", "play_mode": state["play_mode"], + "paused": state["paused"]} + + def control(cmd): + action = cmd.get("action") + if action == "play_mode": + state["play_mode"] = cmd.get("mode") + return {"ok": True, "play_mode": state["play_mode"]} + if action == "pause": + state["paused"] = True + return {"ok": True} + return {"ok": False, "error": "unknown"} + + srv = MjpegServer(lambda: _FAKE_JPEG, host="127.0.0.1", port=0, fps=30, + status_provider=status, control_handler=control) + srv.start() + time.sleep(0.2) + try: + yield srv, state + finally: + srv.stop() + + +def _get(port, path): + r = urllib.request.urlopen(f"http://127.0.0.1:{port}{path}", timeout=3) + return r.status, r.read() + + +def _post(port, path, obj): + data = json.dumps(obj).encode() + req = urllib.request.Request(f"http://127.0.0.1:{port}{path}", data=data, + headers={"Content-Type": "application/json"}) + r = urllib.request.urlopen(req, timeout=3) + return r.status, json.loads(r.read()) + + +def test_index_has_controls(dash): + srv, _ = dash + assert srv.has_controls + _s, body = _get(srv.port, "/") + assert b"control dashboard" in body + assert b"/control" in body and b"E-STOP" in body + + +def test_status_json(dash): + srv, _ = dash + _s, body = _get(srv.port, "/status.json") + obj = json.loads(body) + assert obj["state"] == "WANDER" + assert obj["play_mode"] == "moderate" + + +def test_post_control_play_mode(dash): + srv, state = dash + status, resp = _post(srv.port, "/control", {"action": "play_mode", "mode": "playful"}) + assert status == 200 and resp["ok"] is True + assert state["play_mode"] == "playful" + # reflected in subsequent status + _s, body = _get(srv.port, "/status.json") + assert json.loads(body)["play_mode"] == "playful" + + +def test_post_control_pause(dash): + srv, state = dash + _s, resp = _post(srv.port, "/control", {"action": "pause"}) + assert resp["ok"] is True + assert state["paused"] is True + + +def test_post_unknown_action(dash): + srv, _ = dash + _s, resp = _post(srv.port, "/control", {"action": "nope"}) + assert resp["ok"] is False + + +def test_viewer_only_has_no_controls(): + srv = MjpegServer(lambda: _FAKE_JPEG, host="127.0.0.1", port=0) + srv.start() + time.sleep(0.2) + try: + assert not srv.has_controls + _s, body = _get(srv.port, "/") + assert b"live camera" in body + # control endpoints disabled + _s2, resp = _post(srv.port, "/control", {"action": "estop"}) + assert resp["ok"] is False + finally: + srv.stop() diff --git a/tests/test_factory_transport.py b/tests/test_factory_transport.py new file mode 100644 index 0000000..a47ed4d --- /dev/null +++ b/tests/test_factory_transport.py @@ -0,0 +1,81 @@ +"""Transport selection + CLI wiring tests (no robot SDKs / hardware needed). + +These assert the *routing* and *config mapping* logic. The real backends raise +ImportError at construction when their heavy deps are absent (expected here), so +we assert that path rather than a successful connection. +""" + +from __future__ import annotations + +import pytest + +from config import default_config +from gowelcome.robot.factory import build_robot +from main import build_arg_parser, config_from_args + + +def _cfg(**over): + cfg = default_config() + for k, v in over.items(): + setattr(cfg, k, v) + return cfg + + +def test_default_transport_is_webrtc(): + assert default_config().transport == "webrtc" + + +def test_unknown_transport_raises_value_error(): + with pytest.raises(ValueError): + build_robot(_cfg(mock=False, transport="bogus")) + + +def test_webrtc_backend_requires_library(): + # unitree_webrtc_connect is not installed in the test env -> clear ImportError. + with pytest.raises(ImportError): + build_robot(_cfg(mock=False, transport="webrtc")) + + +def test_dds_backend_requires_sdk(): + with pytest.raises(ImportError): + build_robot(_cfg(mock=False, transport="dds")) + + +def test_cli_maps_webrtc_flags(): + args = build_arg_parser().parse_args([ + "--transport", "webrtc", + "--robot-ip", "192.168.1.50", + "--serial", "B42D2000XX", + "--aes-key", "00112233445566778899aabbccddeeff", + "--connection", "localsta", + "--audio-method", "stream", + ]) + cfg = config_from_args(args) + assert cfg.transport == "webrtc" + assert cfg.webrtc.ip == "192.168.1.50" + assert cfg.webrtc.serial_number == "B42D2000XX" + assert cfg.webrtc.aes_128_key == "00112233445566778899aabbccddeeff" + assert cfg.webrtc.connection_method == "localsta" + assert cfg.webrtc.audio_method == "stream" + + +def test_cli_maps_dds_flags(): + args = build_arg_parser().parse_args(["--transport", "dds", "--interface", "eth0"]) + cfg = config_from_args(args) + assert cfg.transport == "dds" + assert cfg.network.interface == "eth0" + + +def test_webrtc_localsta_without_ip_or_serial_errors(monkeypatch): + """If the library *were* importable, localsta with no ip/serial must raise + ValueError. We simulate by checking the connection-builder guard directly.""" + # Import lazily; skip cleanly if the (heavy) library happens to be installed, + # since then construction would proceed past the guard we want to assert. + import importlib.util + if importlib.util.find_spec("unitree_webrtc_connect") is not None: + pytest.skip("unitree_webrtc_connect installed; guard path not isolated here") + # Without the library, construction raises ImportError before the ValueError + # guard -- which is the correct, safe behaviour off-robot. + with pytest.raises(ImportError): + build_robot(_cfg(mock=False, transport="webrtc", + webrtc=default_config().webrtc)) diff --git a/tests/test_geo_statemachine.py b/tests/test_geo_statemachine.py new file mode 100644 index 0000000..e5b2883 --- /dev/null +++ b/tests/test_geo_statemachine.py @@ -0,0 +1,280 @@ +"""State-machine behaviour with the GPS geofence, dog-play, and dashboard controls. + +Driven off-robot with a recording fake robot, a scripted fake GPS, and a fake +perception source. Stdlib only. +""" + +from __future__ import annotations + +import time +from typing import List, Optional, Tuple + +from config import default_config +from gowelcome.geo.gps import GpsFix +from gowelcome.statemachine import GoWelcomeStateMachine +from gowelcome.types import Detection, PerceptionResult, RoadInfo, State +from gowelcome.robot.interface import RobotInterface + +FRAME_W, FRAME_H = 640, 480 +DT = 1.0 / 15.0 + + +class FakeRobot(RobotInterface): + def __init__(self) -> None: + self.drives: List[Tuple[float, float, float]] = [] + self.stops = 0 + self.gestures: List[str] = [] + self.greetings = 0 + self.balance_stands = 0 + self.damps = 0 + + def get_frame(self): + return None + + def frame_size(self): + return (FRAME_W, FRAME_H) + + def drive(self, vx, vy, vyaw): + self.drives.append((vx, vy, vyaw)) + + def stop(self): + self.stops += 1 + + def set_avoidance(self, on): + pass + + def balance_stand(self): + self.balance_stands += 1 + + def stand_up(self): + pass + + def damp(self): + self.damps += 1 + + def gesture(self, name): + self.gestures.append(name) + + def play_greeting(self): + self.greetings += 1 + + def shutdown(self): + pass + + +class FakeGps: + """Returns a settable fix; records commanded motion.""" + + def __init__(self, fix: Optional[GpsFix] = None) -> None: + self._fix = fix + self.commands: List[Tuple[float, float, float]] = [] + + def set_fix(self, fix: Optional[GpsFix]) -> None: + self._fix = fix + + def latest(self) -> Optional[GpsFix]: + return self._fix + + def on_command(self, vx, vy, vyaw): + self.commands.append((vx, vy, vyaw)) + + +class FakePerception: + def __init__(self, result: PerceptionResult) -> None: + self._result = result + + def latest(self) -> PerceptionResult: + self._result.ts = time.monotonic() + return self._result + + +def _empty_perc() -> PerceptionResult: + return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[], dangers=[], + road=None, ts=time.monotonic(), seq=1) + + +def _fix(lat, lon, course=None) -> GpsFix: + return GpsFix(lat=lat, lon=lon, ts=time.monotonic(), course_deg=course, + speed_mps=0.3, num_sats=10, hdop=0.8) + + +# --------------------------------------------------------------------------- # +# Geofence +# --------------------------------------------------------------------------- # +def test_breach_enters_boundary_and_homes_forward(): + cfg = default_config() + cfg.geofence.enabled = True + cfg.geofence.center_mode = "onstart" + robot = FakeRobot() + gps = FakeGps(_fix(0.0, 0.0, course=0.0)) # at centre + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps) + + sm.step(DT) # captures centre, stays inside + assert sm.state is State.WANDER + + gps.set_fix(_fix(0.0, 0.001, course=90.0)) # ~111 m east -> way outside + state = sm.step(DT) + assert state is State.BOUNDARY + vx, _vy, vyaw = robot.drives[-1] + assert vx > 0.0 # driving back toward centre + assert abs(vyaw) > 0.0 # steering toward centre + + +def test_boundary_low_speed_ignores_garbage_course(): + """Near-stationary GPS course is noise -> homing must use the safe gentle + re-acquire turn, not steer on the (possibly wrong-way) reported course.""" + import pytest + cfg = default_config() + cfg.geofence.enabled = True + cfg.geofence.center_mode = "onstart" # min_speed_for_course default 0.15 + robot = FakeRobot() + gps = FakeGps(_fix(0.0, 0.0, course=0.0)) + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps) + sm.step(DT) # capture centre + # Breached + near-stationary + a course pointing the WRONG way (east, away). + gps.set_fix(GpsFix(lat=0.0, lon=0.001, ts=time.monotonic(), course_deg=90.0, + speed_mps=0.0, num_sats=10, hdop=0.8)) + sm.step(DT) + assert sm.state is State.BOUNDARY + vx, _vy, vyaw = robot.drives[-1] + assert vx > 0.0 + # course gated out -> gentle re-acquire turn (~+0.4), NOT the garbage hard turn. + assert vyaw == pytest.approx(0.4, abs=1e-6) + + +def test_gps_lost_holds(): + cfg = default_config() + cfg.geofence.enabled = True + cfg.geofence.no_fix_behavior = "stop" + robot = FakeRobot() + gps = FakeGps(_fix(0.0, 0.0)) + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps) + sm.step(DT) # capture centre + drives_before = len(robot.drives) + gps.set_fix(None) # GPS lost + sm.step(DT) + assert robot.stops >= 1 + assert len(robot.drives) == drives_before # never drove blind + + +def test_no_center_holds_until_first_fix(): + cfg = default_config() + cfg.geofence.enabled = True + robot = FakeRobot() + gps = FakeGps(None) # never a fix + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps) + sm.step(DT) + assert robot.stops >= 1 + assert not robot.drives + + +def test_geofence_disabled_when_no_gps_source(): + cfg = default_config() + robot = FakeRobot() + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None) + sm.step(DT) + assert sm.state is State.WANDER + assert robot.drives and robot.drives[-1][0] > 0.0 # roams normally + + +# --------------------------------------------------------------------------- # +# Vision-only containment (default: no GPS) -- stay away from road + cars +# --------------------------------------------------------------------------- # +def _perc(road=None, dets=None) -> PerceptionResult: + dets = dets or [] + dangers = [d for d in dets if d.height_ratio(FRAME_H) >= 0.25 and d.label != "person"] + return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[], + dangers=dangers, detections=dets, road=road, + ts=time.monotonic(), seq=1) + + +def test_default_is_vision_only_no_gps_hold(): + """Default config has no GPS -> the dog roams (vision-only), never holds.""" + cfg = default_config() + assert cfg.geofence.enabled is False and cfg.gps.enabled is False + robot = FakeRobot() + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None) + sm.step(DT) + assert sm.state is State.WANDER + assert robot.drives and robot.drives[-1][0] > 0.0 # cruising, not holding + + +def test_road_repulsion_steers_away_and_slows(): + """Soft band road on the LEFT -> veer RIGHT (vyaw<0) and slow down.""" + cfg = default_config() + cfg.wander.yaw_sweep_rate = 0.0 # isolate the repulsion term + robot = FakeRobot() + # road heavier on the left, centre below the hard AVOID trigger (0.35) + road = RoadInfo(coverage=0.2, left=0.30, center=0.2, right=0.03) + sm = GoWelcomeStateMachine(robot, FakePerception(_perc(road=road)), cfg) + sm.step(DT) + assert sm.state is State.WANDER # soft band -> not hard AVOID_DANGER + vx, _vy, vyaw = robot.drives[-1] + assert vyaw < 0.0 # turn right, away from the left road + assert 0.0 < vx < cfg.wander.forward_speed # slowed but still moving + + +def test_car_repulsion_steers_away(): + """A (far/small) car on the RIGHT -> veer LEFT (vyaw>0) away from it.""" + cfg = default_config() + cfg.wander.yaw_sweep_rate = 0.0 + robot = FakeRobot() + car = Detection(label="car", conf=0.5, x1=520, y1=210, x2=600, y2=260) # right, small + sm = GoWelcomeStateMachine(robot, FakePerception(_perc(dets=[car])), cfg) + sm.step(DT) + assert sm.state is State.WANDER # small/far car -> soft steer, not hard avoid + vx, _vy, vyaw = robot.drives[-1] + assert vyaw > 0.0 # turn left, away from the right-side car + + +# --------------------------------------------------------------------------- # +# Dog-play + dashboard controls +# --------------------------------------------------------------------------- # +def test_play_fires_in_wander(): + cfg = default_config() + cfg.play.moderate_interval = 0.0 + cfg.play.jitter = 0.0 + robot = FakeRobot() + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None) + sm.step(DT) + assert sm.state is State.WANDER + assert robot.gestures, "a dog-play gesture should have fired" + assert robot.gestures[-1] in cfg.play.actions + + +def test_set_play_mode_and_status(): + cfg = default_config() + sm = GoWelcomeStateMachine(FakeRobot(), FakePerception(_empty_perc()), cfg) + assert sm.play_mode() == "moderate" + assert sm.set_play_mode("playful") is True + assert sm.play_mode() == "playful" + assert sm.set_play_mode("bogus") is False + s = sm.status() + assert s["play_mode"] == "playful" + assert s["state"] in {st.value for st in State} + + +def test_pause_stops_robot(): + cfg = default_config() + robot = FakeRobot() + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg) + sm.set_paused(True) + sm.step(DT) + assert robot.stops >= 1 and not robot.drives + sm.set_paused(False) + sm.step(DT) + assert robot.drives # resumes + + +def test_estop_damps_and_stops(): + cfg = default_config() + robot = FakeRobot() + sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg) + sm.request_estop() + sm.step(DT) + assert robot.damps >= 1 + assert robot.stops >= 1 + assert not robot.drives + sm.clear_estop() + sm.step(DT) + assert robot.drives diff --git a/tests/test_geofence.py b/tests/test_geofence.py new file mode 100644 index 0000000..6571826 --- /dev/null +++ b/tests/test_geofence.py @@ -0,0 +1,96 @@ +"""Pure geofence geometry tests (no GPS hardware, no threads).""" + +from __future__ import annotations + +import pytest + +from config import GeoFenceConfig +from gowelcome.geo.geofence import GeoFence, bearing_deg, haversine_m, normalize_deg + +_M_PER_DEG = 111_194.0 # ~metres per degree of latitude + + +def test_haversine_known_distances(): + assert haversine_m(0, 0, 0, 0) == pytest.approx(0.0, abs=1e-6) + assert haversine_m(0, 0, 1, 0) == pytest.approx(_M_PER_DEG, rel=0.01) + assert haversine_m(0, 0, 0, 1) == pytest.approx(_M_PER_DEG, rel=0.01) + + +def test_bearing_cardinals(): + assert bearing_deg(0, 0, 1, 0) == pytest.approx(0.0, abs=0.5) # north + assert bearing_deg(0, 0, 0, 1) == pytest.approx(90.0, abs=0.5) # east + assert bearing_deg(0, 0, -1, 0) == pytest.approx(180.0, abs=0.5) # south + assert bearing_deg(0, 0, 0, -1) == pytest.approx(270.0, abs=0.5) # west + + +def test_normalize_deg(): + assert normalize_deg(270) == pytest.approx(-90.0) + assert normalize_deg(-270) == pytest.approx(90.0) + assert normalize_deg(0) == pytest.approx(0.0) + assert normalize_deg(190) == pytest.approx(-170.0) + + +def _fence(**over) -> GeoFence: + cfg = GeoFenceConfig(radius_m=15.0, margin_m=3.0, release_m=2.0) + for k, v in over.items(): + setattr(cfg, k, v) + return GeoFence(cfg) + + +def test_center_capture_onstart(): + f = _fence(center_mode="onstart") + assert not f.has_center + f.maybe_capture_center(10.0, 20.0) + assert f.has_center and f.center == (10.0, 20.0) + f.maybe_capture_center(11.0, 21.0) # ignored once set + assert f.center == (10.0, 20.0) + + +def test_center_fixed_mode(): + f = _fence(center_mode="fixed", center_lat=1.0, center_lon=2.0) + assert f.has_center and f.center == (1.0, 2.0) + + +def test_breached_and_cleared_hysteresis(): + f = _fence(center_mode="onstart") + f.maybe_capture_center(0.0, 0.0) + # radius 15, margin 3 -> breached at >=12 m; cleared at <=10 m. + near = 9.0 / _M_PER_DEG # ~9 m north + edge = 13.0 / _M_PER_DEG # ~13 m north + assert not f.breached(near, 0.0) + assert f.cleared(near, 0.0) + assert f.breached(edge, 0.0) + assert not f.cleared(edge, 0.0) + + +def test_homing_yaw_sign_steers_toward_center(): + f = _fence(center_mode="onstart") + f.maybe_capture_center(0.0, 0.0) + east = 0.001 # ~111 m east of centre -> centre is to the WEST + west = -0.001 # west of centre -> centre is to the EAST + # Heading north (course 0). East of centre -> turn LEFT (vyaw>0) to face west. + assert f.homing_yaw(0.0, east, course_deg=0.0) > 0.0 + # West of centre -> turn RIGHT (vyaw<0) to face east. + assert f.homing_yaw(0.0, west, course_deg=0.0) < 0.0 + + +def test_homing_yaw_no_course_gentle_turn(): + f = _fence(center_mode="onstart") + f.maybe_capture_center(0.0, 0.0) + v = f.homing_yaw(0.0, 0.001, course_deg=None) + assert 0.0 < v <= f.cfg.max_homing_yaw + + +def test_homing_yaw_clamped(): + f = _fence(center_mode="onstart", homing_kp=100.0, max_homing_yaw=0.9) + f.maybe_capture_center(0.0, 0.0) + v = f.homing_yaw(0.0, 0.001, course_deg=90.0) + assert abs(v) <= 0.9 + 1e-9 + + +def test_no_center_returns_none(): + f = _fence(center_mode="onstart") + assert f.distance_from_center(0, 0) is None + assert f.bearing_to_center(0, 0) is None + assert not f.breached(0, 0) + assert f.cleared(0, 0) # nothing to enforce -> treated as clear diff --git a/tests/test_gps.py b/tests/test_gps.py new file mode 100644 index 0000000..47dd91c --- /dev/null +++ b/tests/test_gps.py @@ -0,0 +1,72 @@ +"""GPS source tests: NMEA parsing + the simulated MockGpsSource (no hardware).""" + +from __future__ import annotations + +import time + +import pytest + +from config import default_config +from gowelcome.geo.gps import ( + MockGpsSource, + _nmea_deg, + _parse_gga, + _parse_rmc, + build_gps_source, +) + + +def test_nmea_deg_conversion(): + # 48 deg 07.038' N -> 48.1173 + assert _nmea_deg("4807.038", "N") == pytest.approx(48.1173, abs=1e-4) + assert _nmea_deg("4807.038", "S") == pytest.approx(-48.1173, abs=1e-4) + assert _nmea_deg("01131.000", "E") == pytest.approx(11.5167, abs=1e-3) + assert _nmea_deg("", "N") is None + + +def test_parse_gga(): + s = "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47" + lat, lon, sats, hdop = _parse_gga(s) + assert lat == pytest.approx(48.1173, abs=1e-4) + assert lon == pytest.approx(11.5167, abs=1e-3) + assert sats == 8 + assert hdop == pytest.approx(0.9) + # fix quality 0 -> no fix + assert _parse_gga("$GPGGA,123519,4807.038,N,01131.000,E,0,00,,,M,,M,,*47") is None + + +def test_parse_rmc(): + s = "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A" + course, speed = _parse_rmc(s) + assert course == pytest.approx(84.4) + assert speed == pytest.approx(22.4 * 0.514444, rel=0.01) # knots -> m/s + # status V (void) -> no data + assert _parse_rmc("$GPRMC,123519,V,,,,,,,230394,,*00") == (None, None) + + +def test_build_gps_source_mock_and_disabled(): + cfg = default_config() + cfg.gps.enabled = True # GPS is opt-in now (default vision-only) + cfg.gps.source = "mock" + assert isinstance(build_gps_source(cfg), MockGpsSource) + cfg.gps.enabled = False + assert build_gps_source(cfg) is None + + +def test_mock_gps_integrates_forward_motion(): + cfg = default_config() + src = MockGpsSource(cfg.gps, rate_hz=50.0) + src.start() + try: + # Drive forward (north, heading 0) for a moment. + src.on_command(0.5, 0.0, 0.0) + time.sleep(0.4) + fix = src.latest() + assert fix is not None + # Moved north of the start latitude; longitude ~unchanged. + assert fix.lat > cfg.gps.mock_start_lat + assert fix.lon == pytest.approx(cfg.gps.mock_start_lon, abs=1e-6) + assert fix.course_deg == pytest.approx(0.0, abs=1.0) # heading north + assert fix.num_sats and fix.num_sats >= 4 + finally: + src.stop() diff --git a/tests/test_pid.py b/tests/test_pid.py new file mode 100644 index 0000000..e38f79a --- /dev/null +++ b/tests/test_pid.py @@ -0,0 +1,112 @@ +"""Unit tests for :mod:`gowelcome.control.pid` (PIDController + normalize_angle). + +Pure / stdlib only -- no cv2, ultralytics, SDK or hardware. +""" + +from __future__ import annotations + +import math + +import pytest + +from gowelcome.control.pid import PIDController, normalize_angle + + +# --------------------------------------------------------------------------- # +# PIDController +# --------------------------------------------------------------------------- # +def test_proportional_output(): + """With ki=kd=0 the output is exactly kp * error.""" + pid = PIDController(kp=2.0) + assert pid.update(3.0, dt=0.1) == pytest.approx(6.0) + # The sign of the output tracks the sign of the error. + assert pid.update(-1.5, dt=0.1) == pytest.approx(-3.0) + + +def test_integral_accumulates(): + """The integral term builds up over successive steps of constant error.""" + pid = PIDController(kp=0.0, ki=1.0) + out1 = pid.update(1.0, dt=0.5) # integral = 0.5 -> out 0.5 + out2 = pid.update(1.0, dt=0.5) # integral = 1.0 -> out 1.0 + assert out2 > out1 + assert out1 == pytest.approx(0.5) + assert out2 == pytest.approx(1.0) + + +def test_integral_limit_clamps_windup(): + """integral_limit caps the accumulated integral (anti-windup).""" + pid = PIDController(kp=0.0, ki=1.0, integral_limit=0.3) + # Drive a large constant error for several steps; integral must not exceed + # the clamp regardless of how long we accumulate. + out = 0.0 + for _ in range(20): + out = pid.update(5.0, dt=0.5) + assert pid.integral == pytest.approx(0.3) + assert out == pytest.approx(0.3) + + +def test_deadband_zeroes_integral_and_derivative(): + """Inside the deadband the integral and derivative terms are suppressed. + + The proportional term still acts (per the controller's documented + behaviour), but integral build-up and derivative kick are zeroed. + """ + pid = PIDController(kp=1.0, ki=10.0, kd=10.0, deadband=0.5) + # Error below the deadband: integral and derivative contributions dropped, + # so the output collapses to just kp * error. + out = pid.update(0.2, dt=0.1) + assert pid.integral == pytest.approx(0.0) + assert out == pytest.approx(0.2) # 1.0 * 0.2 only + + +def test_output_limits_clamp(): + """output_limits saturate the controller output on both sides.""" + pid = PIDController(kp=10.0, output_limits=(-1.0, 1.0)) + assert pid.update(5.0, dt=0.1) == pytest.approx(1.0) # would be 50 -> clamp hi + assert pid.update(-5.0, dt=0.1) == pytest.approx(-1.0) # would be -50 -> clamp lo + # Within the band, no clamping. + assert pid.update(0.05, dt=0.1) == pytest.approx(0.5) + + +def test_reset_clears_state(): + """reset() clears the integral accumulator and previous-error memory.""" + pid = PIDController(kp=0.0, ki=1.0) + pid.update(1.0, dt=1.0) + assert pid.integral != 0.0 + pid.reset() + assert pid.integral == pytest.approx(0.0) + assert pid.prev_error == pytest.approx(0.0) + + +def test_nonpositive_dt_is_guarded(): + """dt <= 0 must not corrupt the integral or blow up the derivative.""" + pid = PIDController(kp=1.0, ki=1.0, kd=1.0) + out = pid.update(2.0, dt=0.0) + # Integral unchanged (still zero) and derivative treated as zero -> P only. + assert pid.integral == pytest.approx(0.0) + assert out == pytest.approx(2.0) + + +# --------------------------------------------------------------------------- # +# normalize_angle +# --------------------------------------------------------------------------- # +def test_normalize_angle_wraps_into_range(): + """Angles wrap into (-pi, pi].""" + assert normalize_angle(0.0) == pytest.approx(0.0) + assert normalize_angle(math.pi / 2) == pytest.approx(math.pi / 2) + # 3*pi wraps to +/- pi (same point on the circle). + assert abs(normalize_angle(3.0 * math.pi)) == pytest.approx(math.pi) + # Just past +pi wraps to near -pi. + assert normalize_angle(math.pi + 0.1) == pytest.approx(-math.pi + 0.1) + # A large negative angle (whole 2*pi turns) wraps back to the same point. + wrapped = normalize_angle(-4.0 * math.pi + 0.25) + assert -math.pi <= wrapped <= math.pi + assert wrapped == pytest.approx(0.25, abs=1e-6) + + +def test_normalize_angle_is_periodic(): + """Adding full 2*pi turns does not change the normalized result.""" + for base in (0.3, -1.2, 2.9): + assert normalize_angle(base) == pytest.approx( + normalize_angle(base + 2.0 * math.pi), abs=1e-9 + ) diff --git a/tests/test_road_logic.py b/tests/test_road_logic.py new file mode 100644 index 0000000..9058ad5 --- /dev/null +++ b/tests/test_road_logic.py @@ -0,0 +1,93 @@ +"""Pure-geometry tests for :class:`gowelcome.types.RoadInfo` and +:class:`gowelcome.types.Detection` derived properties. No cv2 / SDK / hardware. +""" + +from __future__ import annotations + +import pytest + +from gowelcome.types import Detection, RoadInfo + + +# --------------------------------------------------------------------------- # +# RoadInfo.clearer_side +# --------------------------------------------------------------------------- # +def test_clearer_side_prefers_right_when_right_has_less_road(): + """Less road on the right (right <= left) -> clearer_side +1 (right is the clear side). + + Note: clearer_side is a *side index* (+1 = right), not a yaw sign; the state + machine negates it to obtain the CW/right turn that steers away from the road. + """ + info = RoadInfo(coverage=0.5, left=0.8, center=0.5, right=0.1) + assert info.clearer_side == 1 + + +def test_clearer_side_prefers_left_when_left_has_less_road(): + """Less road on the left -> -1.""" + info = RoadInfo(coverage=0.5, left=0.1, center=0.5, right=0.8) + assert info.clearer_side == -1 + + +def test_clearer_side_ties_to_right(): + """Equal coverage is a tie; the contract resolves ties to +1 (right).""" + info = RoadInfo(coverage=0.4, left=0.4, center=0.4, right=0.4) + assert info.clearer_side == 1 + + +def test_clearer_side_is_pure_without_mask(): + """clearer_side works with mask=None (no cv2 involvement).""" + info = RoadInfo(coverage=0.0, left=0.2, center=0.0, right=0.05, mask=None) + assert info.mask is None + assert info.clearer_side == 1 + + +# --------------------------------------------------------------------------- # +# Detection geometry +# --------------------------------------------------------------------------- # +def test_detection_centroid_and_dimensions(): + """cx, cy, w, h, area are computed from the corners.""" + det = Detection(label="person", conf=0.9, x1=100, y1=200, x2=300, y2=500) + assert det.cx == pytest.approx(200.0) # (100 + 300) / 2 + assert det.cy == pytest.approx(350.0) # (200 + 500) / 2 + assert det.w == pytest.approx(200.0) + assert det.h == pytest.approx(300.0) + assert det.area == pytest.approx(200.0 * 300.0) + + +def test_detection_height_ratio(): + """height_ratio is box height over frame height.""" + det = Detection(label="person", conf=0.9, x1=0, y1=120, x2=50, y2=360) + # h = 240, frame_h = 480 -> 0.5 + assert det.height_ratio(480) == pytest.approx(0.5) + + +def test_detection_height_ratio_guards_zero_frame(): + """A zero/garbage frame height does not divide-by-zero.""" + det = Detection(label="person", conf=0.9, x1=0, y1=0, x2=10, y2=100) + # max(1, frame_h) guard -> finite result. + val = det.height_ratio(0) + assert val == pytest.approx(100.0) + + +def test_detection_horizontal_offset_centre_is_zero(): + """A box centred in the frame has ~zero horizontal offset.""" + frame_w = 640 + det = Detection(label="person", conf=0.9, x1=300, y1=0, x2=340, y2=10) # cx=320 + assert det.horizontal_offset(frame_w) == pytest.approx(0.0) + + +def test_detection_horizontal_offset_sign_right_positive(): + """A box right of centre has a positive (+) offset, left has negative (-).""" + frame_w = 640 + right = Detection(label="person", conf=0.9, x1=600, y1=0, x2=640, y2=10) + left = Detection(label="person", conf=0.9, x1=0, y1=0, x2=40, y2=10) + assert right.horizontal_offset(frame_w) > 0.0 + assert left.horizontal_offset(frame_w) < 0.0 + + +def test_detection_horizontal_offset_full_right_near_one(): + """A box hard against the right edge approaches +1.""" + frame_w = 640 + det = Detection(label="person", conf=0.9, x1=636, y1=0, x2=640, y2=10) # cx=638 + off = det.horizontal_offset(frame_w) + assert 0.9 < off <= 1.0 diff --git a/tests/test_servoing.py b/tests/test_servoing.py new file mode 100644 index 0000000..6183b1e --- /dev/null +++ b/tests/test_servoing.py @@ -0,0 +1,144 @@ +"""Unit tests for :mod:`gowelcome.control.servoing` (VisualServo). + +Constructs ``Detection`` objects directly and a ``ServoConfig`` from the +project config; asserts the velocity *signs and inequalities* rather than exact +floats, so the tests survive minor gain re-tuning. No cv2 / SDK / hardware. +""" + +from __future__ import annotations + +from config import ServoConfig, default_config +from gowelcome.control.servoing import VisualServo +from gowelcome.types import Detection + + +FRAME_W = 640 +FRAME_H = 480 +DT = 1.0 / 15.0 + + +def _centered_box(frame_w: int, frame_h: int, h_px: int, w_px: int = 80) -> Detection: + """A person box centred horizontally, with the given pixel height.""" + cx = frame_w / 2.0 + cy = frame_h / 2.0 + x1 = int(cx - w_px / 2) + x2 = int(cx + w_px / 2) + y1 = int(cy - h_px / 2) + y2 = int(cy + h_px / 2) + return Detection(label="person", conf=0.95, x1=x1, y1=y1, x2=x2, y2=y2) + + +def test_servoconfig_from_default_config(): + """ServoConfig is reachable from the top-level default config.""" + cfg = default_config() + assert isinstance(cfg.servo, ServoConfig) + assert cfg.servo.yaw_sign == -1.0 # contract default + + +def test_centered_far_target_drives_forward_with_small_yaw(): + """Centred + small box (far) -> vx > 0 and |vyaw| small (near zero).""" + cfg = default_config().servo + servo = VisualServo(cfg) + # Small box -> far away -> below stop_height_ratio. + far_box = _centered_box(FRAME_W, FRAME_H, h_px=60) # ratio 0.125 << 0.50 + vx, vyaw, arrived = servo.compute(far_box, FRAME_W, FRAME_H, DT) + assert not arrived + assert vx > 0.0 + # Perfectly centred -> within the deadband -> essentially no turn. + assert abs(vyaw) < 1e-6 + + +def test_far_right_target_yaw_sign_is_correct(): + """Target far to the RIGHT (norm_err>0) with yaw_sign=-1 -> vyaw < 0.""" + cfg = default_config().servo + assert cfg.yaw_sign == -1.0 + servo = VisualServo(cfg) + # Push the box well to the right of centre (cx ~ 0.9 * frame_w). + box = Detection(label="person", conf=0.9, x1=540, y1=200, x2=620, y2=300) + assert box.horizontal_offset(FRAME_W) > 0.0 # confirm "right of centre" + _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT) + assert vyaw < 0.0 # turn right (CW) to centre a right-side target + + +def test_far_left_target_yaw_sign_is_correct(): + """Target far to the LEFT (norm_err<0) with yaw_sign=-1 -> vyaw > 0.""" + cfg = default_config().servo + servo = VisualServo(cfg) + box = Detection(label="person", conf=0.9, x1=20, y1=200, x2=100, y2=300) + assert box.horizontal_offset(FRAME_W) < 0.0 # left of centre + _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT) + assert vyaw > 0.0 # turn left (CCW) to centre a left-side target + + +def test_arrived_when_box_fills_stop_height_ratio(): + """Box height >= stop_height_ratio of the frame -> arrived True and vx == 0.""" + cfg = default_config().servo + servo = VisualServo(cfg) + # Box taller than stop_height_ratio * frame_h (0.50 * 480 = 240). + h_px = int(cfg.stop_height_ratio * FRAME_H) + 40 + near_box = _centered_box(FRAME_W, FRAME_H, h_px=h_px) + assert near_box.height_ratio(FRAME_H) >= cfg.stop_height_ratio + vx, _vyaw, arrived = servo.compute(near_box, FRAME_W, FRAME_H, DT) + assert arrived is True + assert vx == 0.0 + + +def test_deadband_zeroes_yaw_near_centre(): + """A target inside the yaw deadband produces ~0 yaw.""" + cfg = default_config().servo + servo = VisualServo(cfg) + # Offset the centre by less than yaw_deadband (0.06) of the half-width. + # half-width = 320; 0.06 * 320 ~= 19 px, so nudge by ~10 px. + cx = FRAME_W / 2.0 + 10 + box = Detection( + label="person", conf=0.9, + x1=int(cx - 40), y1=200, x2=int(cx + 40), y2=300, + ) + assert abs(box.horizontal_offset(FRAME_W)) < cfg.yaw_deadband + _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT) + assert abs(vyaw) < 1e-6 + + +def test_no_derivative_kick_crossing_into_deadband(): + """Crossing from off-centre INTO the deadband must not spike the yaw. + + Regression for the derivative-kick: with a non-zero kd, a target that was + off-centre last tick and is centred this tick used to inject a one-tick + derivative spike. The PID's own deadband (fed a pre-zeroed in-band error) + must suppress it, so the centred tick yields ~0 yaw. + """ + cfg = default_config().servo + assert cfg.kd_yaw > 0.0 # the kick only exists with a derivative term + servo = VisualServo(cfg) + + # Tick 1: clearly off-centre (outside the deadband) -> a real turn. + off = Detection(label="person", conf=0.9, x1=560, y1=200, x2=620, y2=300) + assert off.horizontal_offset(FRAME_W) > cfg.yaw_deadband + _vx1, vyaw1, _ = servo.compute(off, FRAME_W, FRAME_H, DT) + assert abs(vyaw1) > 0.0 + + # Tick 2: now centred (inside the deadband) -> must settle to ~0, no kick. + centred = _centered_box(FRAME_W, FRAME_H, h_px=120) + assert abs(centred.horizontal_offset(FRAME_W)) < cfg.yaw_deadband + _vx2, vyaw2, _ = servo.compute(centred, FRAME_W, FRAME_H, DT) + assert abs(vyaw2) < 1e-6 + + +def test_forward_throttled_off_axis(): + """A far box far off-axis should command less forward than the same box centred.""" + cfg = default_config().servo + + far_h = 60 # well below the stop ratio for both cases + + servo_centre = VisualServo(cfg) + centred = _centered_box(FRAME_W, FRAME_H, h_px=far_h) + vx_centre, _, _ = servo_centre.compute(centred, FRAME_W, FRAME_H, DT) + + servo_side = VisualServo(cfg) + off_axis = Detection(label="person", conf=0.9, x1=580, y1=210, + x2=620, y2=210 + far_h) + vx_side, _, _ = servo_side.compute(off_axis, FRAME_W, FRAME_H, DT) + + # Heading-error throttle (exp(-falloff*|err|)) makes the off-axis forward + # speed no greater than the centred one. + assert vx_side <= vx_centre diff --git a/tests/test_statemachine.py b/tests/test_statemachine.py new file mode 100644 index 0000000..d9e1b44 --- /dev/null +++ b/tests/test_statemachine.py @@ -0,0 +1,331 @@ +"""Behaviour tests for :class:`gowelcome.statemachine.GoWelcomeStateMachine`. + +Driven entirely off-robot with a recording fake ``RobotInterface`` and a fake +perception object exposing ``latest()``. Stdlib only -- no cv2, ultralytics, +SDK or hardware. Crafted ``PerceptionResult`` snapshots use a ``ts`` close to +``time.monotonic()`` so the machine treats them as fresh. +""" + +from __future__ import annotations + +import time +from typing import List, Optional, Tuple + +from config import default_config +from gowelcome.statemachine import GoWelcomeStateMachine +from gowelcome.types import Detection, PerceptionResult, RoadInfo, State +from gowelcome.robot.interface import RobotInterface + + +FRAME_W = 640 +FRAME_H = 480 + + +# --------------------------------------------------------------------------- # +# Test doubles +# --------------------------------------------------------------------------- # +class FakeRobot(RobotInterface): + """Records every command so tests can assert on the issued behaviour.""" + + def __init__(self) -> None: + self.drives: List[Tuple[float, float, float]] = [] + self.stops: int = 0 + self.gestures: List[str] = [] + self.greetings: int = 0 + self.balance_stands: int = 0 + self.stand_ups: int = 0 + self.damps: int = 0 + self.avoidance: Optional[bool] = None + self.shutdowns: int = 0 + + # --- perception input (unused by the SM; present to satisfy the ABC) ---- + def get_frame(self): + return None + + def frame_size(self) -> "tuple[int, int]": + return (FRAME_W, FRAME_H) + + # --- locomotion --------------------------------------------------------- + def drive(self, vx: float, vy: float, vyaw: float) -> None: + self.drives.append((vx, vy, vyaw)) + + def stop(self) -> None: + self.stops += 1 + + def set_avoidance(self, on: bool) -> None: + self.avoidance = on + + # --- posture / expression ---------------------------------------------- + def balance_stand(self) -> None: + self.balance_stands += 1 + + def stand_up(self) -> None: + self.stand_ups += 1 + + def damp(self) -> None: + self.damps += 1 + + def gesture(self, name: str) -> None: + self.gestures.append(name) + + # --- greeting payload --------------------------------------------------- + def play_greeting(self) -> None: + self.greetings += 1 + + # --- lifecycle ---------------------------------------------------------- + def shutdown(self) -> None: + self.shutdowns += 1 + + +class FakePerception: + """Minimal stand-in for PerceptionThread serving a single snapshot. + + By default each ``latest()`` re-stamps the snapshot's ``ts`` to "now" so it + reads as *fresh* on every tick -- mirroring a real perception thread that + keeps republishing. Pass ``keep_ts=True`` to leave ``ts`` untouched (used to + exercise the stale-perception path). + """ + + def __init__( + self, result: Optional[PerceptionResult] = None, keep_ts: bool = False + ) -> None: + self._result = result + self._keep_ts = keep_ts + + def set(self, result: Optional[PerceptionResult]) -> None: + self._result = result + + def latest(self) -> Optional[PerceptionResult]: + if self._result is not None and not self._keep_ts: + self._result.ts = time.monotonic() + return self._result + + +# --------------------------------------------------------------------------- # +# Snapshot builders +# --------------------------------------------------------------------------- # +def _fresh_ts() -> float: + """A timestamp the state machine will treat as fresh (within timeout).""" + return time.monotonic() + + +def _person_box(h_px: int, cx: float = FRAME_W / 2.0, conf: float = 0.95) -> Detection: + """A centred person box of the requested pixel height.""" + w_px = 80 + return Detection( + label="person", + conf=conf, + x1=int(cx - w_px / 2), + y1=int(FRAME_H / 2 - h_px / 2), + x2=int(cx + w_px / 2), + y2=int(FRAME_H / 2 + h_px / 2), + ) + + +def _result( + persons=None, + dangers=None, + road: Optional[RoadInfo] = None, +) -> PerceptionResult: + persons = persons or [] + dangers = dangers or [] + return PerceptionResult( + frame_w=FRAME_W, + frame_h=FRAME_H, + detections=list(persons) + list(dangers), + persons=list(persons), + dangers=list(dangers), + road=road, + ts=_fresh_ts(), + seq=1, + ) + + +# --------------------------------------------------------------------------- # +# Tests +# --------------------------------------------------------------------------- # +def test_no_person_wanders_and_drives(): + """With an empty fresh snapshot the machine wanders (drives forward).""" + cfg = default_config() + robot = FakeRobot() + perc = FakePerception(_result()) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + state = sm.step(1.0 / cfg.loop.rate_hz) + assert state is State.WANDER + assert robot.drives, "WANDER should issue a drive command" + vx, _vy, _vyaw = robot.drives[-1] + assert vx > 0.0 # cruising forward + + +def test_near_person_progresses_wander_to_approach_to_greet(): + """A big, confident, centred person -> APPROACH then eventually GREET.""" + cfg = default_config() + robot = FakeRobot() + + # Box tall enough to be 'arrived' immediately (>= stop_height_ratio). + arrived_h = int(cfg.servo.stop_height_ratio * FRAME_H) + 60 + perc = FakePerception(_result(persons=[_person_box(arrived_h)])) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + dt = 1.0 / cfg.loop.rate_hz + + # First tick: a qualifying person -> APPROACH; arriving immediately also + # kicks the machine into GREET within the same handling. + state = sm.step(dt) + assert state in (State.APPROACH, State.GREET) + + # Drive the machine forward in time until it greets, then completes. + greeted = False + deadline = time.monotonic() + cfg.greet.settle_time + \ + len(cfg.greet.gestures) * cfg.greet.gesture_gap + 2.0 + while time.monotonic() < deadline: + st = sm.step(dt) + if st is State.GREET: + greeted = True + if greeted and st is State.WANDER: + break + # Tiny real sleep so monotonic-gated greet sub-sequence advances. + time.sleep(0.01) + + assert greeted, "machine should pass through GREET" + assert robot.greetings >= 1, "greeting audio should have been played" + assert robot.gestures, "at least one gesture should have been dispatched" + # The configured gestures are dispatched in order. + assert robot.gestures[: len(cfg.greet.gestures)] == list(cfg.greet.gestures) + + +def test_low_confidence_person_still_approaches_but_below_conf_is_handled(): + """Persons present -> APPROACH regardless (perception applies the conf gate). + + The state machine treats any populated ``persons`` list as a target; the + confidence gate lives in the perception layer. A far (small) box should + APPROACH and drive forward, not greet. + """ + cfg = default_config() + robot = FakeRobot() + far = _person_box(h_px=60) # ratio ~0.125, well below stop ratio + perc = FakePerception(_result(persons=[far])) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + state = sm.step(1.0 / cfg.loop.rate_hz) + assert state is State.APPROACH + vx, _vy, _vyaw = robot.drives[-1] + assert vx > 0.0 # approaching, not arrived + assert robot.greetings == 0 + + +def test_danger_box_forces_avoid_danger(): + """A danger detection preempts everything -> AVOID_DANGER + reverse drive.""" + cfg = default_config() + robot = FakeRobot() + danger = Detection(label="car", conf=0.9, x1=200, y1=200, x2=440, y2=460) + road = RoadInfo(coverage=0.4, left=0.6, center=0.4, right=0.1) + perc = FakePerception(_result(dangers=[danger], road=road)) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + state = sm.step(1.0 / cfg.loop.rate_hz) + assert state is State.AVOID_DANGER + # Backup phase reverses (vx < 0) while turning away. + vx, _vy, vyaw = robot.drives[-1] + assert vx < 0.0 + # Road is heavy on the LEFT (right < left) -> clearer_side +1 (right clear). + # Steering AWAY from the road = turning right (CW) = negative yaw. + assert vyaw < 0.0 + + +def test_avoid_turns_away_from_road_both_sides(): + """The escape pivot must always turn AWAY from the road-heavy side. + + Regression for the clearer_side sign bug: road on the left -> turn right + (vyaw < 0); road on the right -> turn left (vyaw > 0). + """ + cfg = default_config() + danger = Detection(label="car", conf=0.9, x1=200, y1=200, x2=440, y2=460) + + # Road heavy on the LEFT -> dog should turn RIGHT (negative yaw). + robot_l = FakeRobot() + road_left = RoadInfo(coverage=0.4, left=0.7, center=0.4, right=0.1) + sm_l = GoWelcomeStateMachine( + robot_l, FakePerception(_result(dangers=[danger], road=road_left)), cfg + ) + sm_l.step(1.0 / cfg.loop.rate_hz) + assert sm_l.state is State.AVOID_DANGER + assert robot_l.drives[-1][2] < 0.0 # vyaw < 0 -> turn right, away from left road + + # Road heavy on the RIGHT -> dog should turn LEFT (positive yaw). + robot_r = FakeRobot() + road_right = RoadInfo(coverage=0.4, left=0.1, center=0.4, right=0.7) + sm_r = GoWelcomeStateMachine( + robot_r, FakePerception(_result(dangers=[danger], road=road_right)), cfg + ) + sm_r.step(1.0 / cfg.loop.rate_hz) + assert sm_r.state is State.AVOID_DANGER + assert robot_r.drives[-1][2] > 0.0 # vyaw > 0 -> turn left, away from right road + + +def test_danger_overrides_person(): + """When both a person and a danger are present, danger wins.""" + cfg = default_config() + robot = FakeRobot() + person = _person_box(int(cfg.servo.stop_height_ratio * FRAME_H) + 60) + danger = Detection(label="truck", conf=0.9, x1=100, y1=150, x2=420, y2=460) + perc = FakePerception(_result(persons=[person], dangers=[danger])) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + state = sm.step(1.0 / cfg.loop.rate_hz) + assert state is State.AVOID_DANGER + assert robot.greetings == 0 + + +def test_road_coverage_triggers_avoid_danger(): + """High centre-road coverage (no boxes) still triggers AVOID_DANGER.""" + cfg = default_config() + robot = FakeRobot() + trig = cfg.perception.road_trigger_coverage + road = RoadInfo(coverage=0.9, left=0.9, center=trig + 0.1, right=0.2) + perc = FakePerception(_result(road=road)) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + state = sm.step(1.0 / cfg.loop.rate_hz) + assert state is State.AVOID_DANGER + + +def test_stale_perception_stops_robot(): + """A snapshot older than the perception timeout -> stop(), no drive.""" + cfg = default_config() + robot = FakeRobot() + stale = _result(persons=[_person_box(200)]) + # Backdate ts well beyond the timeout (keep_ts so latest() preserves it). + stale.ts = time.monotonic() - (cfg.safety.perception_timeout + 5.0) + perc = FakePerception(stale, keep_ts=True) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + sm.step(1.0 / cfg.loop.rate_hz) + assert robot.stops >= 1 + assert not robot.drives # never moved on stale data + + +def test_none_perception_stops_robot(): + """A missing snapshot (latest() -> None) -> stop(), no drive.""" + cfg = default_config() + robot = FakeRobot() + perc = FakePerception(None) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + sm.step(1.0 / cfg.loop.rate_hz) + assert robot.stops >= 1 + assert not robot.drives + + +def test_dry_run_never_drives(): + """In dry-run mode the machine logs but issues stop() instead of drive().""" + cfg = default_config() + cfg.dry_run = True + robot = FakeRobot() + perc = FakePerception(_result()) # would normally WANDER (drive forward) + sm = GoWelcomeStateMachine(robot, perc, cfg) + + sm.step(1.0 / cfg.loop.rate_hz) + assert not robot.drives, "dry_run must suppress drive()" + assert robot.stops >= 1 diff --git a/tests/test_web.py b/tests/test_web.py new file mode 100644 index 0000000..9cacda3 --- /dev/null +++ b/tests/test_web.py @@ -0,0 +1,72 @@ +"""Tests for the optional MJPEG web viewer (stdlib only -- no cv2 / hardware). + +Binds an ephemeral localhost port, hits each route, and checks the framing. +""" + +from __future__ import annotations + +import time +import urllib.request + +import pytest + +from gowelcome.web import MjpegServer + +# A minimal valid JPEG (SOI ... EOI) used as the stub frame. +_FAKE_JPEG = bytes.fromhex("ffd8ffe000104a46494600010100000100010000ffd9") + + +@pytest.fixture() +def server(): + srv = MjpegServer(lambda: _FAKE_JPEG, host="127.0.0.1", port=0, fps=30) + srv.start() + time.sleep(0.2) + try: + yield srv + finally: + srv.stop() + + +def _get(port: int, path: str, n: int | None = None): + resp = urllib.request.urlopen(f"http://127.0.0.1:{port}{path}", timeout=3) + return resp.status, resp.headers.get("Content-Type"), (resp.read(n) if n else resp.read()) + + +def test_healthz(server): + status, ctype, body = _get(server.port, "/healthz") + assert status == 200 + assert body == b"ok" + + +def test_index_embeds_stream(server): + status, ctype, body = _get(server.port, "/") + assert status == 200 + assert "text/html" in ctype + assert b"/stream.mjpg" in body + + +def test_snapshot_returns_jpeg(server): + status, ctype, body = _get(server.port, "/snapshot.jpg") + assert status == 200 + assert ctype == "image/jpeg" + assert body[:2] == b"\xff\xd8" # JPEG SOI + + +def test_stream_is_multipart_mjpeg(server): + resp = urllib.request.urlopen(f"http://127.0.0.1:{server.port}/stream.mjpg", timeout=3) + assert "multipart/x-mixed-replace" in resp.headers.get("Content-Type", "") + chunk = resp.read(400) + resp.close() + assert b"--gowelcomeframe" in chunk + assert b"image/jpeg" in chunk + + +def test_unknown_path_404(server): + with pytest.raises(urllib.error.HTTPError) as exc: + _get(server.port, "/nope") + assert exc.value.code == 404 + + +def test_stop_is_idempotent(server): + server.stop() + server.stop() # must not raise diff --git a/welcome.sh b/welcome.sh new file mode 100755 index 0000000..6a092b6 --- /dev/null +++ b/welcome.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Launch GoWelcome inside the 'welcome' conda env. Any arguments are passed +# straight through to main.py. +# +# ./welcome.sh # mock / webcam (no robot) +# ./welcome.sh --interface eth0 # real Go2 over eth0 (TEST SUSPENDED FIRST) +# ./welcome.sh --interface eth0 --dry-run --headless +set -euo pipefail + +ENV_NAME="welcome" +# Resolve this script's own directory (the project root) so it runs from any CWD. +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +# --- locate + activate conda -------------------------------------------- +if ! command -v conda >/dev/null 2>&1; then + for c in "$HOME/miniconda3" "$HOME/anaconda3" "$HOME/miniforge3" "/opt/conda"; do + if [ -f "$c/etc/profile.d/conda.sh" ]; then + # shellcheck disable=SC1091 + source "$c/etc/profile.d/conda.sh" + break + fi + done +fi +command -v conda >/dev/null 2>&1 || { + echo "ERROR: conda not found. Install Miniconda first, then run ./requirement.sh" >&2 + exit 1 +} +# shellcheck disable=SC1091 +source "$(conda info --base)/etc/profile.d/conda.sh" +conda activate "$ENV_NAME" 2>/dev/null || { + echo "ERROR: conda env '$ENV_NAME' not found. Run ./requirement.sh first." >&2 + exit 1 +} + +# Run from the project root so bundled assets (yolov8n.pt, greeting.wav) resolve. +cd "$SCRIPT_DIR" + +# --- default to mock when no args, so a bare ./welcome.sh just runs ------ +if [ "$#" -eq 0 ]; then + echo "[welcome] no args -> MOCK mode (webcam 0)." + echo "[welcome] real robot (WebRTC, default): ./welcome.sh --robot-ip [--aes-key ]" + echo "[welcome] real robot (DDS/EDU wired): ./welcome.sh --transport dds --interface eth0" + set -- --mock --source 0 +fi + +exec python main.py "$@"