Update 2026-07-04 23:37:55
This commit is contained in:
commit
cc4df6dbad
23
.gitignore
vendored
Normal file
23
.gitignore
vendored
Normal file
@ -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
|
||||
397
README.md
Normal file
397
README.md
Normal file
@ -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 <hex> 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://<dog-ip>: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://<dog-ip>:<port>/`. 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.
|
||||
43
assets/greeting.README.md
Normal file
43
assets/greeting.README.md
Normal file
@ -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.
|
||||
357
config.py
Normal file
357
config.py
Normal file
@ -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=<output_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://<host-ip>:<port>/`` 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()
|
||||
10
gowelcome/__init__.py
Normal file
10
gowelcome/__init__.py
Normal file
@ -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"
|
||||
6
gowelcome/control/__init__.py
Normal file
6
gowelcome/control/__init__.py
Normal file
@ -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`.
|
||||
"""
|
||||
113
gowelcome/control/pid.py
Normal file
113
gowelcome/control/pid.py
Normal file
@ -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
|
||||
111
gowelcome/control/servoing.py
Normal file
111
gowelcome/control/servoing.py
Normal file
@ -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)
|
||||
14
gowelcome/geo/__init__.py
Normal file
14
gowelcome/geo/__init__.py
Normal file
@ -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",
|
||||
]
|
||||
126
gowelcome/geo/geofence.py
Normal file
126
gowelcome/geo/geofence.py
Normal file
@ -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))
|
||||
334
gowelcome/geo/gps.py
Normal file
334
gowelcome/geo/gps.py
Normal file
@ -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)
|
||||
15
gowelcome/perception/__init__.py
Normal file
15
gowelcome/perception/__init__.py
Normal file
@ -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"]
|
||||
116
gowelcome/perception/detector.py
Normal file
116
gowelcome/perception/detector.py
Normal file
@ -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
|
||||
91
gowelcome/perception/road_mask.py
Normal file
91
gowelcome/perception/road_mask.py
Normal file
@ -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)
|
||||
134
gowelcome/perception/vision_thread.py
Normal file
134
gowelcome/perception/vision_thread.py
Normal file
@ -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
|
||||
1
gowelcome/robot/__init__.py
Normal file
1
gowelcome/robot/__init__.py
Normal file
@ -0,0 +1 @@
|
||||
"""Robot abstraction layer: hardware-agnostic interface + concrete backends."""
|
||||
504
gowelcome/robot/audio.py
Normal file
504
gowelcome/robot/audio.py
Normal file
@ -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("<I")
|
||||
if chunk_id != 0x46464952: # "RIFF"
|
||||
logger.error("read_wav: chunk_id != 'RIFF': %s", hex(chunk_id))
|
||||
return [], -1, -1, False
|
||||
|
||||
_chunk_size, = read("<I")
|
||||
format_tag, = read("<I")
|
||||
if format_tag != 0x45564157: # "WAVE"
|
||||
logger.error("read_wav: format != 'WAVE': %s", hex(format_tag))
|
||||
return [], -1, -1, False
|
||||
|
||||
# === Subchunk1: fmt ===
|
||||
subchunk1_id, = read("<I")
|
||||
subchunk1_size, = read("<I")
|
||||
|
||||
if subchunk1_id == 0x4B4E554A: # JUNK
|
||||
f.seek(subchunk1_size, 1)
|
||||
subchunk1_id, = read("<I")
|
||||
subchunk1_size, = read("<I")
|
||||
|
||||
if subchunk1_id != 0x20746D66: # "fmt "
|
||||
logger.error("read_wav: subchunk1_id != 'fmt ': %s", hex(subchunk1_id))
|
||||
return [], -1, -1, False
|
||||
|
||||
if subchunk1_size not in (16, 18):
|
||||
logger.error("read_wav: subchunk1_size != 16 or 18: %s", subchunk1_size)
|
||||
return [], -1, -1, False
|
||||
|
||||
audio_format, = read("<H")
|
||||
if audio_format != 1:
|
||||
logger.error("read_wav: audio_format != PCM (1): %s", audio_format)
|
||||
return [], -1, -1, False
|
||||
|
||||
num_channels, = read("<H")
|
||||
sample_rate, = read("<I")
|
||||
byte_rate, = read("<I")
|
||||
block_align, = read("<H")
|
||||
bits_per_sample, = read("<H")
|
||||
|
||||
expected_byte_rate = sample_rate * num_channels * bits_per_sample // 8
|
||||
if byte_rate != expected_byte_rate:
|
||||
logger.error(
|
||||
"read_wav: byte_rate mismatch: got %s, expected %s",
|
||||
byte_rate, expected_byte_rate,
|
||||
)
|
||||
return [], -1, -1, False
|
||||
|
||||
expected_align = num_channels * bits_per_sample // 8
|
||||
if block_align != expected_align:
|
||||
logger.error(
|
||||
"read_wav: block_align mismatch: got %s, expected %s",
|
||||
block_align, expected_align,
|
||||
)
|
||||
return [], -1, -1, False
|
||||
|
||||
if bits_per_sample != 16:
|
||||
logger.error(
|
||||
"read_wav: only 16-bit samples supported, got %s",
|
||||
bits_per_sample,
|
||||
)
|
||||
return [], -1, -1, False
|
||||
|
||||
if subchunk1_size == 18:
|
||||
extra_size, = read("<H")
|
||||
if extra_size != 0:
|
||||
logger.error("read_wav: extra_size != 0: %s", extra_size)
|
||||
return [], -1, -1, False
|
||||
|
||||
# === Subchunk2: data ===
|
||||
while True:
|
||||
subchunk2_id, subchunk2_size = read("<II")
|
||||
if subchunk2_id == 0x61746164: # "data"
|
||||
break
|
||||
f.seek(subchunk2_size, 1)
|
||||
|
||||
raw_pcm = f.read(subchunk2_size)
|
||||
if len(raw_pcm) != subchunk2_size:
|
||||
logger.error("read_wav: failed to read full PCM data")
|
||||
return [], -1, -1, False
|
||||
|
||||
return list(raw_pcm), sample_rate, num_channels, True
|
||||
|
||||
except Exception as exc: # noqa: BLE001 -- never raise out of audio
|
||||
logger.error("read_wav() failed: %s", exc)
|
||||
return [], -1, -1, False
|
||||
|
||||
|
||||
def play_pcm_stream(
|
||||
client,
|
||||
pcm_list: List[int],
|
||||
stream_name: str = "example",
|
||||
chunk_size: int = 96000,
|
||||
sleep_time: float = 1.0,
|
||||
verbose: bool = False,
|
||||
) -> 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("<h", chunk, i)[0])
|
||||
for i in range(0, min(20, len(chunk) - 1), 2)
|
||||
)
|
||||
logger.debug(
|
||||
"[CHUNK %s] offset=%s size=%s first samples: %s",
|
||||
chunk_index, offset, current_chunk_size, preview,
|
||||
)
|
||||
|
||||
ret_code, _ = client.PlayStream(stream_name, stream_id, chunk)
|
||||
if ret_code != 0:
|
||||
logger.error(
|
||||
"play_pcm_stream: failed to send chunk %s, return code: %s",
|
||||
chunk_index, ret_code,
|
||||
)
|
||||
return False
|
||||
logger.debug("play_pcm_stream: chunk %s sent", chunk_index)
|
||||
|
||||
offset += current_chunk_size
|
||||
chunk_index += 1
|
||||
time.sleep(sleep_time)
|
||||
|
||||
return True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Backends
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class NullAudio(AudioBackend):
|
||||
"""No-op backend: logs what *would* play. Used by the mock robot / CI."""
|
||||
|
||||
def play(self, wav_path: str, blocking: bool = False) -> 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 <wav>``). 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=<sink> <wav>`` (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()
|
||||
59
gowelcome/robot/factory.py
Normal file
59
gowelcome/robot/factory.py
Normal file
@ -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')."
|
||||
)
|
||||
440
gowelcome/robot/go2_robot.py
Normal file
440
gowelcome/robot/go2_robot.py
Normal file
@ -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
|
||||
123
gowelcome/robot/interface.py
Normal file
123
gowelcome/robot/interface.py
Normal file
@ -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()
|
||||
183
gowelcome/robot/mock_robot.py
Normal file
183
gowelcome/robot/mock_robot.py
Normal file
@ -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)
|
||||
514
gowelcome/robot/webrtc_robot.py
Normal file
514
gowelcome/robot/webrtc_robot.py
Normal file
@ -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 <ip> or --serial <sn>)."
|
||||
)
|
||||
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")
|
||||
527
gowelcome/statemachine.py
Normal file
527
gowelcome/statemachine.py
Normal file
@ -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)
|
||||
119
gowelcome/types.py
Normal file
119
gowelcome/types.py
Normal file
@ -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)
|
||||
5
gowelcome/web/__init__.py
Normal file
5
gowelcome/web/__init__.py
Normal file
@ -0,0 +1,5 @@
|
||||
"""Optional HTTP/MJPEG camera viewer for GoWelcome."""
|
||||
|
||||
from gowelcome.web.stream import MjpegServer
|
||||
|
||||
__all__ = ["MjpegServer"]
|
||||
291
gowelcome/web/stream.py
Normal file
291
gowelcome/web/stream.py
Normal file
@ -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 = (
|
||||
"<!doctype html><html><head><meta charset='utf-8'>"
|
||||
"<meta name='viewport' content='width=device-width,initial-scale=1'>"
|
||||
f"<title>{title}</title>"
|
||||
"<style>body{margin:0;background:#111;color:#eee;font-family:sans-serif}"
|
||||
".wrap{max-width:900px;margin:0 auto;padding:8px;text-align:center}"
|
||||
"img{max-width:100%;max-height:70vh;object-fit:contain;background:#000}"
|
||||
"h3{margin:8px}button{font-size:15px;margin:3px;padding:8px 12px;border:0;"
|
||||
"border-radius:6px;background:#2a2a2a;color:#eee;cursor:pointer}"
|
||||
"button:hover{background:#3a3a3a}.estop{background:#a11}.estop:hover{background:#c22}"
|
||||
"#status{font-family:monospace;font-size:13px;text-align:left;background:#1a1a1a;"
|
||||
"padding:8px;border-radius:6px;margin-top:8px;white-space:pre-wrap}"
|
||||
".row{margin:6px 0}</style></head><body><div class='wrap'>"
|
||||
)
|
||||
if not controls:
|
||||
body = (
|
||||
f"<h3>{title} — live camera</h3>"
|
||||
"<img src='/stream.mjpg' alt='camera stream'/>"
|
||||
)
|
||||
else:
|
||||
body = (
|
||||
f"<h3>{title} — control dashboard</h3>"
|
||||
"<img src='/stream.mjpg' alt='camera stream'/>"
|
||||
"<div class='row'><b>Play:</b>"
|
||||
"<button onclick=\"ctl({action:'play_mode',mode:'calm'})\">Calm</button>"
|
||||
"<button onclick=\"ctl({action:'play_mode',mode:'moderate'})\">Moderate</button>"
|
||||
"<button onclick=\"ctl({action:'play_mode',mode:'playful'})\">Playful</button>"
|
||||
"</div>"
|
||||
"<div class='row'>"
|
||||
"<button onclick=\"ctl({action:'pause',paused:true})\">Pause</button>"
|
||||
"<button onclick=\"ctl({action:'resume'})\">Resume</button>"
|
||||
"<button onclick=\"ctl({action:'set_center'})\">Set fence centre here</button>"
|
||||
"</div>"
|
||||
"<div class='row'>"
|
||||
"<button class='estop' onclick=\"ctl({action:'estop'})\">E-STOP</button>"
|
||||
"<button onclick=\"ctl({action:'clear_estop'})\">Clear E-STOP</button>"
|
||||
"</div>"
|
||||
"<div id='status'>loading...</div>"
|
||||
"<script>"
|
||||
"function ctl(c){fetch('/control',{method:'POST',headers:{'Content-Type':"
|
||||
"'application/json'},body:JSON.stringify(c)}).then(r=>r.json()).then(poll);}"
|
||||
"function poll(){fetch('/status.json').then(r=>r.json()).then(s=>{"
|
||||
"document.getElementById('status').textContent=JSON.stringify(s,null,2);})"
|
||||
".catch(()=>{});}"
|
||||
"setInterval(poll,1000);poll();"
|
||||
"</script>"
|
||||
)
|
||||
return (head + body + "</div></body></html>").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)",
|
||||
"<host-ip>" 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")
|
||||
497
main.py
Executable file
497
main.py
Executable file
@ -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=<sink>'. 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://<host-ip>:<port>/ 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://<host-ip>:%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())
|
||||
6
pytest.ini
Normal file
6
pytest.ini
Normal file
@ -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
|
||||
59
requirement.sh
Executable file
59
requirement.sh
Executable file
@ -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"
|
||||
36
requirements.txt
Normal file
36
requirements.txt
Normal file
@ -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).
|
||||
7
ruff.toml
Normal file
7
ruff.toml
Normal file
@ -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"]
|
||||
17
scripts/run_mock.sh
Executable file
17
scripts/run_mock.sh
Executable file
@ -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 "$@"
|
||||
29
scripts/run_robot.sh
Executable file
29
scripts/run_robot.sh
Executable file
@ -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}" "$@"
|
||||
185
scripts/smoke_test.py
Normal file
185
scripts/smoke_test.py
Normal file
@ -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())
|
||||
6
tests/__init__.py
Normal file
6
tests/__init__.py
Normal file
@ -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.
|
||||
"""
|
||||
108
tests/test_dashboard.py
Normal file
108
tests/test_dashboard.py
Normal file
@ -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()
|
||||
81
tests/test_factory_transport.py
Normal file
81
tests/test_factory_transport.py
Normal file
@ -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))
|
||||
280
tests/test_geo_statemachine.py
Normal file
280
tests/test_geo_statemachine.py
Normal file
@ -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
|
||||
96
tests/test_geofence.py
Normal file
96
tests/test_geofence.py
Normal file
@ -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
|
||||
72
tests/test_gps.py
Normal file
72
tests/test_gps.py
Normal file
@ -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()
|
||||
112
tests/test_pid.py
Normal file
112
tests/test_pid.py
Normal file
@ -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
|
||||
)
|
||||
93
tests/test_road_logic.py
Normal file
93
tests/test_road_logic.py
Normal file
@ -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
|
||||
144
tests/test_servoing.py
Normal file
144
tests/test_servoing.py
Normal file
@ -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
|
||||
331
tests/test_statemachine.py
Normal file
331
tests/test_statemachine.py
Normal file
@ -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
|
||||
72
tests/test_web.py
Normal file
72
tests/test_web.py
Normal file
@ -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
|
||||
46
welcome.sh
Executable file
46
welcome.sh
Executable file
@ -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 <robot-ip> [--aes-key <hex>]"
|
||||
echo "[welcome] real robot (DDS/EDU wired): ./welcome.sh --transport dds --interface eth0"
|
||||
set -- --mock --source 0
|
||||
fi
|
||||
|
||||
exec python main.py "$@"
|
||||
Loading…
x
Reference in New Issue
Block a user