diff --git a/Cargo.lock b/Cargo.lock index 99c8f23fd37..9956bd11a38 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -4684,6 +4684,21 @@ dependencies = [ "url", ] +[[package]] +name = "moq-carla-bridge" +version = "0.1.0" +dependencies = [ + "anyhow", + "ash", + "bytes", + "moq-mux", + "moq-net", + "moq-video", + "parking_lot", + "pollster", + "tracing", +] + [[package]] name = "moq-cli" version = "0.14.2" diff --git a/Cargo.toml b/Cargo.toml index 6a4f1c1cd39..1d39c81c213 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,6 @@ [workspace] members = [ + "demo/carla/bridge", "rs/hang", "rs/kio", "rs/libmoq", diff --git a/bun.lock b/bun.lock index 4b2bfb3cfd8..53d11e64c16 100644 --- a/bun.lock +++ b/bun.lock @@ -49,6 +49,24 @@ "vite-plugin-solid": "^2.11.14", }, }, + "demo/carla/web": { + "name": "@moq/demo-carla", + "version": "0.1.0", + "dependencies": { + "@moq/clock": "workspace:^", + "@moq/json": "workspace:^", + "@moq/net": "workspace:^", + "@moq/watch": "workspace:^", + "zod": "^4.6.5", + }, + "devDependencies": { + "@types/bun": "^1.4.2", + "rolldown": "^1.2.12", + "typescript": "7.0.2", + "vite": "^8.3.2", + "wrangler": "^4.147.0", + }, + }, "demo/web": { "name": "@moq/demo", "version": "0.1.1", @@ -754,6 +772,8 @@ "@moq/demo-boy": ["@moq/demo-boy@workspace:demo/boy"], + "@moq/demo-carla": ["@moq/demo-carla@workspace:demo/carla/web"], + "@moq/drain-test": ["@moq/drain-test@workspace:test/drain"], "@moq/e2ee": ["@moq/e2ee@workspace:js/e2ee"], diff --git a/demo/carla/README.md b/demo/carla/README.md new file mode 100644 index 00000000000..f6ba35db1da --- /dev/null +++ b/demo/carla/README.md @@ -0,0 +1,233 @@ +# CARLA teleop demo + +Drive simulated trucks from a browser over MoQ. Trucks share one +[CARLA](https://carla.org) world and drive themselves until someone takes +control. Each truck publishes its cameras and a status track; a web player +watches them on a map and publishes a steering **control** track back. Control +follows the [moq-boy](../boy) pattern: the viewer publishes, the truck +subscribes, and the truck echoes what it accepted on `status`. + +```text +├── web/ # the player: map, video, keyboard and steering-wheel input (@moq/demo-carla) +├── sim/ # the worker: one Python process driving every truck in CARLA +│ ├── moq_carla_sim/ # safety gate, CARLA world, MoQ publish and control +│ └── systemd/ # rootless user units for an always-on workstation +├── bridge/ # Rust: the GPU video bridge the simulator loads (moq-carla-bridge) +└── justfile # `just carla ` +``` + +The simulator itself is a CARLA source build with the bridge's Unreal module, +in [moq-dev/carla](https://github.com/moq-dev/carla). + +## Running locally + +Needs a Linux host with an NVIDIA GPU and podman; the repo's dev shell +(`nix develop`, or direnv) provides everything else. Build the simulator image +with [moq-dev/carla](https://github.com/moq-dev/carla) and the worker's +virtualenv once, then leave the simulator running: + +```bash +git clone https://github.com/moq-dev/carla ../carla +../carla/build.sh . # from this checkout's root; prints the image tag +just carla venv +CARLA_IMAGE= just carla server # separate terminal +just carla # relay + every truck + web player +``` + +`just carla cdi` generates the rootless `nvidia.com/gpu` CDI device the +container runs with; rerun it after a driver update, and `just carla server` +refuses a stale one. On a SELinux host the container runs with +`label=disable`, because SELinux denies `/dev/nvidia*` in a way that looks like +a missing driver. + +The player alone runs against any relay with `just carla web `. Append +`?preview` to the local URL for a fixture-driven layout review that opens no +connection; it is removed from production builds. + +## Broadcasts + +```text +carla/{truck}/{camera}.hang <- one media broadcast per camera (hang catalog) + sd 240p H.264, for map previews + hd 720p H.264, for the focused view + status JSON Snapshot telemetry + +viewer/carla/{truck}/{driverId} <- one broadcast per driver + control JSON Snapshot commands +``` + +The worker consumes `viewer/carla/{truck}/` for each truck, fanning in every +driver's `control` track. Cameras are sibling broadcasts, so switching views +never creates a second claim on the same wheel, and the driver scope is +disjoint from every media path. + +### Control (player to truck) + +```json +{ "type": "drive", "sequence": 42, "steering": -1.0..1.0, + "throttle": 0..1, "brake": 0..1, + "gear": "neutral|forward|reverse", "timestamps": [...] } +{ "type": "estop", "sequence": 43, "timestamps": [...] } +{ "type": "reset_estop", "sequence": 44, "timestamps": [...] } +{ "type": "respawn", "sequence": 45, "timestamps": [...] } +``` + +### Status (truck to player) + +```json +{ "accepted": { "steering": .., "throttle": .., "brake": .., "gear": ".." }, + "autonomy": "autonomous|manual", "estopped": true, + "driver": "ab12cd", "estop_by": "ef34gh", "sequence": 44, + "media_pipeline": { "ab12cd": [ { "label": "received→decoded", "ms": 12 } ] }, + "map": "Town10HD_Opt", + "pose": { "x": 12.35, "y": -78.9, "yaw": 180.1, "speed": 9.88 }, + "measured": { "steering": .., "throttle": .., "brake": .., "gear": ".." }, + "motion": { "yaw_rate": 3.4, "accel_lon": 1.23, "accel_lat": -2.35 }, + "collision": { "at": 1756334400123, "other": "vehicle.tesla.model3", "impulse": 4210.6 } } +``` + +Status republishes every 100 ms whether or not anyone is driving, since an +autonomous truck changes no accepted state as it moves. `pose` is in CARLA +world metres (x east, y south, yaw clockwise from +x), which is what the +player's baked road maps use. + +`accepted` is the gate repeating what it let through. `measured`, `motion`, and +`collision` come from the vehicle itself, so the player can draw requested +against measured steering and show the gap. `collision.at` is the unix +millisecond contact began: a fixed instant gives the event an identity on a +snapshot track, so a late joiner never mistakes a retained collision for a +fresh one. Continuing contact with the same actor within a second is one +collision. + +### The safety gate + +The worker's gate (`sim/moq_carla_sim/drive.py`) owns every truck's controls: + +- **One driver at a time.** The first driver to send a command holds the + lease until its broadcast ends or its 1 s watchdog expires; the player + heartbeats every 250 ms. The lease is bound to the driver's path and the + epoch it announced with, so a driver that reconnects at the same path is a + new claimant rather than a continuation. +- **E-stop latches** until an explicit reset, from any viewer: the player + latches and resets it through a short-lived safety broadcast without taking + the lease. +- **Gear changes are interlocked**: the gate keeps the current gear unless the + command releases throttle and applies some brake. +- **An unclaimed truck goes back to autonomy**, not to full brake. It first + coasts onto the nearest lane running its way (`handback.py`), because + Traffic Manager steers a truck it takes back toward the nearest waypoint + whatever its heading. +- **A wedged truck respawns**: an autonomous truck that has not moved 2 m in + 5 minutes returns to a free spawn point. + +The lease is cooperative. Anyone who can publish under `viewer/carla/` can +announce a driver path, so it stops two honest drivers fighting over a truck, +not a deliberate takeover. Anything beyond a simulator needs per-driver tokens +scoped to the driver's own path. + +## The simulator + +The trucks listed in `sim/trucks.json` share one CARLA map, so they can see +each other. The worker is Python because CARLA's own client is a Python wheel +and moq's Python bindings (built from `py/` in this checkout by +`just carla venv`) cover publishing and the JSON tracks. CARLA advises a single +command-sending client, and one process keeps Traffic Manager coherent. + +- **Video follows demand.** A map preview subscribes to `sd` and the focused + view to `hd`. The simulator renders only the camera covering the largest + demanded rendition, scales the rest from it on the GPU, and parks the camera, + its encoders, and the world renderer once the last subscriber leaves. +- **So does the clock.** Stepping physics costs most of a core with rendering + parked, so the world stops once no viewer, status subscriber, or driver + needs it. +- **The world runs at a fixed step.** CARLA is in synchronous mode at 1/30 s, + so every frame is stamped exactly a frame apart instead of with however long + the last tick took. The worker returns the world to free-running on exit. +- **An idle simulator parks.** The server boots with rendering on and outlives + every client; `just carla park` disables rendering unless a worker holds the + simulator. + +Bake a town's roads for the player's map against a running simulator with +`just carla map`, and commit what it writes to `web/src/maps/`. See +[`web/src/maps/README.md`](web/src/maps/README.md) for an aligned aerial image. + +## The GPU video bridge + +Camera pixels never leave the GPU. `bridge/` is a library the simulator loads: +the CARLA plugin ([moq-dev/carla](https://github.com/moq-dev/carla)'s +`plugin/MoqBridge`) copies each camera's render target +into an exportable Vulkan image on Unreal's own queue, and the bridge hands it +to the `moq-video` encoder on the same device, which converts, scales, and +encodes it there. Compressed frames cross a Unix socket to the worker +(`sim/moq_carla_sim/bridge.py`), which publishes them on `sd` and `hd`. There +is no CPU path: a camera that cannot reach the bridge fails its spawn, and the +worker exits for systemd to restart. + +- **Bounded.** Each camera cycles a few slots; with none free it drops the + picture before anything encodes it, and a worker that falls behind loses + packets only up to the next keyframe. +- **Tested without CARLA.** A Vulkan stand-in for the plugin drives the + bridge: `just carla bridge-gpu` runs it on an NVIDIA host, and + `cargo run -p moq-carla-bridge --example standin -- truck1/front` + feeds a worker by hand. The plugin itself compiles only in the CARLA build. + +The C ABI the plugin calls is `bridge/include/carla_bridge.h`. moq-dev/carla +holds an identical copy that the plugin compiles against: change both +together, since its image build refuses a mismatch. + +## Keeping it running + +On a Linux workstation, the simulator and worker run as rootless systemd user +units. The installer records this checkout's path; rerun it after moving the +checkout. + +```bash +just carla venv +just carla service-install + +# The units read this. MOQ_URL carries a token allowed to publish `carla/**` +# and subscribe `viewer/carla/**`; keeping it here keeps it off the command +# line. CARLA_IMAGE is the tag the moq-dev/carla build printed, since the worker +# refuses a stock simulator. +cat >~/.config/moq/carla.env <<'EOF' +MOQ_URL=https://cdn.moq.dev/demo?jwt=... +CARLA_IMAGE=localhost/moq-carla:0.9.16-bridge-... +EOF +chmod 600 ~/.config/moq/carla.env + +just carla service-enable +just carla service-status +just carla service-logs +``` + +`carla-sim.service` owns the podman container and `carla-worker.service` the +Python process; both restart with capped backoff. Run +`sudo loginctl enable-linger "$USER"` once to start them at boot rather than +at login. + +## The player + +`just carla deploy` builds the player against `https://cdn.moq.dev/demo` and +deploys it to `carla.moq.dev`. With no simulator publishing, it says so. + +Hover a truck on the map to stream its 240p preview; click it for 720p. A drive +key, a wheel movement, or **Take control** claims the lease, and **Esc** +releases it. Inputs ramp rather than snap and hold where you leave them: +**A**/**D** steer, **C** recenters, **W** releases the brake then feeds +throttle, **S** cuts throttle then brakes, **Space** or **G** changes gear, +**E** e-stops, and **R** resets the e-stop. **Reset** returns the focused truck +to a free spawn point under autonomy. + +A steering wheel works through the +[Gamepad API](https://developer.mozilla.org/docs/Web/API/Gamepad_API). Unlike +the keys it is absolute: rim angle is steering angle and pedal travel is +throttle and brake. It takes over only once it moves with the throttle +released, so a resting foot cannot claim control, and losing an armed wheel +cuts the throttle and brakes. Click its name in the header to calibrate an +unfamiliar layout; the result is stored per device. + +## Checks + +`just carla check` lints and tests the worker and its launchers without CARLA +or a GPU. `just js` and `just rs` cover the player and the bridge, and +`just carla bridge-gpu` runs the bridge's hardware test. diff --git a/demo/carla/bridge/Cargo.toml b/demo/carla/bridge/Cargo.toml new file mode 100644 index 00000000000..215417a62f0 --- /dev/null +++ b/demo/carla/bridge/Cargo.toml @@ -0,0 +1,37 @@ +[package] +name = "moq-carla-bridge" +description = "CARLA video bridge: encodes Unreal render targets on the GPU and hands compressed frames to the demo's worker." +authors = ["Luke Curley "] +repository = "https://github.com/moq-dev/moq" +license = "MIT OR Apache-2.0" +publish = false + +version = "0.1.0" +edition = "2024" +rust-version.workspace = true + +[lib] +name = "carla_bridge" +# cdylib: what the CARLA plugin loads through include/carla_bridge.h. +# rlib: what the tests link. +crate-type = ["cdylib", "rlib"] + +# Linux only: elsewhere the crate is empty, so the workspace still builds on a Mac. +[target.'cfg(target_os = "linux")'.dependencies] +anyhow = { workspace = true } +bytes = { workspace = true } +moq-mux = { workspace = true } +moq-net = { workspace = true } +# The NVIDIA encoder, which imports the plugin's Vulkan images. The encoder is +# opened for the plugin's device, so no software fallback can take a frame. +moq-video = { workspace = true, features = ["nvidia"] } +parking_lot = "0.12" +# Blocking on a slot's GPU completion when a source closes, which is the one +# place the bridge waits on the GPU. +pollster = { workspace = true } +tracing = { workspace = true } + +[target.'cfg(target_os = "linux")'.dev-dependencies] +# The stand-in producer: a native Vulkan renderer that hands the bridge +# exportable slots exactly the way the CARLA plugin does. +ash = { workspace = true } diff --git a/demo/carla/bridge/examples/standin.rs b/demo/carla/bridge/examples/standin.rs new file mode 100644 index 00000000000..0e031218df1 --- /dev/null +++ b/demo/carla/bridge/examples/standin.rs @@ -0,0 +1,73 @@ +//! Feed the bridge without CARLA: the test's stand-in renderer drives an sd +//! and an hd camera of one stream at 30 fps, cycling colors, for exercising +//! the worker's bridge client, a relay and the player by hand. +//! +//! ```sh +//! cargo run -p moq-carla-bridge --example standin -- /tmp/carla-bridge.sock truck1/front +//! ``` +//! +//! Needs the host NVIDIA driver on the loader path, as `just carla bridge-gpu` +//! arranges. + +#[cfg(target_os = "linux")] +#[path = "../tests/standin/mod.rs"] +mod standin; + +#[cfg(target_os = "linux")] +use std::time::{Duration, Instant}; + +#[cfg(target_os = "linux")] +use carla_bridge::{Bridge, Size}; + +#[cfg(target_os = "linux")] +const STEP: Duration = Duration::from_nanos(33_333_333); + +// An example needs a `main` on every target, or `--all-targets` stops compiling +// on a Mac, so elsewhere it refuses instead of vanishing. +#[cfg(not(target_os = "linux"))] +fn main() -> Result<(), &'static str> { + Err("standin needs Linux and the NVIDIA driver") +} + +#[cfg(target_os = "linux")] +fn main() -> anyhow::Result<()> { + let mut args = std::env::args().skip(1); + let (Some(socket), Some(stream)) = (args.next(), args.next()) else { + anyhow::bail!("usage: standin "); + }; + let gpu = standin::Gpu::open().ok_or_else(|| anyhow::anyhow!("no NVIDIA Vulkan device"))?; + let bridge = Bridge::listen(&socket)?; + let mut cameras = [ + standin::Camera::new(&gpu, &bridge, &stream, Size::new(426, 240), 3), + standin::Camera::new(&gpu, &bridge, &stream, Size::new(1280, 720), 3), + ]; + eprintln!("standin: rendering {stream} for the worker at {socket}"); + + let start = Instant::now(); + let mut wanted = [false; 2]; + let mut frame = 0u64; + loop { + frame += 1; + let deadline = start + STEP * frame as u32; + std::thread::sleep(deadline.saturating_duration_since(Instant::now())); + // A slow sweep through the hue circle, so motion is visible. + let phase = frame as f64 / 90.0 * std::f64::consts::TAU; + let channel = |offset: f64| (128.0 + 100.0 * (phase + offset).sin()) as u8; + let rgb = [channel(0.0), channel(2.1), channel(4.2)]; + for (index, camera) in cameras.iter_mut().enumerate() { + let now = camera.wanted(); + if now != wanted[index] { + eprintln!( + "standin: {}x{} camera {}", + camera.size.width, + camera.size.height, + if now { "rendering" } else { "parked" } + ); + wanted[index] = now; + } + if now && !camera.render(rgb, frame, frame * 33_333) { + eprintln!("standin: frame {frame} dropped, every slot busy"); + } + } + } +} diff --git a/demo/carla/bridge/gpu.sh b/demo/carla/bridge/gpu.sh new file mode 100755 index 00000000000..ee0cd1b0d8a --- /dev/null +++ b/demo/carla/bridge/gpu.sh @@ -0,0 +1,57 @@ +#!/usr/bin/env bash +# Run a command against this host's NVIDIA driver and Vulkan loader, or skip on +# a host with no NVIDIA GPU. The GPU is found on the PCI bus rather than by its +# libraries, because the userspace libraries outlive a removed card. A card +# without a loaded driver is an error, so a host that should run the command +# never skips. CARLA_GPU_ROOT prefixes every probed path for the test. +set -euo pipefail + +root="${CARLA_GPU_ROOT:-}" + +gpu= +shopt -s nullglob +for device in "$root"/sys/bus/pci/devices/*; do + # NVIDIA's vendor ID with a display controller class (VGA, 3D, ...). + if [[ "$(<"$device/vendor")" == 0x10de && "$(<"$device/class")" == 0x03* ]]; then + gpu="${device##*/}" + break + fi +done +shopt -u nullglob + +if [[ -z "$gpu" ]]; then + echo "bridge-gpu: skipped, no NVIDIA GPU on this host" >&2 + exit 0 +fi + +if [[ ! -d "$root/proc/driver/nvidia" ]]; then + echo "bridge-gpu: NVIDIA GPU $gpu has no loaded kernel driver (no /proc/driver/nvidia); install the NVIDIA driver or load the nvidia module" >&2 + exit 1 +fi + +# NixOS keeps the loader outside the driver's directory, so they are found separately. +driver= +for candidate in "$root/run/opengl-driver/lib" "$root/usr/lib64" "$root/usr/lib/x86_64-linux-gnu" "$root/usr/lib"; do + if [[ -e "$candidate/libcuda.so.1" && -e "$candidate/libnvidia-encode.so.1" ]]; then + driver="$candidate" + break + fi +done +if [[ -z "$driver" ]]; then + echo "bridge-gpu: NVIDIA GPU $gpu has no driver libraries with CUDA and NVENC (libcuda.so.1, libnvidia-encode.so.1); install the NVIDIA driver's userspace" >&2 + exit 1 +fi + +vulkan= +for candidate in "$driver" "$root/run/current-system/sw/lib" "$root/usr/lib64" "$root/usr/lib/x86_64-linux-gnu" "$root/usr/lib"; do + if [[ -e "$candidate/libvulkan.so.1" ]]; then + vulkan="$candidate" + break + fi +done +if [[ -z "$vulkan" ]]; then + echo "bridge-gpu: the NVIDIA driver is in $driver but no Vulkan loader (libvulkan.so.1) was found; install one" >&2 + exit 1 +fi + +LD_LIBRARY_PATH="$driver:$vulkan${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" exec "$@" diff --git a/demo/carla/bridge/gpu.test.sh b/demo/carla/bridge/gpu.test.sh new file mode 100755 index 00000000000..ae9800e5ab7 --- /dev/null +++ b/demo/carla/bridge/gpu.test.sh @@ -0,0 +1,76 @@ +#!/usr/bin/env bash +# Offline regression for gpu.sh's NVIDIA detection against a fake host root. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +tmp="$(mktemp -d)" +trap 'rm -rf "$tmp"' EXIT + +pci() { + mkdir -p "$tmp/root/sys/bus/pci/devices/$1" + echo "$2" >"$tmp/root/sys/bus/pci/devices/$1/vendor" + echo "$3" >"$tmp/root/sys/bus/pci/devices/$1/class" +} + +# Runs gpu.sh against the fake root; the command prints the loader path it got. +run() { + CARLA_GPU_ROOT="$tmp/root" LD_LIBRARY_PATH='' bash "$here/gpu.sh" bash -c 'echo "ran $LD_LIBRARY_PATH"' >"$tmp/out" 2>&1 +} + +expect_skip() { + run || { + cat "$tmp/out" >&2 + echo "gpu.sh failed, expected a skip: $1" >&2 + exit 1 + } + if ! grep -Fq 'skipped, no NVIDIA GPU' "$tmp/out" || grep -Fq ran "$tmp/out"; then + cat "$tmp/out" >&2 + echo "gpu.sh did not skip: $1" >&2 + exit 1 + fi +} + +expect_error() { + if run; then + cat "$tmp/out" >&2 + echo "gpu.sh succeeded, expected an error: $1" >&2 + exit 1 + fi + grep -Fq "$2" "$tmp/out" +} + +# No /sys at all, like a Mac. +mkdir -p "$tmp/root" +expect_skip "no sysfs" + +# Leftover NVIDIA libraries with only Intel and AMD GPUs, plus an NVIDIA +# non-display function (an audio controller), must still skip. +mkdir -p "$tmp/root/usr/lib/x86_64-linux-gnu" +touch "$tmp/root/usr/lib/x86_64-linux-gnu/"{libcuda.so.1,libnvidia-encode.so.1,libvulkan.so.1} +pci 0000:00:02.0 0x8086 0x030000 +pci 0000:04:00.0 0x1002 0x030000 +pci 0000:04:00.1 0x10de 0x040300 +expect_skip "stale libraries without an NVIDIA GPU" + +# An NVIDIA GPU without the kernel driver loaded. +pci 0000:01:00.0 0x10de 0x030000 +expect_error "no kernel driver" "no loaded kernel driver" + +# The kernel driver without the userspace libraries. +mkdir -p "$tmp/root/proc/driver/nvidia" +rm "$tmp/root/usr/lib/x86_64-linux-gnu/"{libcuda.so.1,libnvidia-encode.so.1} +expect_error "no driver libraries" "no driver libraries" + +# The driver without a Vulkan loader. +touch "$tmp/root/usr/lib/x86_64-linux-gnu/"{libcuda.so.1,libnvidia-encode.so.1} +rm "$tmp/root/usr/lib/x86_64-linux-gnu/libvulkan.so.1" +expect_error "no Vulkan loader" "no Vulkan loader" + +# NixOS: the driver and loader live in separate directories. +mkdir -p "$tmp/root/run/opengl-driver/lib" "$tmp/root/run/current-system/sw/lib" +touch "$tmp/root/run/opengl-driver/lib/"{libcuda.so.1,libnvidia-encode.so.1} +touch "$tmp/root/run/current-system/sw/lib/libvulkan.so.1" +run +grep -Fxq "ran $tmp/root/run/opengl-driver/lib:$tmp/root/run/current-system/sw/lib" "$tmp/out" + +printf 'bridge GPU detection test: ok\n' diff --git a/demo/carla/bridge/include/carla_bridge.h b/demo/carla/bridge/include/carla_bridge.h new file mode 100644 index 00000000000..6ee64c639c1 --- /dev/null +++ b/demo/carla/bridge/include/carla_bridge.h @@ -0,0 +1,111 @@ +/* + * The CARLA demo's GPU video bridge, as the CARLA plugin sees it. + * + * A source is one camera. The plugin allocates `slots` exportable Vulkan + * images on Unreal's device, each a dedicated VK_IMAGE_TILING_OPTIMAL + * allocation of the capture size in R8G8B8A8 or B8G8R8A8 UNORM with + * VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT, plus one timeline semaphore + * per image exported as an opaque fd and starting at 0. Per rendered frame, + * on the RHI thread: + * + * if (carla_bridge_source_wanted(source) && + * carla_bridge_source_acquire(source, &slot, &ready) == 1) { + * record: barrier (after every earlier use of the image on this queue) + * -> copy the render target into image[slot] + * -> transition image[slot] to VK_IMAGE_LAYOUT_GENERAL + * vkQueueSubmit(..., signal timeline[slot] = ready) + * carla_bridge_source_submit(source, slot, frame, timestamp_us); + * } + * + * Nothing here blocks except carla_bridge_source_close. Functions returning + * int report -1 on failure with a message in carla_bridge_error(). A producer + * that cannot go on, whether for such an error or its own (a failed copy or + * submission), reports it with carla_bridge_source_fail, which the worker + * sees as the stream failing, then closes the source. + * + * This file has two identical copies: moq's + * demo/carla/bridge/include/carla_bridge.h, which the bridge library + * implements, and https://github.com/moq-dev/carla's include/carla_bridge.h, + * which the plugin compiles against. Change both together; the simulator + * image build refuses a mismatch. + */ + +#ifndef CARLA_BRIDGE_H +#define CARLA_BRIDGE_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CarlaBridgeSource CarlaBridgeSource; + +#define CARLA_BRIDGE_RGBA 0u +#define CARLA_BRIDGE_BGRA 1u + +/* + * Register a camera of stream `stream` (the worker's broadcast, e.g. + * "truck1/front"). The first call in a process starts the bridge listening on + * `socket`, which fails while another process's bridge holds that path; later + * calls must name the same path. `device_uuid` and `driver_uuid` are the + * 16-byte VkPhysicalDeviceIDProperties::deviceUUID and ::driverUUID of the + * rendering device; every camera of a stream renders on one device. Returns + * NULL on failure. + */ +CarlaBridgeSource *carla_bridge_source_open(const char *socket, const char *stream, const uint8_t *device_uuid, + const uint8_t *driver_uuid, uint32_t width, uint32_t height, uint32_t channels, + uint32_t slots); + +/* + * Import slot `slot`'s image memory and timeline semaphore, each exported as an + * opaque fd. Both descriptors are consumed whatever the result. + * `allocation_size` is the whole VkDeviceMemory size and `memory_type` the + * VkMemoryAllocateInfo::memoryTypeIndex it was allocated from; an opaque + * import must repeat both. + */ +int carla_bridge_source_import(CarlaBridgeSource *source, uint32_t slot, int memory_fd, int timeline_fd, + uint64_t allocation_size, uint32_t memory_type); + +/* Whether the stream wants this camera's pictures now: render only if so. */ +bool carla_bridge_source_wanted(const CarlaBridgeSource *source); + +/* + * Take an idle slot. Returns 1 with the slot and the timeline value the copy's + * submission must signal, 0 when every slot is busy (drop this picture), or -1. + */ +int carla_bridge_source_acquire(CarlaBridgeSource *source, uint32_t *slot, uint64_t *ready); + +/* + * Hand an acquired slot to the encoder once its submission, including the + * `ready` signal, is queued. `frame` is CARLA's frame counter and + * `timestamp_us` the simulation time of the picture. + */ +int carla_bridge_source_submit(CarlaBridgeSource *source, uint32_t slot, uint64_t frame, uint64_t timestamp_us); + +/* + * The producer can no longer feed this camera, for `message`. The stream stops + * wanting it and fails the worker's session for the stream, and every session + * opened on it until another camera registers; close the source next. Never + * blocks. + */ +int carla_bridge_source_fail(CarlaBridgeSource *source, const char *message); + +/* Return an acquired slot without submitting it, when recording failed. */ +int carla_bridge_source_cancel(CarlaBridgeSource *source, uint32_t slot); + +/* + * Stop the camera and free `source`. Blocks until the GPU has finished with + * every slot, after which the images and semaphores may be destroyed. + */ +void carla_bridge_source_close(CarlaBridgeSource *source); + +/* The calling thread's last error, valid until its next bridge call. */ +const char *carla_bridge_error(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/demo/carla/bridge/src/ffi.rs b/demo/carla/bridge/src/ffi.rs new file mode 100644 index 00000000000..615b3b0bce6 --- /dev/null +++ b/demo/carla/bridge/src/ffi.rs @@ -0,0 +1,253 @@ +//! The C ABI the CARLA plugin calls, declared in `include/carla_bridge.h`. +//! +//! One bridge per process, created by the first source that names a socket. +//! Every entry point reports failure through its return value and +//! [`carla_bridge_error`]; a panic is caught at the boundary and reported the +//! same way rather than unwinding into Unreal. + +use std::cell::RefCell; +use std::ffi::{CStr, CString, c_char, c_int}; +use std::os::fd::{FromRawFd, OwnedFd}; +use std::panic::{AssertUnwindSafe, catch_unwind}; +use std::path::Path; + +use anyhow::{Context, Result, anyhow, bail}; +use parking_lot::Mutex; + +use crate::{Bridge, Device, Format, Size, Source}; + +static BRIDGE: Mutex> = Mutex::new(None); + +thread_local! { + static ERROR: RefCell = RefCell::new(CString::default()); +} + +/// Opaque to C. +pub struct CarlaBridgeSource(Source); + +fn guard(failed: T, body: impl FnOnce() -> Result) -> T { + let error = match catch_unwind(AssertUnwindSafe(body)) { + Ok(Ok(value)) => return value, + Ok(Err(err)) => format!("{err:#}"), + Err(panic) => { + let message = panic + .downcast_ref::<&str>() + .map(|s| s.to_string()) + .or_else(|| panic.downcast_ref::().cloned()) + .unwrap_or_default(); + format!("CARLA bridge panicked: {message}") + } + }; + tracing::error!(%error, "CARLA bridge call failed"); + let error = CString::new(error.replace('\0', " ")).expect("NULs replaced"); + ERROR.with(|slot| *slot.borrow_mut() = error); + failed +} + +/// # Safety +/// `value` is NULL or a NUL-terminated string. +unsafe fn text<'a>(value: *const c_char, what: &str) -> Result<&'a str> { + if value.is_null() { + bail!("{what} is NULL"); + } + // SAFETY: the caller passes a NUL-terminated string. + unsafe { CStr::from_ptr(value) } + .to_str() + .with_context(|| format!("{what} is not UTF-8")) +} + +/// # Safety +/// `source` is NULL or a live pointer from `carla_bridge_source_open`. +unsafe fn source<'a>(source: *const CarlaBridgeSource) -> Result<&'a Source> { + // SAFETY: the caller passes a live source. + unsafe { source.as_ref() } + .map(|source| &source.0) + .ok_or_else(|| anyhow!("source is NULL")) +} + +/// Register a camera; see `include/carla_bridge.h`. +/// +/// # Safety +/// `socket` and `stream` are NUL-terminated strings, and `device_uuid` and +/// `driver_uuid` each point at 16 bytes. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_open( + socket: *const c_char, + stream: *const c_char, + device_uuid: *const u8, + driver_uuid: *const u8, + width: u32, + height: u32, + channels: u32, + slots: u32, +) -> *mut CarlaBridgeSource { + guard(std::ptr::null_mut(), || { + // SAFETY: forwarded from the caller. + let socket = unsafe { text(socket, "socket") }?; + // SAFETY: forwarded from the caller. + let stream = unsafe { text(stream, "stream") }?; + if device_uuid.is_null() || driver_uuid.is_null() { + bail!("device or driver UUID is NULL"); + } + // SAFETY: the caller passes 16 readable bytes for each. + let device = Device { + device_uuid: unsafe { *device_uuid.cast::<[u8; 16]>() }, + driver_uuid: unsafe { *driver_uuid.cast::<[u8; 16]>() }, + render_node: None, + }; + let format = match channels { + 0 => Format::Rgba8, + 1 => Format::Bgra8, + other => bail!("unknown channel order {other}"), + }; + + let mut bridge = BRIDGE.lock(); + let bridge = match &mut *bridge { + Some(bridge) if bridge.path() == Path::new(socket) => bridge, + Some(bridge) => bail!("the bridge already listens on {}", bridge.path().display()), + None => bridge.insert(Bridge::listen(socket)?), + }; + let source = bridge.source( + stream, + device, + Size::new(width, height), + format, + slots as usize, + )?; + Ok(Box::into_raw(Box::new(CarlaBridgeSource(source)))) + }) +} + +/// # Safety +/// `source` is a live source; the descriptors are owned by the caller and +/// consumed by this call whatever it returns. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_import( + source_: *mut CarlaBridgeSource, + slot: u32, + memory_fd: c_int, + timeline_fd: c_int, + allocation_size: u64, + memory_type: u32, +) -> c_int { + // Owned first, so an early error still closes them. + // SAFETY: the caller transfers ownership of both descriptors. + let memory = (memory_fd >= 0).then(|| unsafe { OwnedFd::from_raw_fd(memory_fd) }); + // SAFETY: as above. + let timeline = (timeline_fd >= 0).then(|| unsafe { OwnedFd::from_raw_fd(timeline_fd) }); + guard(-1, || { + let (Some(memory), Some(timeline)) = (memory, timeline) else { + bail!("invalid file descriptor"); + }; + // SAFETY: forwarded from the caller. + unsafe { source(source_) }?.import( + slot as usize, + memory, + timeline, + allocation_size, + memory_type, + )?; + Ok(0) + }) +} + +/// # Safety +/// `source` is a live source. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_wanted(source_: *const CarlaBridgeSource) -> bool { + // SAFETY: forwarded from the caller. + guard(false, || Ok(unsafe { source(source_) }?.wanted())) +} + +/// # Safety +/// `source` is a live source and `slot`/`ready` are writable. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_acquire( + source_: *mut CarlaBridgeSource, + slot: *mut u32, + ready: *mut u64, +) -> c_int { + guard(-1, || { + if slot.is_null() || ready.is_null() { + bail!("output pointer is NULL"); + } + // SAFETY: forwarded from the caller. + let Some((index, value)) = unsafe { source(source_) }?.acquire()? else { + return Ok(0); + }; + // SAFETY: the caller passes writable outputs. + unsafe { + *slot = index as u32; + *ready = value; + } + Ok(1) + }) +} + +/// # Safety +/// `source` is a live source. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_submit( + source_: *mut CarlaBridgeSource, + slot: u32, + frame: u64, + timestamp_us: u64, +) -> c_int { + guard(-1, || { + // SAFETY: forwarded from the caller. + unsafe { source(source_) }?.submit(slot as usize, frame, timestamp_us)?; + Ok(0) + }) +} + +/// # Safety +/// `source` is a live source. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_cancel( + source_: *mut CarlaBridgeSource, + slot: u32, +) -> c_int { + guard(-1, || { + // SAFETY: forwarded from the caller. + unsafe { source(source_) }?.cancel(slot as usize)?; + Ok(0) + }) +} + +/// Report that the producer can no longer feed a camera; see +/// `include/carla_bridge.h`. +/// +/// # Safety +/// `source` is a live source and `message` a NUL-terminated string. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_fail( + source_: *mut CarlaBridgeSource, + message: *const c_char, +) -> c_int { + guard(-1, || { + // SAFETY: forwarded from the caller. + let message = unsafe { text(message, "message") }?; + // SAFETY: forwarded from the caller. + unsafe { source(source_) }?.fail(message); + Ok(0) + }) +} + +/// # Safety +/// `source` is NULL or a live source, which this call frees. +#[unsafe(no_mangle)] +pub unsafe extern "C" fn carla_bridge_source_close(source_: *mut CarlaBridgeSource) { + guard((), || { + if !source_.is_null() { + // SAFETY: the caller hands back the pointer `open` returned, once. + drop(unsafe { Box::from_raw(source_) }); + } + Ok(()) + }) +} + +/// The calling thread's last error, valid until its next bridge call. +#[unsafe(no_mangle)] +pub extern "C" fn carla_bridge_error() -> *const c_char { + ERROR.with(|slot| slot.borrow().as_ptr()) +} diff --git a/demo/carla/bridge/src/lib.rs b/demo/carla/bridge/src/lib.rs new file mode 100644 index 00000000000..a640dcbb056 --- /dev/null +++ b/demo/carla/bridge/src/lib.rs @@ -0,0 +1,218 @@ +//! The native half of the CARLA demo's GPU video path. +//! +//! CARLA renders each truck camera with Unreal. The CARLA plugin copies the +//! render target on the GPU into an exportable Vulkan image it shares with this +//! library, which hands it to moq-video's encoder opened for the plugin's +//! device: NVENC converts, scales, and encodes it there. Compressed frames +//! leave through a Unix socket to the Python worker, which publishes them on +//! the truck's MoQ broadcast. Raw pixels never reach CPU memory, and nothing +//! here falls back to a CPU path: an unsupported device or a failed GPU step is +//! an error. +//! +//! - [`Bridge`] listens for the worker and owns the streams. The CARLA plugin +//! holds one per process through the C ABI in `include/carla_bridge.h`. +//! - A stream is one worker broadcast (`truck1/front`) with its renditions, +//! demand and encoders. Cameras register as sources of a stream by name. +//! - A [`Source`] is one camera's bounded set of exportable image slots. +//! +//! The worker protocol is [`protocol`]. +//! +//! Linux only; the crate is empty elsewhere. +#![cfg(target_os = "linux")] + +mod ffi; +pub mod protocol; +mod server; +mod source; +mod stream; + +use std::collections::HashMap; +use std::fs::{File, TryLockError}; +use std::os::unix::fs::FileTypeExt; +use std::os::unix::net::{UnixListener, UnixStream}; +use std::path::{Path, PathBuf}; +use std::sync::Arc; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::thread::JoinHandle; + +use anyhow::{Context, Result, bail}; +use parking_lot::Mutex; + +pub use moq_video::Size; +pub use moq_video::frame::vulkan::{Device, Format}; +pub use source::Source; + +use server::Output; +use stream::Stream; + +/// The bridge: a socket for the worker and the streams it encodes. +pub struct Bridge { + shared: Arc, + listener: Option>, + /// Held for the bridge's life: whoever holds it owns the socket path, so a + /// second process refuses the path instead of unlinking a live listener. + _lock: File, +} + +pub(crate) struct Shared { + path: PathBuf, + output: Arc, + streams: Mutex>>, + connection: Mutex>, + closing: AtomicBool, +} + +impl Bridge { + /// Listen for the worker at `path`, replacing a socket a previous process + /// left behind. Another live bridge on `path` is an error. + pub fn listen(path: impl Into) -> Result { + let path = path.into(); + // A socket file cannot say whether its listener is alive, and probing it + // with a connection would displace that bridge's worker. An flock beside + // it can: the kernel releases it when its holder exits. + let mut lock_path = path.clone().into_os_string(); + lock_path.push(".lock"); + let lock_path = PathBuf::from(lock_path); + let lock = File::options() + .write(true) + .create(true) + .truncate(false) + .open(&lock_path) + .with_context(|| format!("open {}", lock_path.display()))?; + match lock.try_lock() { + Ok(()) => {} + Err(TryLockError::WouldBlock) => { + bail!("another bridge is listening on {}", path.display()) + } + Err(TryLockError::Error(err)) => { + return Err(err).with_context(|| format!("lock {}", lock_path.display())); + } + } + match std::fs::symlink_metadata(&path) { + Ok(meta) if meta.file_type().is_socket() => { + std::fs::remove_file(&path) + .with_context(|| format!("remove the stale socket {}", path.display()))?; + } + Ok(_) => bail!("{} exists and is not a socket", path.display()), + Err(err) if err.kind() == std::io::ErrorKind::NotFound => {} + Err(err) => return Err(err).with_context(|| format!("inspect {}", path.display())), + } + let listener = + UnixListener::bind(&path).with_context(|| format!("listen on {}", path.display()))?; + let shared = Arc::new(Shared { + path, + output: Arc::default(), + streams: Mutex::default(), + connection: Mutex::default(), + closing: AtomicBool::new(false), + }); + let listener = std::thread::Builder::new() + .name("carla-bridge:listen".into()) + .spawn({ + let shared = shared.clone(); + move || server::listen(&shared, listener) + }) + .context("start the bridge listener")?; + Ok(Self { + shared, + listener: Some(listener), + _lock: lock, + }) + } + + /// The socket the worker connects to. + pub fn path(&self) -> &Path { + &self.shared.path + } + + /// Register a camera of `stream` rendering `size` images in `format` on the + /// Vulkan `device`, with `slots` images to cycle. Every camera of a stream + /// renders on one device. + pub fn source( + &self, + stream: &str, + device: Device, + size: Size, + format: Format, + slots: usize, + ) -> Result { + let stream = self.shared.stream(stream)?; + Source::new(stream, device, size, format, slots) + } +} + +impl Drop for Bridge { + fn drop(&mut self) { + self.shared.closing.store(true, Ordering::Release); + // Wake the blocking accept; it sees `closing` and exits. + let _ = UnixStream::connect(&self.shared.path); + if let Some(listener) = self.listener.take() { + let _ = listener.join(); + } + if let Some((_, socket)) = self.shared.connection.lock().take() { + let _ = socket.shutdown(std::net::Shutdown::Both); + } + for stream in self.shared.streams.lock().drain().map(|(_, stream)| stream) { + stream.shutdown(); + } + let _ = std::fs::remove_file(&self.shared.path); + } +} + +impl Shared { + fn closing(&self) -> bool { + self.closing.load(Ordering::Acquire) + } + + /// The stream called `name`, started on first use by either side. + fn stream(&self, name: &str) -> Result> { + let mut streams = self.streams.lock(); + if let Some(stream) = streams.get(name) { + return Ok(stream.clone()); + } + let stream = Stream::spawn(name.to_owned(), self.output.clone())?; + streams.insert(name.to_owned(), stream.clone()); + Ok(stream) + } + + /// Make `socket` the worker connection, returning the one it replaces. + fn replace_connection(&self, key: u64, socket: UnixStream) -> Option { + self.connection + .lock() + .replace((key, socket)) + .map(|(_, socket)| socket) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn socket_path(name: &str) -> PathBuf { + std::env::temp_dir().join(format!("carla-bridge-{name}-{}.sock", std::process::id())) + } + + #[test] + fn a_live_bridge_keeps_its_socket() { + let path = socket_path("live"); + let first = Bridge::listen(&path).unwrap(); + let err = Bridge::listen(&path) + .err() + .expect("a second bridge on the path"); + assert!(err.to_string().contains("another bridge"), "{err:#}"); + // The first bridge still owns a socket that accepts connections. + UnixStream::connect(&path).expect("the first bridge still listens"); + drop(first); + } + + #[test] + fn a_stale_socket_is_replaced() { + let path = socket_path("stale"); + let _ = std::fs::remove_file(&path); + // A listener that exited without unlinking, like a crashed process. + drop(UnixListener::bind(&path).unwrap()); + let bridge = Bridge::listen(&path).unwrap(); + UnixStream::connect(&path).expect("the new bridge listens"); + drop(bridge); + } +} diff --git a/demo/carla/bridge/src/protocol.rs b/demo/carla/bridge/src/protocol.rs new file mode 100644 index 00000000000..2eb30908157 --- /dev/null +++ b/demo/carla/bridge/src/protocol.rs @@ -0,0 +1,523 @@ +//! The encoded-frame protocol between the bridge and the demo's worker. +//! +//! The bridge runs inside CARLA; the worker is the Python process that owns the +//! MoQ broadcasts. They share one Unix stream socket that carries compressed +//! video and control only: raw pixels never cross it. +//! +//! Every message is `u32 length | u8 kind | body`, where `length` counts the +//! kind byte and the body. Integers are little-endian, a string is +//! `u16 length | UTF-8`, and a blob is `u32 length | bytes`. +//! `moq_carla_sim/bridge.py` mirrors this module and tests the same vectors. + +use std::io::{self, Read}; + +use bytes::{Buf, BufMut, Bytes, BytesMut}; + +/// Bumped on any incompatible change; both sides refuse a mismatch. +pub const VERSION: u16 = 1; + +/// The largest message either side accepts. A 720p IDR is a few hundred kB. +pub const MAX_MESSAGE: u32 = 16 << 20; + +/// Renditions per stream, so demand fits a one-byte mask. +pub const MAX_RENDITIONS: usize = 8; + +const HELLO: u8 = 0x01; +const OPEN: u8 = 0x02; +const DEMAND: u8 = 0x03; +const KEYFRAME: u8 = 0x04; +const CLOSE: u8 = 0x05; + +const EVENT_HELLO: u8 = 0x81; +const EVENT_CONFIG: u8 = 0x82; +const EVENT_FRAME: u8 = 0x83; +const EVENT_ERROR: u8 = 0x84; + +/// One encoded size of a stream. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct Rendition { + /// The MoQ track name the worker publishes it as. + pub name: String, + pub width: u32, + pub height: u32, + /// Target bitrate in bits per second. + pub bitrate: u64, + /// Frames per second the simulation steps at. + pub framerate: u32, +} + +/// Worker to bridge. +#[derive(Clone, Debug, PartialEq, Eq)] +pub enum Request { + /// The first message on a connection. + Hello { version: u16 }, + /// Encode the capture sources registered as `name` as stream `id`. The + /// bridge answers with one [`Event::Config`] per rendition, in order. + Open { + id: u32, + name: String, + renditions: Vec, + }, + /// The renditions that have subscribers, as a mask of `Open` indices. + Demand { id: u32, renditions: u8 }, + /// Encode the next frame of one rendition as an IDR. + Keyframe { id: u32, rendition: u8 }, + /// Stop encoding stream `id`. + Close { id: u32 }, +} + +/// Bridge to worker. +#[derive(Clone, Debug, PartialEq, Eq)] +pub enum Event { + /// The answer to [`Request::Hello`]. + Hello { version: u16 }, + /// A rendition's Annex-B parameter sets, sent before any of its frames so + /// the worker can advertise the track before anyone subscribes. + Config { id: u32, rendition: u8, init: Bytes }, + /// One encoded access unit. + Frame(Frame), + /// A failure. Stream 0 is the connection itself; any other stream has + /// stopped encoding. + Error { id: u32, message: String }, +} + +/// One encoded access unit and where it came from. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct Frame { + pub id: u32, + pub rendition: u8, + /// CARLA's frame counter at capture. + pub frame: u64, + /// Simulation time at capture, in microseconds. + pub timestamp_us: u64, + /// An IDR: a subscriber can start decoding here. + pub keyframe: bool, + /// Annex-B, with in-band parameter sets on every IDR. + pub payload: Bytes, +} + +impl Request { + pub fn encode(&self) -> Bytes { + let mut out = Message::new(); + match self { + Self::Hello { version } => { + out.kind(HELLO); + out.buf.put_u16_le(*version); + } + Self::Open { + id, + name, + renditions, + } => { + out.kind(OPEN); + out.buf.put_u32_le(*id); + out.string(name); + out.buf.put_u8(renditions.len() as u8); + for rendition in renditions { + out.string(&rendition.name); + out.buf.put_u32_le(rendition.width); + out.buf.put_u32_le(rendition.height); + out.buf.put_u64_le(rendition.bitrate); + out.buf.put_u32_le(rendition.framerate); + } + } + Self::Demand { id, renditions } => { + out.kind(DEMAND); + out.buf.put_u32_le(*id); + out.buf.put_u8(*renditions); + } + Self::Keyframe { id, rendition } => { + out.kind(KEYFRAME); + out.buf.put_u32_le(*id); + out.buf.put_u8(*rendition); + } + Self::Close { id } => { + out.kind(CLOSE); + out.buf.put_u32_le(*id); + } + } + out.finish() + } + + pub fn decode(kind: u8, mut body: Bytes) -> io::Result { + let body = &mut body; + let request = match kind { + HELLO => Self::Hello { + version: u16_le(body)?, + }, + OPEN => { + let id = u32_le(body)?; + let name = string(body)?; + let count = usize::from(u8_(body)?); + if count == 0 || count > MAX_RENDITIONS { + return Err(invalid(format!("stream {name:?} has {count} renditions"))); + } + let mut renditions = Vec::with_capacity(count); + for _ in 0..count { + renditions.push(Rendition { + name: string(body)?, + width: u32_le(body)?, + height: u32_le(body)?, + bitrate: u64_le(body)?, + framerate: u32_le(body)?, + }); + } + Self::Open { + id, + name, + renditions, + } + } + DEMAND => Self::Demand { + id: u32_le(body)?, + renditions: u8_(body)?, + }, + KEYFRAME => Self::Keyframe { + id: u32_le(body)?, + rendition: u8_(body)?, + }, + CLOSE => Self::Close { id: u32_le(body)? }, + kind => return Err(invalid(format!("unknown request kind {kind:#04x}"))), + }; + end(body)?; + Ok(request) + } +} + +impl Event { + pub fn encode(&self) -> Bytes { + let mut out = Message::new(); + match self { + Self::Hello { version } => { + out.kind(EVENT_HELLO); + out.buf.put_u16_le(*version); + } + Self::Config { + id, + rendition, + init, + } => { + out.kind(EVENT_CONFIG); + out.buf.put_u32_le(*id); + out.buf.put_u8(*rendition); + out.blob(init); + } + Self::Frame(frame) => { + out.kind(EVENT_FRAME); + out.buf.put_u32_le(frame.id); + out.buf.put_u8(frame.rendition); + out.buf.put_u64_le(frame.frame); + out.buf.put_u64_le(frame.timestamp_us); + out.buf.put_u8(u8::from(frame.keyframe)); + out.blob(&frame.payload); + } + Self::Error { id, message } => { + out.kind(EVENT_ERROR); + out.buf.put_u32_le(*id); + out.string(message); + } + } + out.finish() + } + + pub fn decode(kind: u8, mut body: Bytes) -> io::Result { + let body = &mut body; + let event = match kind { + EVENT_HELLO => Self::Hello { + version: u16_le(body)?, + }, + EVENT_CONFIG => Self::Config { + id: u32_le(body)?, + rendition: u8_(body)?, + init: blob(body)?, + }, + EVENT_FRAME => Self::Frame(Frame { + id: u32_le(body)?, + rendition: u8_(body)?, + frame: u64_le(body)?, + timestamp_us: u64_le(body)?, + keyframe: match u8_(body)? { + 0 => false, + 1 => true, + other => return Err(invalid(format!("keyframe flag {other}"))), + }, + payload: blob(body)?, + }), + EVENT_ERROR => Self::Error { + id: u32_le(body)?, + message: string(body)?, + }, + kind => return Err(invalid(format!("unknown event kind {kind:#04x}"))), + }; + end(body)?; + Ok(event) + } +} + +/// Read one message's kind and body, or `None` at a clean end of stream, which +/// is only between messages: a torn header is an error like a torn body. +pub fn read(reader: &mut impl Read) -> io::Result> { + let mut length = [0u8; 4]; + let mut filled = 0; + while filled < length.len() { + match reader.read(&mut length[filled..]) { + Ok(0) if filled == 0 => return Ok(None), + Ok(0) => { + return Err(io::Error::new( + io::ErrorKind::UnexpectedEof, + "end of stream inside a message header", + )); + } + Ok(read) => filled += read, + Err(err) if err.kind() == io::ErrorKind::Interrupted => {} + Err(err) => return Err(err), + } + } + let length = u32::from_le_bytes(length); + if length == 0 || length > MAX_MESSAGE { + return Err(invalid(format!("message length {length}"))); + } + let mut message = vec![0u8; length as usize]; + reader.read_exact(&mut message)?; + let mut message = Bytes::from(message); + let kind = message.get_u8(); + Ok(Some((kind, message))) +} + +struct Message { + buf: BytesMut, +} + +impl Message { + fn new() -> Self { + let mut buf = BytesMut::new(); + // The length is patched in by `finish`. + buf.put_u32_le(0); + Self { buf } + } + + fn kind(&mut self, kind: u8) { + self.buf.put_u8(kind); + } + + fn string(&mut self, value: &str) { + let len = u16::try_from(value.len()).expect("protocol string longer than 64 KiB"); + self.buf.put_u16_le(len); + self.buf.put_slice(value.as_bytes()); + } + + fn blob(&mut self, value: &[u8]) { + let len = u32::try_from(value.len()).expect("protocol blob longer than 4 GiB"); + self.buf.put_u32_le(len); + self.buf.put_slice(value); + } + + fn finish(mut self) -> Bytes { + let length = u32::try_from(self.buf.len() - 4).expect("protocol message longer than 4 GiB"); + self.buf[..4].copy_from_slice(&length.to_le_bytes()); + self.buf.freeze() + } +} + +fn invalid(message: String) -> io::Error { + io::Error::new(io::ErrorKind::InvalidData, message) +} + +fn need(body: &Bytes, len: usize) -> io::Result<()> { + if body.remaining() < len { + return Err(invalid(format!( + "truncated message: need {len} bytes, have {}", + body.remaining() + ))); + } + Ok(()) +} + +fn u8_(body: &mut Bytes) -> io::Result { + need(body, 1)?; + Ok(body.get_u8()) +} + +fn u16_le(body: &mut Bytes) -> io::Result { + need(body, 2)?; + Ok(body.get_u16_le()) +} + +fn u32_le(body: &mut Bytes) -> io::Result { + need(body, 4)?; + Ok(body.get_u32_le()) +} + +fn u64_le(body: &mut Bytes) -> io::Result { + need(body, 8)?; + Ok(body.get_u64_le()) +} + +fn string(body: &mut Bytes) -> io::Result { + let len = usize::from(u16_le(body)?); + need(body, len)?; + String::from_utf8(body.split_to(len).to_vec()).map_err(|err| invalid(format!("string: {err}"))) +} + +fn blob(body: &mut Bytes) -> io::Result { + let len = u32_le(body)? as usize; + need(body, len)?; + Ok(body.split_to(len)) +} + +fn end(body: &Bytes) -> io::Result<()> { + if body.has_remaining() { + return Err(invalid(format!("{} trailing bytes", body.remaining()))); + } + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + // Shared with `moq_carla_sim/bridge.py`'s tests: both sides must produce and + // accept exactly these bytes. + const HELLO_HEX: &str = "03000000010100"; + const OPEN_HEX: &str = "2c00000002070000000c00747275636b312f66726f6e740102007364aa010000f000000000350c00000000001e000000"; + const DEMAND_HEX: &str = "06000000030700000002"; + const KEYFRAME_HEX: &str = "06000000040700000001"; + const CLOSE_HEX: &str = "050000000507000000"; + const EVENT_HELLO_HEX: &str = "03000000810100"; + const CONFIG_HEX: &str = "0f000000820700000000050000000000000167"; + const FRAME_HEX: &str = + "20000000830700000001d204000000000000404b4c000000000001050000000000000165"; + const ERROR_HEX: &str = "0b00000084000000000400626f6f6d"; + + fn requests() -> Vec<(Request, &'static str)> { + vec![ + (Request::Hello { version: 1 }, HELLO_HEX), + ( + Request::Open { + id: 7, + name: "truck1/front".into(), + renditions: vec![Rendition { + name: "sd".into(), + width: 426, + height: 240, + bitrate: 800_000, + framerate: 30, + }], + }, + OPEN_HEX, + ), + ( + Request::Demand { + id: 7, + renditions: 0b10, + }, + DEMAND_HEX, + ), + ( + Request::Keyframe { + id: 7, + rendition: 1, + }, + KEYFRAME_HEX, + ), + (Request::Close { id: 7 }, CLOSE_HEX), + ] + } + + fn events() -> Vec<(Event, &'static str)> { + vec![ + (Event::Hello { version: 1 }, EVENT_HELLO_HEX), + ( + Event::Config { + id: 7, + rendition: 0, + init: Bytes::from_static(&[0, 0, 0, 1, 0x67]), + }, + CONFIG_HEX, + ), + ( + Event::Frame(Frame { + id: 7, + rendition: 1, + frame: 1234, + timestamp_us: 5_000_000, + keyframe: true, + payload: Bytes::from_static(&[0, 0, 0, 1, 0x65]), + }), + FRAME_HEX, + ), + ( + Event::Error { + id: 0, + message: "boom".into(), + }, + ERROR_HEX, + ), + ] + } + + fn hex(bytes: &[u8]) -> String { + bytes.iter().map(|b| format!("{b:02x}")).collect() + } + + fn unhex(text: &str) -> Vec { + (0..text.len()) + .step_by(2) + .map(|i| u8::from_str_radix(&text[i..i + 2], 16).unwrap()) + .collect() + } + + #[test] + fn requests_match_the_shared_vectors() { + for (request, expected) in requests() { + assert_eq!(hex(&request.encode()), expected, "{request:?}"); + let wire = unhex(expected); + let (kind, body) = read(&mut &wire[..]).unwrap().unwrap(); + assert_eq!(Request::decode(kind, body).unwrap(), request); + } + } + + #[test] + fn events_match_the_shared_vectors() { + for (event, expected) in events() { + assert_eq!(hex(&event.encode()), expected, "{event:?}"); + let wire = unhex(expected); + let (kind, body) = read(&mut &wire[..]).unwrap().unwrap(); + assert_eq!(Event::decode(kind, body).unwrap(), event); + } + } + + #[test] + fn rejects_malformed_messages() { + // Truncated body, and a header torn after 1 to 3 bytes. + let wire = unhex("06000000030700"); + assert!(read(&mut &wire[..]).is_err()); + for torn in 1..4 { + assert!(read(&mut &wire[..torn]).is_err(), "{torn} header bytes"); + } + // Trailing bytes after a complete request. + let (kind, body) = read(&mut &unhex("0700000003070000000200")[..]) + .unwrap() + .unwrap(); + assert!(Request::decode(kind, body).is_err()); + // Unknown kind. + let (kind, body) = read(&mut &unhex("0100000009")[..]).unwrap().unwrap(); + assert!(Request::decode(kind, body).is_err()); + // Too many renditions. + let mut open = Request::Open { + id: 1, + name: "x".into(), + renditions: Vec::new(), + } + .encode() + .to_vec(); + assert!(Request::decode(open[4], Bytes::copy_from_slice(&open[5..])).is_err()); + open[4 + 1 + 4 + 3] = 9; + assert!(Request::decode(open[4], Bytes::copy_from_slice(&open[5..])).is_err()); + // Zero and oversized lengths. + assert!(read(&mut &unhex("00000000")[..]).is_err()); + assert!(read(&mut &unhex("ffffffff01")[..]).is_err()); + // A clean end of stream between messages. + assert!(read(&mut &[][..]).unwrap().is_none()); + } +} diff --git a/demo/carla/bridge/src/server.rs b/demo/carla/bridge/src/server.rs new file mode 100644 index 00000000000..f26a4a8e256 --- /dev/null +++ b/demo/carla/bridge/src/server.rs @@ -0,0 +1,346 @@ +//! The worker's side of the socket: one connection at a time. +//! +//! The newest connection wins. A restarted worker reconnects while the old +//! socket may still look open, so a new connection shuts the old one down and +//! closes every stream it opened. + +use std::collections::HashMap; +use std::io::{self, BufReader, BufWriter, Write}; +use std::os::unix::net::{UnixListener, UnixStream}; +use std::sync::Arc; +use std::sync::mpsc::{self, SyncSender, TrySendError}; + +use anyhow::{Context, Result, bail}; +use bytes::Bytes; +use parking_lot::Mutex; + +use crate::Shared; +use crate::protocol::{self, Event, MAX_RENDITIONS, Request, VERSION}; +use crate::stream::{Session, SessionId, Stream}; + +/// Encoded messages queued for the worker: about two seconds of two +/// renditions at 30 fps. Beyond that the worker is not keeping up, and a +/// stream drops to its next IDR rather than queue without bound. +const OUTPUT_DEPTH: usize = 128; + +/// Where encoded frames go: the current connection's writer, if any. +#[derive(Default)] +pub(crate) struct Output { + current: Mutex)>>, +} + +pub(crate) enum Sent { + Queued, + /// The worker is behind; the message was dropped. + Full, + /// That connection is gone; its streams are closing. + Gone, +} + +impl Output { + fn sender(&self, connection: u64) -> Option> { + match &*self.current.lock() { + Some((current, sender)) if *current == connection => Some(sender.clone()), + _ => None, + } + } + + /// Queue a frame without waiting. + pub fn try_send(&self, connection: u64, event: &Event) -> Sent { + let Some(sender) = self.sender(connection) else { + return Sent::Gone; + }; + match sender.try_send(event.encode()) { + Ok(()) => Sent::Queued, + Err(TrySendError::Full(_)) => Sent::Full, + Err(TrySendError::Disconnected(_)) => Sent::Gone, + } + } + + /// Queue a message that must not be dropped, waiting for room. + pub fn send(&self, connection: u64, event: &Event) { + if let Some(sender) = self.sender(connection) { + let _ = sender.send(event.encode()); + } + } + + fn connect(&self, connection: u64, sender: SyncSender) { + *self.current.lock() = Some((connection, sender)); + } + + fn disconnect(&self, connection: u64) { + let mut current = self.current.lock(); + if current + .as_ref() + .is_some_and(|(current, _)| *current == connection) + { + *current = None; + } + } +} + +pub(crate) fn listen(shared: &Arc, listener: UnixListener) { + let mut connection = 0u64; + for socket in listener.incoming() { + if shared.closing() { + break; + } + let socket = match socket { + Ok(socket) => socket, + Err(err) => { + tracing::error!(%err, "accept a worker"); + continue; + } + }; + connection += 1; + if let Err(err) = accept(shared, connection, socket) { + tracing::error!(err = format!("{err:#}"), "start a worker connection"); + } + } +} + +fn accept(shared: &Arc, connection: u64, socket: UnixStream) -> Result<()> { + let (sender, receiver) = mpsc::sync_channel::(OUTPUT_DEPTH); + let reader = socket.try_clone().context("clone the worker socket")?; + let writer = socket.try_clone().context("clone the worker socket")?; + if let Some(previous) = shared.replace_connection(connection, socket) { + tracing::warn!("a new worker connected; dropping the previous one"); + let _ = previous.shutdown(std::net::Shutdown::Both); + } + shared.output.connect(connection, sender); + + std::thread::Builder::new() + .name("carla-bridge:write".into()) + .spawn(move || { + let mut writer = BufWriter::new(writer); + // Ends once the connection is replaced or its reader finishes, after + // writing whatever was already queued, such as a final error. + while let Ok(message) = receiver.recv() { + if let Err(err) = write(&mut writer, &receiver, message) { + tracing::warn!(%err, "worker write failed"); + break; + } + } + let _ = writer.get_ref().shutdown(std::net::Shutdown::Both); + }) + .context("start the worker writer")?; + + let shared = shared.clone(); + std::thread::Builder::new() + .name("carla-bridge:read".into()) + .spawn(move || { + let mut connection = Connection { + shared: &shared, + key: connection, + streams: HashMap::new(), + hello: false, + }; + let result = connection.serve(&mut BufReader::new(reader)); + if let Err(err) = &result { + tracing::error!(err = format!("{err:#}"), "worker protocol error"); + shared.output.send( + connection.key, + &Event::Error { + id: 0, + message: format!("{err:#}"), + }, + ); + } + connection.finish(); + }) + .context("start the worker reader")?; + Ok(()) +} + +/// Write `message` and everything queued behind it, then flush once, so a +/// burst of packets costs one syscall rather than one each. +fn write( + writer: &mut BufWriter, + queue: &mpsc::Receiver, + message: Bytes, +) -> io::Result<()> { + writer.write_all(&message)?; + while let Ok(message) = queue.try_recv() { + writer.write_all(&message)?; + } + writer.flush() +} + +struct Connection<'a> { + shared: &'a Arc, + key: u64, + streams: HashMap>, + hello: bool, +} + +impl Connection<'_> { + fn serve(&mut self, reader: &mut impl io::Read) -> Result<()> { + while let Some((kind, body)) = protocol::read(reader).context("read from the worker")? { + let request = Request::decode(kind, body).context("decode a worker request")?; + self.handle(request)?; + } + Ok(()) + } + + fn handle(&mut self, request: Request) -> Result<()> { + // A replaced connection's reader can still be mid-request. Holding the + // connection lock orders each request against a replacement, and a + // superseded connection may not touch the streams its successor owns. + let current = self.shared.connection.lock(); + if current.as_ref().map(|(key, _)| *key) != Some(self.key) { + bail!("superseded by a newer worker connection"); + } + if !self.hello && !matches!(request, Request::Hello { .. }) { + bail!("expected hello, got {request:?}"); + } + match request { + Request::Hello { version } => { + if self.hello { + bail!("repeated hello"); + } + if version != VERSION { + bail!("worker speaks protocol {version}, bridge speaks {VERSION}"); + } + self.hello = true; + // Sending waits for queue room; never while a replacement waits. + drop(current); + self.shared + .output + .send(self.key, &Event::Hello { version: VERSION }); + } + Request::Open { + id, + name, + renditions, + } => { + if id == 0 { + bail!("stream id 0 is reserved for the connection"); + } + validate(&name, &renditions)?; + if self.streams.contains_key(&id) { + bail!("stream {id} is already open"); + } + if self.streams.values().any(|stream| stream.name() == name) { + bail!("stream {name:?} is already open"); + } + let stream = self.shared.stream(&name)?; + stream.open(Session { + key: self.session(id), + renditions, + }); + self.streams.insert(id, stream); + } + Request::Demand { id, renditions } => { + self.stream(id)?.demand(self.session(id), renditions)? + } + Request::Keyframe { id, rendition } => { + self.stream(id)?.keyframe(self.session(id), rendition)? + } + Request::Close { id } => { + let stream = self + .streams + .remove(&id) + .with_context(|| format!("stream {id} is not open"))?; + stream.close(self.session(id)); + } + } + Ok(()) + } + + fn stream(&self, id: u32) -> Result<&Arc> { + self.streams + .get(&id) + .with_context(|| format!("stream {id} is not open")) + } + + fn session(&self, id: u32) -> SessionId { + SessionId { + connection: self.key, + id, + } + } + + /// The worker is gone: stop every stream it opened. + fn finish(self) { + self.shared.output.disconnect(self.key); + for (id, stream) in self.streams { + stream.close(SessionId { + connection: self.key, + id, + }); + } + } +} + +fn validate(name: &str, renditions: &[protocol::Rendition]) -> Result<()> { + if name.is_empty() { + bail!("stream name is empty"); + } + if renditions.is_empty() || renditions.len() > MAX_RENDITIONS { + bail!("stream {name:?} has {} renditions", renditions.len()); + } + for (index, rendition) in renditions.iter().enumerate() { + let label = format!("stream {name:?} rendition {:?}", rendition.name); + if rendition.name.is_empty() { + bail!("{label}: empty name"); + } + if renditions[..index] + .iter() + .any(|other| other.name == rendition.name) + { + bail!("{label}: duplicate name"); + } + if rendition.width == 0 + || rendition.height == 0 + || !rendition.width.is_multiple_of(2) + || !rendition.height.is_multiple_of(2) + { + bail!( + "{label}: size {}x{} must be non-zero and even", + rendition.width, + rendition.height + ); + } + if rendition.framerate == 0 || rendition.bitrate == 0 { + bail!("{label}: framerate and bitrate must be non-zero"); + } + } + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn a_superseded_connection_changes_nothing() { + let path = std::env::temp_dir().join(format!( + "carla-bridge-superseded-{}.sock", + std::process::id() + )); + let bridge = crate::Bridge::listen(&path).unwrap(); + // Not the current connection: another worker replaced it (here, none + // is current at all). + let mut stale = Connection { + shared: &bridge.shared, + key: u64::MAX, + streams: HashMap::new(), + hello: true, + }; + let err = stale + .handle(Request::Open { + id: 1, + name: "truck1/front".into(), + renditions: vec![protocol::Rendition { + name: "sd".into(), + width: 426, + height: 240, + bitrate: 800_000, + framerate: 30, + }], + }) + .unwrap_err(); + assert!(err.to_string().contains("superseded"), "{err:#}"); + assert!(bridge.shared.streams.lock().is_empty()); + } +} diff --git a/demo/carla/bridge/src/source.rs b/demo/carla/bridge/src/source.rs new file mode 100644 index 00000000000..df82fa858b3 --- /dev/null +++ b/demo/carla/bridge/src/source.rs @@ -0,0 +1,407 @@ +//! A capture source: one CARLA camera's exportable image slots. +//! +//! The producer (the CARLA plugin on Unreal's RHI thread) owns the Vulkan +//! images; the bridge holds their exported handles as moq-video slots, which +//! the encoders import on the producer's device. A slot cycles +//! +//! ```text +//! Idle -> Acquired (producer copies the render target and signals `ready`) +//! -> Queued (waiting for the stream thread) +//! -> InFlight (the encoders wait `ready`, read, and signal `complete`) +//! -> Idle once the GPU has finished +//! ``` +//! +//! Every producer call is non-blocking: a slot comes back through a completion +//! the producer checks on its next acquire, and a producer with no idle slot +//! drops that frame before anything encodes it. Only [`Source::close`] waits, +//! so the producer can free its images afterwards. + +use std::os::fd::OwnedFd; +use std::sync::Arc; +use std::sync::atomic::{AtomicBool, AtomicU64, Ordering}; + +use anyhow::{Context, Result, bail}; +use moq_video::Size; +use moq_video::frame::vulkan::{ + self, Completion, Device, Format, Handles, Image, Memory, Slot, Timeline, +}; +use parking_lot::{Condvar, Mutex}; + +use crate::stream::{Stream, Work}; + +static NEXT_ID: AtomicU64 = AtomicU64::new(1); + +/// One camera feeding a stream. +pub struct Source { + inner: Arc, +} + +pub(crate) struct Inner { + pub id: u64, + pub size: Size, + format: Format, + device: Device, + pub stream: Arc, + /// Set by the stream: whether this camera is the one it wants pictures from. + pub wanted: Arc, + slots: Mutex, + /// Signalled whenever a queued slot leaves the queue. + settled: Condvar, + /// Frames the producer rendered with no idle slot to copy into. + dropped: AtomicU64, +} + +struct Slots { + states: Vec, + /// The last timeline value handed out per slot. Values only grow: a + /// cancelled or unpublished use just skips ahead. + values: Vec, + closed: bool, +} + +enum State { + /// Not imported yet. + Empty, + Idle(Slot<()>), + /// With the stream generation at acquire time, so a switch away and back + /// before submit cannot relabel this picture as the resumed stream's. + Acquired(Slot<()>, Timeline, u64), + Queued, + InFlight(Completion<()>), + /// The GPU failed after publishing, and the slot was released rather than + /// handed back for reuse. + Failed, +} + +impl Source { + pub(crate) fn new( + stream: Arc, + device: Device, + size: Size, + format: Format, + slots: usize, + ) -> Result { + if slots == 0 { + bail!("a source needs at least one slot"); + } + if size.width == 0 + || size.height == 0 + || !size.width.is_multiple_of(2) + || !size.height.is_multiple_of(2) + { + bail!("capture size {size} must be non-zero and even"); + } + let inner = Arc::new(Inner { + id: NEXT_ID.fetch_add(1, Ordering::Relaxed), + size, + format, + device, + stream, + wanted: Arc::new(AtomicBool::new(false)), + slots: Mutex::new(Slots { + states: (0..slots).map(|_| State::Empty).collect(), + values: vec![0; slots], + closed: false, + }), + settled: Condvar::new(), + dropped: AtomicU64::new(0), + }); + inner + .stream + .attach(inner.id, device, size, inner.wanted.clone())?; + Ok(Self { inner }) + } + + /// Import slot `index`: a dedicated optimal-tiling image of the capture + /// size and format, allocated from the device's `memory_type`, in + /// `VK_IMAGE_LAYOUT_GENERAL` whenever the bridge owns it, and a timeline + /// semaphore starting at 0. The descriptors are consumed whatever happens. + pub fn import( + &self, + index: usize, + memory: OwnedFd, + timeline: OwnedFd, + allocation_size: u64, + memory_type: u32, + ) -> Result<()> { + let mut slots = self.inner.slots.lock(); + if slots.closed { + bail!("source is closed"); + } + match slots.states.get(index) { + Some(State::Empty) => {} + Some(_) => bail!("slot {index} is already imported"), + None => bail!( + "slot {index} is out of range for {} slots", + slots.states.len() + ), + } + let image = Image { + device: self.inner.device, + memory: Memory::OpaqueFd { memory_type }, + size: self.inner.size, + allocation_size, + format: self.inner.format, + }; + let slot = Slot::new(Handles { memory, timeline }, image, ()) + .with_context(|| format!("import slot {index}"))?; + slots.states[index] = State::Idle(slot); + Ok(()) + } + + /// Whether the stream wants this camera's pictures now. Render and submit + /// only while this holds; it is a single atomic load. + pub fn wanted(&self) -> bool { + self.inner.wanted.load(Ordering::Acquire) + } + + /// Take an idle slot for the next picture: its index and the timeline value + /// the producer's queue submission must signal once the copy has landed. + /// + /// `None` means every slot is still in use, and the producer drops this + /// picture: nothing has been encoded from it, so no reference chain breaks. + /// The producer must order each copy after the previous use of the same + /// image on its queue, as any reuse of a render target already is. + pub fn acquire(&self) -> Result> { + let mut guard = self.inner.slots.lock(); + let slots = &mut *guard; + if slots.closed { + bail!("source is closed"); + } + reap(&mut slots.states); + if slots + .states + .iter() + .all(|state| matches!(state, State::Failed)) + { + bail!("every slot failed on the GPU"); + } + let Some(index) = slots + .states + .iter() + .position(|state| matches!(state, State::Idle(_))) + else { + self.inner.dropped.fetch_add(1, Ordering::Relaxed); + return Ok(None); + }; + let State::Idle(slot) = std::mem::replace(&mut slots.states[index], State::Empty) else { + unreachable!() + }; + let ready = slots.values[index] + 1; + let timeline = Timeline::new(ready, ready + 1).expect("complete follows ready"); + slots.values[index] = ready + 1; + let generation = self.inner.stream.generation(); + slots.states[index] = State::Acquired(slot, timeline, generation); + Ok(Some((index, ready))) + } + + /// Hand an acquired slot to the encoder once the producer's queue + /// submission, including its `ready` signal, has been made. + /// + /// `frame` is CARLA's frame counter and `timestamp_us` the simulation time + /// the picture shows; both ride through to the worker with the packet. + pub fn submit(&self, index: usize, frame: u64, timestamp_us: u64) -> Result<()> { + let job = { + let mut slots = self.inner.slots.lock(); + let Some(State::Acquired(..)) = slots.states.get(index) else { + bail!("slot {index} was not acquired"); + }; + let State::Acquired(slot, timeline, generation) = + std::mem::replace(&mut slots.states[index], State::Queued) + else { + unreachable!() + }; + Job { + source: self.inner.clone(), + index, + slot: Some(slot), + timeline, + generation, + frame, + timestamp_us, + } + }; + // A stream thread that has shut down drops the job, which hands the + // slot straight back. + self.inner.stream.send(Work::Frame(job)); + Ok(()) + } + + /// Return an acquired slot the producer did not fill. + pub fn cancel(&self, index: usize) -> Result<()> { + let mut slots = self.inner.slots.lock(); + let Some(State::Acquired(..)) = slots.states.get(index) else { + bail!("slot {index} was not acquired"); + }; + let State::Acquired(slot, ..) = std::mem::replace(&mut slots.states[index], State::Empty) + else { + unreachable!() + }; + slots.states[index] = State::Idle(slot); + Ok(()) + } + + /// The producer can no longer feed this camera, for `message`. The stream + /// stops wanting it and fails the worker's session; close it next. + pub fn fail(&self, message: &str) { + self.inner.stream.fail_producer(self.inner.id, message); + } + + /// Frames dropped so far because every slot was busy. + pub fn dropped(&self) -> u64 { + self.inner.dropped.load(Ordering::Relaxed) + } + + /// Stop capturing and release every import once the GPU is done with it. Returns only after that, so the producer may destroy its images and + /// semaphores next. The stream keeps encoding from any other camera. + pub fn close(self) { + drop(self); + } +} + +impl Drop for Source { + fn drop(&mut self) { + self.inner.stream.detach(self.inner.id); + let mut slots = self.inner.slots.lock(); + slots.closed = true; + // A queued slot is about to be published or handed back; wait for it + // rather than guess, since publishing starts GPU work on the image. + while slots + .states + .iter() + .any(|state| matches!(state, State::Queued)) + { + self.inner.settled.wait(&mut slots); + } + let states = std::mem::take(&mut slots.states); + drop(slots); + for state in states { + if let State::InFlight(completion) = state { + // Resolves once the encoders have signalled completion. On a device + // error the slot is released instead; either way nothing on the GPU + // still references the image afterwards. + let _ = pollster::block_on(completion.wait()); + } + } + } +} + +/// Move every slot the GPU has finished with back to idle, without waiting. +fn reap(states: &mut [State]) { + for state in states.iter_mut() { + let State::InFlight(completion) = state else { + continue; + }; + match completion.try_wait() { + Ok(Some(slot)) => *state = State::Idle(slot), + Ok(None) => {} + Err(err) => { + tracing::error!(%err, "capture slot lost"); + *state = State::Failed; + } + } + } +} + +/// One submitted picture on its way to the stream thread. +pub(crate) struct Job { + source: Arc, + index: usize, + slot: Option>, + timeline: Timeline, + /// The stream generation at submission: a picture from before a camera + /// switch is never encoded into the new stream. + pub generation: u64, + pub frame: u64, + pub timestamp_us: u64, +} + +impl Job { + pub fn source(&self) -> &Arc { + &self.source + } + + /// Publish the producer's copy. The slot stays in flight until the returned + /// frame, and every encoder's read of it, has finished on the GPU. + pub fn publish(mut self) -> Result { + let slot = self.slot.take().expect("job published twice"); + let result = slot.publish(self.timeline); + let mut slots = self.source.slots.lock(); + let published = match result { + Ok((frame, completion)) => { + slots.states[self.index] = State::InFlight(completion); + Ok(frame) + } + Err(err) => { + let (error, slot) = err.into_parts(); + slots.states[self.index] = State::Idle(slot); + Err(anyhow::Error::from(error).context("publish the capture slot")) + } + }; + drop(slots); + self.source.settled.notify_all(); + published + } +} + +impl Drop for Job { + fn drop(&mut self) { + // Never published, so the GPU holds nothing of the image. + if let Some(slot) = self.slot.take() { + let mut slots = self.source.slots.lock(); + slots.states[self.index] = State::Idle(slot); + drop(slots); + self.source.settled.notify_all(); + } + } +} + +#[cfg(test)] +mod tests { + use std::fs::File; + + use super::*; + use crate::server::Output; + + fn device(byte: u8) -> Device { + Device { + device_uuid: [byte; 16], + driver_uuid: [0; 16], + render_node: None, + } + } + + fn handle() -> OwnedFd { + File::open("/dev/null").unwrap().into() + } + + #[test] + fn a_picture_nobody_encodes_returns_its_slot() { + let stream = Stream::spawn("truck1/front".into(), Arc::new(Output::default())).unwrap(); + let size = Size::new(64, 64); + let source = Source::new(stream.clone(), device(1), size, Format::Bgra8, 1).unwrap(); + source + .import(0, handle(), handle(), 64 * 64 * 4, 0) + .unwrap(); + + let (index, _) = source.acquire().unwrap().expect("an idle slot"); + source.submit(index, 1, 1).unwrap(); + // No session wants the picture. Publishing it would leave a frame no + // encoder reads, which never completes and loses the only slot. + stream.shutdown(); + assert!(source.acquire().unwrap().is_some()); + } + + #[test] + fn every_camera_of_a_stream_renders_on_one_device() { + let stream = Stream::spawn("truck1/front".into(), Arc::new(Output::default())).unwrap(); + let size = Size::new(64, 64); + let _first = Source::new(stream.clone(), device(1), size, Format::Bgra8, 1).unwrap(); + let _same = Source::new(stream.clone(), device(1), size, Format::Rgba8, 1).unwrap(); + let err = Source::new(stream.clone(), device(2), size, Format::Bgra8, 1) + .err() + .expect("a camera on another device"); + assert!(err.to_string().contains("encodes on"), "{err:#}"); + stream.shutdown(); + } +} diff --git a/demo/carla/bridge/src/stream.rs b/demo/carla/bridge/src/stream.rs new file mode 100644 index 00000000000..1cc018eea80 --- /dev/null +++ b/demo/carla/bridge/src/stream.rs @@ -0,0 +1,864 @@ +//! A stream: the video of one worker broadcast, fed by one or more cameras. +//! +//! Each stream owns a thread, because encoders are thread-confined and the +//! producer's RHI thread must never wait on a conversion or an encode. The +//! stream picks which camera renders: the smallest one covering the largest +//! demanded rendition. Every demanded rendition is scaled on the GPU from that +//! one capture, so SD and HD demand together still render once. + +use std::sync::Arc; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::mpsc; +use std::thread::JoinHandle; + +use anyhow::{Context, Result, bail}; +use bytes::Bytes; +use moq_mux::codec::annexb; +use moq_video::encode::{Encoder, Kind}; +use moq_video::frame::vulkan::{self, Device}; +use moq_video::{Color, Frame, I420, Rate, Size, Surface}; +use parking_lot::Mutex; + +use crate::protocol::{self, Event, Rendition}; +use crate::server::{Output, Sent}; +use crate::source::Job; + +/// Identifies one worker `Open`: a stream id is only meaningful on the +/// connection that chose it. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub(crate) struct SessionId { + pub connection: u64, + pub id: u32, +} + +/// Every rendition is written and labelled BT.709 limited. `Color::infer` would +/// pick BT.601 for the 240p rendition, but its pixels are scaled from the same +/// conversion, so the label follows the conversion rather than the size. +const COLOR: Color = Color::BT709; + +#[derive(Clone, Debug)] +pub(crate) struct Session { + pub key: SessionId, + pub renditions: Vec, +} + +pub(crate) enum Work { + Open(Session), + Close(SessionId), + Frame(Job), + /// The first camera attached, so a session waiting for its device can open. + Attached, + /// A camera's producer failed: fail this session. + Fail(SessionId, String), + Shutdown, +} + +pub(crate) struct Stream { + name: String, + state: Mutex, + work: mpsc::Sender, + thread: Mutex>>, +} + +struct State { + session: Option, + /// Renditions with subscribers, as a mask of session indices. + demand: u8, + /// Renditions whose next frame must be an IDR. + resync: u8, + sources: Vec, + /// The Vulkan device every camera renders on, learned from the first. The + /// encoders import on it, so a stream cannot open before a camera attaches. + device: Option, + /// Why a camera's producer failed, until another camera attaches. Every + /// session opened meanwhile fails with it rather than wait on video that + /// may never come. + failed: Option, + /// The camera currently rendering. + target: Option, + /// Bumped whenever the target changes, so pictures submitted before a + /// camera switch are never encoded after it. + generation: u64, + /// The simulation time each rendition last encoded. Every camera captures + /// on the same tick, so a switch can offer a rendition the same instant + /// twice; its timestamps must strictly increase. + latest: Vec>, +} + +struct Attached { + id: u64, + size: Size, + wanted: Arc, +} + +/// What to encode from one picture. +struct Plan { + session: SessionId, + renditions: Vec, +} + +struct Planned { + index: usize, + keyframe: bool, +} + +impl Stream { + pub fn spawn(name: String, output: Arc) -> Result> { + let (work, receiver) = mpsc::channel(); + let stream = Arc::new(Self { + name, + state: Mutex::new(State { + session: None, + demand: 0, + resync: 0, + sources: Vec::new(), + device: None, + failed: None, + target: None, + generation: 0, + latest: Vec::new(), + }), + work, + thread: Mutex::new(None), + }); + let thread = std::thread::Builder::new() + .name(format!("carla-bridge:{}", stream.name)) + .spawn({ + let stream = stream.clone(); + move || run(&stream, &output, receiver) + }) + .context("start the stream thread")?; + *stream.thread.lock() = Some(thread); + Ok(stream) + } + + pub fn name(&self) -> &str { + &self.name + } + + pub fn send(&self, work: Work) { + // Fails only after shutdown, when dropping the work is the right thing: + // a job hands its slot back as it drops. + let _ = self.work.send(work); + } + + pub fn shutdown(&self) { + self.send(Work::Shutdown); + if let Some(thread) = self.thread.lock().take() { + let _ = thread.join(); + } + } + + pub fn generation(&self) -> u64 { + self.state.lock().generation + } + + pub fn attach( + &self, + id: u64, + device: Device, + size: Size, + wanted: Arc, + ) -> Result<()> { + let mut state = self.state.lock(); + match state.device { + Some(current) if current != device => { + bail!( + "camera renders on {device}, but stream {} encodes on {current}", + self.name + ) + } + Some(_) => {} + None => { + state.device = Some(device); + self.send(Work::Attached); + } + } + state.failed = None; + state.sources.push(Attached { id, size, wanted }); + state.retarget(); + Ok(()) + } + + fn device(&self) -> Option { + self.state.lock().device + } + + pub fn detach(&self, id: u64) { + let mut state = self.state.lock(); + if let Some(attached) = state.sources.iter().find(|source| source.id == id) { + attached.wanted.store(false, Ordering::Release); + } + state.sources.retain(|source| source.id != id); + state.retarget(); + } + + /// A camera's producer can no longer feed it: stop wanting its pictures and + /// fail the open session, so the worker errors instead of waiting on video + /// that will not come. + pub fn fail_producer(&self, id: u64, message: &str) { + self.detach(id); + let message = format!("camera failed: {message}"); + let key = { + let mut state = self.state.lock(); + state.failed = Some(message.clone()); + state.session.as_ref().map(|session| session.key) + }; + if let Some(key) = key { + self.send(Work::Fail(key, message)); + } + } + + pub fn open(&self, session: Session) { + let mut state = self.state.lock(); + state.latest = vec![None; session.renditions.len()]; + state.session = Some(session.clone()); + state.demand = 0; + state.resync = 0; + state.retarget(); + let failed = state.failed.clone(); + drop(state); + let key = session.key; + self.send(Work::Open(session)); + if let Some(message) = failed { + self.send(Work::Fail(key, message)); + } + } + + pub fn close(&self, key: SessionId) { + self.end(key); + self.send(Work::Close(key)); + } + + /// Stop wanting pictures for a session, keeping its encoders until the + /// thread sees the matching close. + fn end(&self, key: SessionId) { + let mut state = self.state.lock(); + if state + .session + .as_ref() + .is_some_and(|session| session.key == key) + { + state.session = None; + state.demand = 0; + state.resync = 0; + state.retarget(); + } + } + + pub fn demand(&self, key: SessionId, renditions: u8) -> Result<()> { + let mut state = self.state.lock(); + let count = state.renditions(key)?; + if u32::from(renditions) >> count != 0 { + bail!("demand mask {renditions:#b} names renditions beyond {count}"); + } + // A rendition that gains subscribers starts at an IDR; pictures it + // skipped while idle are not references anything can use. + state.resync |= renditions & !state.demand; + state.demand = renditions; + state.retarget(); + Ok(()) + } + + pub fn keyframe(&self, key: SessionId, rendition: u8) -> Result<()> { + let mut state = self.state.lock(); + let count = state.renditions(key)?; + if usize::from(rendition) >= count { + bail!("rendition {rendition} is beyond {count}"); + } + state.resync |= 1 << rendition; + Ok(()) + } + + fn resync(&self, key: SessionId, rendition: usize) { + let mut state = self.state.lock(); + if state + .session + .as_ref() + .is_some_and(|session| session.key == key) + { + state.resync |= 1 << rendition; + } + } + + fn plan(&self, source: u64, generation: u64, timestamp_us: u64) -> Option { + self.state.lock().plan(source, generation, timestamp_us) + } +} + +impl State { + fn renditions(&self, key: SessionId) -> Result { + match &self.session { + Some(session) if session.key == key => Ok(session.renditions.len()), + _ => bail!("stream {} is not open", key.id), + } + } + + /// What to encode from a picture of `source` at `timestamp_us`, taken at + /// `generation`, or `None` when nothing should: no session, nobody watching, + /// or a picture from a camera the stream has since switched away from. + fn plan(&mut self, source: u64, generation: u64, timestamp_us: u64) -> Option { + if self.generation != generation || self.target != Some(source) { + return None; + } + let session = self.session.as_ref()?; + let mut renditions = Vec::new(); + for index in 0..session.renditions.len() { + let bit = 1 << index; + if self.demand & bit == 0 + || self.latest[index].is_some_and(|latest| latest >= timestamp_us) + { + continue; + } + self.latest[index] = Some(timestamp_us); + renditions.push(Planned { + index, + keyframe: self.resync & bit != 0, + }); + // Only once planned: a skipped instant leaves the IDR to the next one. + self.resync &= !bit; + } + Some(Plan { + session: session.key, + renditions, + }) + } + + fn retarget(&mut self) { + let largest = self.session.as_ref().and_then(|session| { + session + .renditions + .iter() + .enumerate() + .filter(|(index, _)| self.demand & (1 << index) != 0) + .map(|(_, rendition)| Size::new(rendition.width, rendition.height)) + .max_by_key(Size::pixels) + }); + let target = largest.and_then(|largest| { + self.sources + .iter() + .filter(|source| { + source.size.width >= largest.width && source.size.height >= largest.height + }) + .min_by_key(|source| source.size.pixels()) + .map(|source| source.id) + }); + if target != self.target { + self.target = target; + self.generation += 1; + // Every rendition restarts from the new camera at an IDR. + self.resync |= self.demand; + } + for source in &self.sources { + source + .wanted + .store(Some(source.id) == target, Ordering::Release); + } + } +} + +fn run(stream: &Arc, output: &Output, work: mpsc::Receiver) { + let mut encoding: Option = None; + // A session opened before any camera attached, so before its device is known. + let mut waiting: Option = None; + while let Ok(work) = work.recv() { + match work { + Work::Open(session) => { + encoding = None; + waiting = None; + match stream.device() { + Some(device) => encoding = open(stream, output, session, device), + None => waiting = Some(session), + } + } + Work::Attached => { + if let (Some(session), Some(device)) = (waiting.take(), stream.device()) { + encoding = open(stream, output, session, device); + } + } + Work::Close(key) => { + if encoding + .as_ref() + .is_some_and(|open| open.session.key == key) + { + encoding = None; + } + if waiting.as_ref().is_some_and(|session| session.key == key) { + waiting = None; + } + } + Work::Frame(job) => { + if let Err((key, err)) = frame(stream, output, encoding.as_mut(), job) { + encoding = None; + fail(stream, output, key, &err); + } + } + Work::Fail(key, message) => { + if encoding + .as_ref() + .is_some_and(|open| open.session.key == key) + { + encoding = None; + } + if waiting.as_ref().is_some_and(|session| session.key == key) { + waiting = None; + } + fail(stream, output, key, &anyhow::anyhow!(message)); + } + Work::Shutdown => break, + } + } +} + +/// Open the encoders for `session` on `device` and send the worker each +/// rendition's parameter sets. +fn open(stream: &Stream, output: &Output, session: Session, device: Device) -> Option { + let key = session.key; + match Encoding::open(session, device) { + Ok((opened, configs)) => { + for (rendition, init) in configs.into_iter().enumerate() { + output.send( + key.connection, + &Event::Config { + id: key.id, + rendition: rendition as u8, + init, + }, + ); + } + Some(opened) + } + Err(err) => { + fail(stream, output, key, &err); + None + } + } +} + +/// Report a stream failure to the worker, which treats it as fatal. +fn fail(stream: &Stream, output: &Output, key: SessionId, err: &anyhow::Error) { + tracing::error!( + stream = stream.name(), + err = format!("{err:#}"), + "stream failed" + ); + stream.end(key); + output.send( + key.connection, + &Event::Error { + id: key.id, + message: format!("{}: {err:#}", stream.name()), + }, + ); +} + +fn frame( + stream: &Stream, + output: &Output, + encoding: Option<&mut Encoding>, + job: Job, +) -> Result<(), (SessionId, anyhow::Error)> { + let (generation, number, timestamp_us) = (job.generation, job.frame, job.timestamp_us); + // Publish only a picture an encoder will read: an unread frame never signals + // completion, which loses its slot. Dropping the job instead hands the slot + // straight back, which is safe because the producer orders every copy into + // an image after its previous one. + let Some(plan) = stream.plan(job.source().id, generation, timestamp_us) else { + return Ok(()); + }; + let key = plan.session; + let Some(encoding) = encoding.filter(|open| open.session.key == key) else { + return Ok(()); + }; + if plan.renditions.is_empty() { + return Ok(()); + } + let image = job.publish().map_err(|err| (key, err))?; + encoding + .encode(stream, output, image, plan, number, timestamp_us) + .map_err(|err| (key, err)) +} + +struct Encoding { + session: Session, + encoders: Vec, + gate: Gate, +} + +/// Keeps each rendition's reference chain whole across packets the worker +/// had no room for. A rendition that loses a packet drops everything after it +/// until the IDR requested at the loss is queued, so the worker never receives +/// a frame whose reference is missing. +#[derive(Default)] +struct Gate { + broken: u8, +} + +impl Gate { + fn admit(&self, rendition: usize, keyframe: bool) -> bool { + keyframe || self.broken & (1 << rendition) == 0 + } + + /// Record what became of an admitted packet. True when the rendition just + /// broke and needs an IDR. + fn sent(&mut self, rendition: usize, keyframe: bool, sent: &Sent) -> bool { + let bit = 1 << rendition; + match sent { + Sent::Queued => { + if keyframe { + self.broken &= !bit; + } + false + } + Sent::Full => { + self.broken |= bit; + true + } + Sent::Gone => false, + } + } +} + +impl Encoding { + /// Open one encoder per rendition on the cameras' device, and read each + /// one's parameter sets so the worker can advertise the track before it has + /// a subscriber. + fn open(session: Session, device: Device) -> Result<(Self, Vec)> { + let mut encoders = Vec::with_capacity(session.renditions.len()); + let mut configs = Vec::with_capacity(session.renditions.len()); + for rendition in &session.renditions { + let config = config(rendition, device)?; + configs.push( + probe(&config).with_context(|| format!("probe rendition {}", rendition.name))?, + ); + encoders.push( + Encoder::new(&config) + .with_context(|| format!("open the encoder for {}", rendition.name))?, + ); + } + Ok(( + Self { + session, + encoders, + gate: Gate::default(), + }, + configs, + )) + } + + fn encode( + &mut self, + stream: &Stream, + output: &Output, + image: vulkan::Frame, + plan: Plan, + number: u64, + timestamp_us: u64, + ) -> Result<()> { + let key = plan.session; + let timestamp = + moq_net::Timestamp::from_micros(timestamp_us).context("capture timestamp")?; + // The encoders share one conversion of the capture and each scales it to + // its rendition on the GPU. The slot's completion is signalled once the + // last of them has read it, after this frame and its clones drop. + let frame = Frame::new(Surface::Vulkan(image), timestamp); + for planned in plan.renditions { + let encoder = &mut self.encoders[planned.index]; + if planned.keyframe { + encoder.cut().context("request an IDR")?; + } + let packets = encoder.encode(&frame).context("encode")?; + for packet in packets { + let keyframe = packet.keyframe; + if !self.gate.admit(planned.index, keyframe) { + continue; + } + let event = Event::Frame(protocol::Frame { + id: key.id, + rendition: planned.index as u8, + frame: number, + timestamp_us, + keyframe, + payload: packet.payload, + }); + let sent = output.try_send(key.connection, &event); + if self.gate.sent(planned.index, keyframe, &sent) { + tracing::warn!( + stream = stream.name(), + frame = number, + "worker queue full; resuming at an IDR" + ); + stream.resync(key, planned.index); + } + } + } + Ok(()) + } +} + +fn config(rendition: &Rendition, device: Device) -> Result { + let framerate = Rate::new(rendition.framerate, 1).context("framerate")?; + let mut config = moq_video::encode::Config::new(rendition.width, rendition.height, framerate); + config.bitrate = Some(moq_net::bandwidth::Rate::from_bps(rendition.bitrate)); + config.kind = Kind::Named("nvenc".into()); + config.color = Some(COLOR); + config.input = Some(device); + Ok(config) +} + +/// The parameter sets `config` produces, from a throwaway session on the same +/// device fed one synthetic mid-gray picture. A separate session so the one +/// that encodes camera pictures only ever receives the cameras' images. +fn probe(config: &moq_video::encode::Config) -> Result { + let size = config.size(); + let luma = size.width as usize * size.height as usize; + let gray = I420::new(size, vec![0x80; luma + luma / 2])?; + let mut encoder = Encoder::new(config)?; + let mut packets = encoder.encode(&Frame::new( + Surface::I420(gray), + moq_net::Timestamp::from_micros(0)?, + ))?; + packets.extend(encoder.finish()?); + let mut sets = Vec::new(); + for packet in &packets { + for nal in nals(&packet.payload)? { + if matches!(nal.first().map(|header| header & 0x1f), Some(7 | 8)) { + sets.push(nal); + } + } + } + if sets.is_empty() { + bail!("the encoder emitted no SPS/PPS"); + } + Ok(annexb::build_prefix(&sets)) +} + +fn nals(payload: &Bytes) -> Result> { + let mut buf = payload.clone(); + let mut iter = annexb::NalIterator::new(&mut buf); + let mut nals = Vec::new(); + for nal in iter.by_ref() { + nals.push(nal?); + } + nals.extend(iter.flush()?); + Ok(nals) +} + +#[cfg(test)] +mod tests { + use super::*; + + const SD: Size = Size { + width: 426, + height: 240, + }; + const HD: Size = Size { + width: 1280, + height: 720, + }; + const KEY: SessionId = SessionId { + connection: 1, + id: 1, + }; + + fn rendition(name: &str, size: Size) -> Rendition { + Rendition { + name: name.into(), + width: size.width, + height: size.height, + bitrate: 1_000_000, + framerate: 30, + } + } + + /// A stream with the demo's shape: an sd and an hd camera, and the + /// worker's sd and hd renditions. + fn state() -> (State, Arc, Arc) { + let mut state = State { + session: Some(Session { + key: KEY, + renditions: vec![rendition("sd", SD), rendition("hd", HD)], + }), + demand: 0, + resync: 0, + sources: Vec::new(), + device: None, + failed: None, + target: None, + generation: 0, + latest: vec![None; 2], + }; + let (sd, hd) = ( + Arc::new(AtomicBool::new(false)), + Arc::new(AtomicBool::new(false)), + ); + state.sources.push(Attached { + id: 10, + size: SD, + wanted: sd.clone(), + }); + state.sources.push(Attached { + id: 20, + size: HD, + wanted: hd.clone(), + }); + state.retarget(); + (state, sd, hd) + } + + fn demand(state: &mut State, mask: u8) { + state.resync |= mask & !state.demand; + state.demand = mask; + state.retarget(); + } + + fn planned(plan: &Plan) -> Vec<(usize, bool)> { + plan.renditions + .iter() + .map(|planned| (planned.index, planned.keyframe)) + .collect() + } + + #[test] + fn nothing_renders_without_demand() { + let (mut state, sd, hd) = state(); + assert_eq!(state.target, None); + assert!(!sd.load(Ordering::Acquire) && !hd.load(Ordering::Acquire)); + assert!(state.plan(10, state.generation, 1).is_none()); + } + + #[test] + fn renders_the_smallest_camera_covering_the_largest_demand() { + let (mut state, sd, hd) = state(); + demand(&mut state, 0b01); + assert_eq!(state.target, Some(10)); + assert!(sd.load(Ordering::Acquire) && !hd.load(Ordering::Acquire)); + + // SD and HD together render once, from the HD camera. + demand(&mut state, 0b11); + assert_eq!(state.target, Some(20)); + assert!(!sd.load(Ordering::Acquire) && hd.load(Ordering::Acquire)); + let plan = state.plan(20, state.generation, 1).unwrap(); + assert_eq!(planned(&plan), vec![(0, true), (1, true)]); + } + + #[test] + fn resuming_and_switching_force_idrs_once() { + let (mut state, ..) = state(); + demand(&mut state, 0b01); + let generation = state.generation; + assert_eq!( + planned(&state.plan(10, generation, 1).unwrap()), + vec![(0, true)] + ); + assert_eq!( + planned(&state.plan(10, generation, 2).unwrap()), + vec![(0, false)] + ); + + // Parking and resuming the same camera still starts at an IDR. + demand(&mut state, 0); + demand(&mut state, 0b01); + assert_eq!( + planned(&state.plan(10, state.generation, 3).unwrap()), + vec![(0, true)] + ); + + // An explicit keyframe request. + state.resync |= 0b01; + assert_eq!( + planned(&state.plan(10, state.generation, 4).unwrap()), + vec![(0, true)] + ); + } + + #[test] + fn pictures_from_before_a_switch_are_not_encoded() { + let (mut state, ..) = state(); + demand(&mut state, 0b01); + let before = state.generation; + demand(&mut state, 0b11); + // The SD camera's queued picture, and even an HD picture stamped before + // the switch, are stale. + assert!(state.plan(10, before, 1).is_none()); + assert!(state.plan(20, before, 1).is_none()); + assert!(state.plan(20, state.generation, 1).is_some()); + } + + #[test] + fn a_rendition_never_encodes_an_instant_twice() { + let (mut state, ..) = state(); + demand(&mut state, 0b01); + // The SD camera captured tick 5 and it was encoded. + assert_eq!( + planned(&state.plan(10, state.generation, 5).unwrap()), + vec![(0, true)] + ); + // HD demand arrives mid-tick; the HD camera's picture of the same tick + // feeds only HD, and SD's IDR waits for the next tick. + demand(&mut state, 0b11); + let generation = state.generation; + assert_eq!( + planned(&state.plan(20, generation, 5).unwrap()), + vec![(1, true)] + ); + assert_eq!( + planned(&state.plan(20, generation, 6).unwrap()), + vec![(0, true), (1, false)] + ); + // Nor does time run backwards. + assert_eq!(planned(&state.plan(20, generation, 4).unwrap()), vec![]); + } + + #[test] + fn losing_the_camera_falls_back_to_one_that_still_covers() { + let (mut state, sd, hd) = state(); + demand(&mut state, 0b01); + assert_eq!(state.target, Some(10)); + let generation = state.generation; + state.sources.retain(|source| source.id != 10); + state.retarget(); + // The HD camera covers SD too. + assert_eq!(state.target, Some(20)); + assert!(hd.load(Ordering::Acquire)); + assert!(state.generation > generation); + let _ = sd; + + // Nothing covers HD once its camera is gone. + demand(&mut state, 0b10); + state.sources.clear(); + state.retarget(); + assert_eq!(state.target, None); + } + + #[test] + fn closing_the_session_parks_the_cameras() { + let (mut state, sd, _) = state(); + demand(&mut state, 0b01); + assert!(sd.load(Ordering::Acquire)); + state.session = None; + state.demand = 0; + state.retarget(); + assert!(!sd.load(Ordering::Acquire)); + } + + #[test] + fn a_lost_packet_drops_its_rendition_until_the_next_idr() { + let mut gate = Gate::default(); + assert!(gate.admit(0, false)); + assert!(!gate.sent(0, false, &Sent::Queued)); + // The worker had no room: rendition 0 breaks, rendition 1 does not. + assert!(gate.sent(0, false, &Sent::Full)); + assert!(!gate.admit(0, false)); + assert!(gate.admit(1, false)); + // A keyframe gets through, but only a queued one heals the chain. + assert!(gate.admit(0, true)); + assert!(gate.sent(0, true, &Sent::Full)); + assert!(!gate.admit(0, false)); + assert!(!gate.sent(0, true, &Sent::Queued)); + assert!(gate.admit(0, false)); + // A departed worker neither breaks nor heals anything. + assert!(!gate.sent(1, false, &Sent::Gone)); + assert!(gate.admit(1, false)); + } +} diff --git a/demo/carla/bridge/tests/gpu.rs b/demo/carla/bridge/tests/gpu.rs new file mode 100644 index 00000000000..785b8d39487 --- /dev/null +++ b/demo/carla/bridge/tests/gpu.rs @@ -0,0 +1,480 @@ +//! The bridge on real hardware: a stand-in for the CARLA plugin renders on +//! Vulkan, the bridge encodes with NVENC through moq-video, and a worker +//! client on the socket checks every packet and decodes it. +//! +//! Needs a Linux NVIDIA GPU, so it is ignored by default and run by +//! `just carla bridge-gpu`, which `just carla check` calls on such a host. +#![cfg(target_os = "linux")] + +mod standin; + +use std::collections::{HashMap, VecDeque}; +use std::io::Write; +use std::os::unix::net::UnixStream; +use std::path::{Path, PathBuf}; +use std::time::{Duration, Instant}; + +use carla_bridge::Bridge; +use carla_bridge::protocol::{self, Event, Frame, Rendition, Request}; +use moq_video::decode::{Config, Decoder, Kind}; +use moq_video::{Output, Size}; +use standin::{Camera, Gpu}; + +const STREAM: &str = "truck1/front"; +const SD: Size = Size { + width: 426, + height: 240, +}; +const HD: Size = Size { + width: 1280, + height: 720, +}; +/// One simulation step at 30 fps. +const STEP_US: u64 = 33_333; + +fn renditions() -> Vec { + vec![ + Rendition { + name: "sd".into(), + width: SD.width, + height: SD.height, + bitrate: 800_000, + framerate: 30, + }, + Rendition { + name: "hd".into(), + width: HD.width, + height: HD.height, + bitrate: 4_000_000, + framerate: 30, + }, + ] +} + +/// The worker's end of the socket. +struct Worker { + socket: UnixStream, + /// A decoder per rendition, fed every packet in order, so a broken + /// reference chain shows up as a failed decode or a wrong color. + decoders: HashMap, + /// Pictures decoded and color-checked. + checked: usize, +} + +impl Worker { + fn connect(path: &Path) -> Self { + let socket = UnixStream::connect(path).expect("connect to the bridge"); + let mut worker = Self { + socket, + decoders: HashMap::new(), + checked: 0, + }; + worker.send(Request::Hello { + version: protocol::VERSION, + }); + assert_eq!( + worker.next(), + Event::Hello { + version: protocol::VERSION + } + ); + worker + } + + fn send(&mut self, request: Request) { + self.socket + .write_all(&request.encode()) + .expect("write to the bridge"); + } + + fn next(&mut self) -> Event { + let (kind, body) = protocol::read(&mut self.socket) + .expect("read from the bridge") + .expect("the bridge closed the connection"); + Event::decode(kind, body).expect("decode an event") + } + + /// Open the stream and check each rendition's parameter sets describe it. + fn open(&mut self, id: u32) { + self.send(Request::Open { + id, + name: STREAM.into(), + renditions: renditions(), + }); + self.decoders.clear(); + for (index, rendition) in renditions().iter().enumerate() { + let event = self.next(); + let Event::Config { + id: got, + rendition: got_rendition, + init, + } = event + else { + panic!("expected the config for {}, got {event:?}", rendition.name); + }; + assert_eq!((got, usize::from(got_rendition)), (id, index)); + let config = moq_mux::codec::h264::config(&init).expect("parse the parameter sets"); + assert_eq!( + (config.coded_width, config.coded_height), + (Some(rendition.width), Some(rendition.height)) + ); + eprintln!( + "config {}: {} bytes, {:?}", + rendition.name, + init.len(), + config.codec + ); + } + } + + /// The next frame event, checked and decoded. + fn frame(&mut self, rendition: u8, number: u64, rgb: [u8; 3]) -> Frame { + let Event::Frame(frame) = self.next() else { + panic!("expected a frame"); + }; + assert_eq!( + (frame.rendition, frame.frame), + (rendition, number), + "frame out of order" + ); + assert_eq!(frame.timestamp_us, number * STEP_US); + self.decode(&frame, rgb); + frame + } + + fn decode(&mut self, frame: &Frame, rgb: [u8; 3]) { + if frame.keyframe { + let catalog = + moq_mux::codec::h264::config(&frame.payload).expect("an IDR carries its SPS"); + let mut config = Config::new(); + config.kind = Kind::Named("nvdec".into()); + config.output = Output::Cpu; + let decoder = Decoder::new(&catalog, &config).expect("open a decoder"); + self.decoders + .insert(frame.rendition, (decoder, VecDeque::new())); + } + let (decoder, expected) = self + .decoders + .get_mut(&frame.rendition) + .expect("the first packet of a rendition is an IDR"); + expected.push_back((frame.frame, rgb)); + let timestamp = moq_net::Timestamp::from_micros(frame.timestamp_us).unwrap(); + // The decoder may hold a picture back; match pictures to + // packets in order rather than one for one. + for picture in decoder + .decode(&frame.payload, timestamp, frame.keyframe) + .expect("decode the packet") + { + let (number, rgb) = expected.pop_front().expect("more pictures than packets"); + let i420 = picture.surface.to_i420().expect("CPU picture"); + let size = SIZES[usize::from(frame.rendition)]; + assert_eq!((i420.width(), i420.height()), (size.width, size.height)); + let (y, u, v) = (mean(i420.y()), mean(i420.u()), mean(i420.v())); + let (ey, eu, ev) = bt709_limited(rgb); + assert!( + (y - ey).abs() < 3.0 && (u - eu).abs() < 3.0 && (v - ev).abs() < 3.0, + "frame {number} rendition {}: decoded YUV ({y:.1}, {u:.1}, {v:.1}), expected ({ey:.1}, {eu:.1}, {ev:.1}) for {rgb:?}", + frame.rendition + ); + self.checked += 1; + } + } +} + +const SIZES: [Size; 2] = [SD, HD]; + +/// A rendition's decoder and the frames (number, color) it has yet to output. +type Decoding = (Decoder, VecDeque<(u64, [u8; 3])>); + +fn mean(plane: &[u8]) -> f64 { + plane.iter().map(|&sample| f64::from(sample)).sum::() / plane.len() as f64 +} + +/// The display-referred bytes as BT.709 limited-range Y'CbCr, with no transfer +/// function applied: what the bridge's conversion promises. +fn bt709_limited([r, g, b]: [u8; 3]) -> (f64, f64, f64) { + let (r, g, b) = ( + f64::from(r) / 255.0, + f64::from(g) / 255.0, + f64::from(b) / 255.0, + ); + let y = 0.2126 * r + 0.7152 * g + 0.0722 * b; + ( + 16.0 + 219.0 * y, + 128.0 + 224.0 * (b - y) / 1.8556, + 128.0 + 224.0 * (r - y) / 1.5748, + ) +} + +/// A distinct, well-inside-gamut color per frame. +fn color(number: u64) -> [u8; 3] { + let n = number as u32; + [ + (40 + (n * 53) % 170) as u8, + (40 + (n * 97 + 64) % 170) as u8, + (40 + (n * 29 + 128) % 170) as u8, + ] +} + +/// Spin until another thread publishes some state, named by `_what` for the +/// reader. Unbounded, like every read here: a slow host must not fail a +/// correct run. +fn until(_what: &str, mut done: impl FnMut() -> bool) { + while !done() { + std::thread::yield_now(); + } +} + +/// Latency from submitting a picture to receiving its packets, for the report. +#[derive(Default)] +struct Latency(Vec); + +impl Latency { + fn report(&mut self, phase: &str) { + if self.0.is_empty() { + return; + } + self.0.sort(); + let pick = |q: f64| self.0[((self.0.len() - 1) as f64 * q) as usize]; + eprintln!( + "{phase}: {} frames, submit->packets p50 {:?} p90 {:?} max {:?}", + self.0.len(), + pick(0.5), + pick(0.9), + pick(1.0) + ); + self.0.clear(); + } +} + +/// Render frame `number` on `camera` and receive its packet for every +/// rendition in `renditions`, returning which were keyframes. +fn step( + camera: &mut Camera, + worker: &mut Worker, + latency: &mut Latency, + number: u64, + renditions: &[u8], +) -> Vec { + let rgb = color(number); + until("an idle slot", || { + camera.render(rgb, number, number * STEP_US) + }); + let submitted = Instant::now(); + let keyframes = renditions + .iter() + .map(|&rendition| worker.frame(rendition, number, rgb).keyframe) + .collect(); + latency.0.push(submitted.elapsed()); + keyframes +} + +fn socket_path() -> PathBuf { + std::env::temp_dir().join(format!("carla-bridge-test-{}.sock", std::process::id())) +} + +#[test] +#[ignore = "needs a Linux NVIDIA GPU; run by `just carla bridge-gpu`"] +fn gpu_video_reaches_the_worker() { + let gpu = Gpu::open().expect("an NVIDIA Vulkan device"); + let bridge = Bridge::listen(socket_path()).expect("listen"); + let mut sd = Camera::new(&gpu, &bridge, STREAM, SD, 3); + let mut hd = Camera::new(&gpu, &bridge, STREAM, HD, 3); + let mut latency = Latency::default(); + let mut number = 0u64; + let mut next = || { + number += 1; + number + }; + + let mut worker = Worker::connect(bridge.path()); + worker.open(1); + // No subscriber, no rendering. + assert!(!sd.wanted() && !hd.wanted()); + + eprintln!("-- sd demand renders the sd camera only"); + worker.send(Request::Demand { + id: 1, + renditions: 0b01, + }); + until("the sd camera", || sd.wanted()); + assert!(!hd.wanted()); + for i in 0..45 { + let keyframes = step(&mut sd, &mut worker, &mut latency, next(), &[0]); + assert_eq!( + keyframes, + [i == 0], + "sd starts at an IDR and then follows its GOP" + ); + } + latency.report("sd from the sd camera"); + + eprintln!("-- sd and hd demand render once, from the hd camera"); + worker.send(Request::Demand { + id: 1, + renditions: 0b11, + }); + until("the hd camera", || hd.wanted()); + assert!(!sd.wanted()); + for i in 0..45 { + let keyframes = step(&mut hd, &mut worker, &mut latency, next(), &[0, 1]); + assert_eq!( + keyframes, + [i == 0, i == 0], + "a camera switch restarts both renditions at an IDR" + ); + } + latency.report("sd+hd from the hd camera"); + + eprintln!("-- a keyframe request is honored out of cadence"); + worker.send(Request::Keyframe { + id: 1, + rendition: 1, + }); + let mut forced = false; + for _ in 0..10 { + let keyframes = step(&mut hd, &mut worker, &mut latency, next(), &[0, 1]); + assert!(!keyframes[0], "only the requested rendition cuts"); + if keyframes[1] { + forced = true; + break; + } + } + assert!(forced, "no IDR followed the request"); + + eprintln!("-- no demand parks both cameras; resuming starts at an IDR"); + worker.send(Request::Demand { + id: 1, + renditions: 0, + }); + until("both cameras to park", || !sd.wanted() && !hd.wanted()); + worker.send(Request::Demand { + id: 1, + renditions: 0b10, + }); + until("the hd camera", || hd.wanted()); + for i in 0..10 { + let keyframes = step(&mut hd, &mut worker, &mut latency, next(), &[1]); + assert_eq!(keyframes, [i == 0]); + } + latency.report("hd from the hd camera"); + + eprintln!("-- a restarted worker replaces the old connection"); + let replacement = Worker::connect(bridge.path()); + assert!( + protocol::read(&mut worker.socket).expect("read").is_none(), + "the replaced worker's socket was not closed" + ); + until("the old session to park its camera", || !hd.wanted()); + let checked = worker.checked; + drop(worker); + let mut worker = replacement; + worker.open(7); + worker.send(Request::Demand { + id: 7, + renditions: 0b01, + }); + until("the sd camera", || sd.wanted()); + for i in 0..5 { + let keyframes = step(&mut sd, &mut worker, &mut latency, next(), &[0]); + assert_eq!(keyframes, [i == 0]); + } + + eprintln!("-- closing a camera with work in flight waits for the GPU"); + worker.send(Request::Demand { + id: 7, + renditions: 0b11, + }); + until("the hd camera", || hd.wanted()); + let in_flight = next(); + until("an idle slot", || { + hd.render(color(in_flight), in_flight, in_flight * STEP_US) + }); + drop(hd); + // Nothing left covers hd, so nothing renders. + until("the sd camera to park", || !sd.wanted()); + worker.send(Request::Demand { + id: 7, + renditions: 0b01, + }); + until("the sd camera", || sd.wanted()); + let resumed = next(); + let rgb = color(resumed); + until("an idle slot", || { + sd.render(rgb, resumed, resumed * STEP_US) + }); + // The closed camera's last picture may or may not have been encoded. + let frame = loop { + let Event::Frame(frame) = worker.next() else { + panic!("expected a frame"); + }; + if frame.frame != in_flight { + break frame; + } + }; + assert_eq!(frame.frame, resumed); + assert!(frame.keyframe); + + eprintln!("-- a camera whose producer fails fails the worker's stream"); + sd.source().fail("the copy could not be submitted"); + let Event::Error { id: 7, message } = worker.next() else { + panic!("expected the stream to fail"); + }; + assert!( + message.contains(STREAM) && message.contains("the copy could not be submitted"), + "{message}" + ); + until("the failed camera to park", || !sd.wanted()); + + eprintln!("-- the failure outlives the session until a camera replaces it"); + let checked = checked + worker.checked; + drop(worker); + let mut worker = Worker::connect(bridge.path()); + worker.open(9); + let Event::Error { id: 9, message } = worker.next() else { + panic!("expected the next session to fail"); + }; + assert!( + message.contains("the copy could not be submitted"), + "{message}" + ); + // The worker treats a stream error as fatal, and its replacement respawns + // the cameras. + drop(worker); + drop(sd); + let sd = Camera::new(&gpu, &bridge, STREAM, SD, 3); + let mut worker = Worker::connect(bridge.path()); + worker.open(10); + worker.send(Request::Demand { + id: 10, + renditions: 0b01, + }); + until("the replacement sd camera", || sd.wanted()); + + eprintln!("-- a departed worker parks the cameras"); + let checked = checked + worker.checked; + drop(worker); + until("the sd camera to park", || !sd.wanted()); + + eprintln!("-- malformed requests end the connection with an error"); + let mut worker = Worker::connect(bridge.path()); + let mut odd = renditions(); + odd[0].width = 425; + worker.send(Request::Open { + id: 1, + name: STREAM.into(), + renditions: odd, + }); + let Event::Error { id: 0, message } = worker.next() else { + panic!("expected a connection error"); + }; + assert!(message.contains("even"), "{message}"); + assert!(protocol::read(&mut worker.socket).expect("read").is_none()); + + eprintln!("dropped for want of a slot: {}", sd.source().dropped()); + assert!( + checked > 100, + "only {checked} pictures were decoded and checked" + ); + drop(sd); +} diff --git a/demo/carla/bridge/tests/standin/mod.rs b/demo/carla/bridge/tests/standin/mod.rs new file mode 100644 index 00000000000..dd97f4b1948 --- /dev/null +++ b/demo/carla/bridge/tests/standin/mod.rs @@ -0,0 +1,493 @@ +//! A stand-in for the CARLA plugin: a native Vulkan renderer that feeds the +//! bridge exactly the way the plugin will from Unreal's RHI thread. +//! +//! Each camera owns a private render target, like a scene capture's, and a +//! ring of exportable slots. A frame clears the render target to a known +//! color (the "render"), copies it on the GPU into an acquired slot, moves the +//! slot to `GENERAL`, and signals the slot's timeline with the value the bridge +//! handed out. No pixel ever passes through host memory. + +use std::ffi::CStr; +use std::os::fd::{FromRawFd, OwnedFd}; + +use ash::vk; +use carla_bridge::{Bridge, Device, Format, Size, Source}; + +pub struct Gpu { + _entry: ash::Entry, + instance: ash::Instance, + physical: vk::PhysicalDevice, + device: ash::Device, + queue: vk::Queue, + queue_family: u32, + pub identity: Device, +} + +impl Gpu { + /// The first NVIDIA Vulkan device, or `None` when there is none. + pub fn open() -> Option { + // SAFETY: every handle is destroyed in reverse order by `Drop`. + match unsafe { Self::create() } { + Ok(gpu) => Some(gpu), + Err(err) => { + eprintln!("no NVIDIA Vulkan device: {err:#}"); + None + } + } + } + + unsafe fn create() -> anyhow::Result { + let entry = unsafe { ash::Entry::load()? }; + let app = vk::ApplicationInfo::default().api_version(vk::API_VERSION_1_2); + let instance = unsafe { + entry.create_instance( + &vk::InstanceCreateInfo::default().application_info(&app), + None, + )? + }; + let mut selected = None; + for physical in unsafe { instance.enumerate_physical_devices()? } { + let properties = unsafe { instance.get_physical_device_properties(physical) }; + if properties.vendor_id != 0x10de { + continue; + } + let family = unsafe { instance.get_physical_device_queue_family_properties(physical) } + .iter() + .position(|family| { + family + .queue_flags + .contains(vk::QueueFlags::GRAPHICS | vk::QueueFlags::TRANSFER) + }); + if let Some(family) = family { + selected = Some((physical, family as u32, properties)); + break; + } + } + let Some((physical, queue_family, properties)) = selected else { + unsafe { instance.destroy_instance(None) }; + anyhow::bail!("no NVIDIA graphics queue"); + }; + let mut id = vk::PhysicalDeviceIDProperties::default(); + let mut properties2 = vk::PhysicalDeviceProperties2::default().push_next(&mut id); + unsafe { instance.get_physical_device_properties2(physical, &mut properties2) }; + let name = unsafe { CStr::from_ptr(properties.device_name.as_ptr()) }.to_string_lossy(); + eprintln!( + "stand-in renderer on {name}, driver {:#x}", + properties.driver_version + ); + + let priority = [1.0]; + let queues = [vk::DeviceQueueCreateInfo::default() + .queue_family_index(queue_family) + .queue_priorities(&priority)]; + // What the plugin enables on Unreal's device, through its extension hook + // and the timeline-semaphore feature. + let extensions = [ + ash::khr::external_memory_fd::NAME.as_ptr(), + ash::khr::external_semaphore_fd::NAME.as_ptr(), + ]; + let mut timeline = + vk::PhysicalDeviceTimelineSemaphoreFeatures::default().timeline_semaphore(true); + let create = vk::DeviceCreateInfo::default() + .queue_create_infos(&queues) + .enabled_extension_names(&extensions) + .push_next(&mut timeline); + let device = unsafe { instance.create_device(physical, &create, None)? }; + let queue = unsafe { device.get_device_queue(queue_family, 0) }; + Ok(Self { + _entry: entry, + instance, + physical, + device, + queue, + queue_family, + identity: Device { + device_uuid: id.device_uuid, + driver_uuid: id.driver_uuid, + render_node: None, + }, + }) + } + + fn memory_type(&self, bits: u32, flags: vk::MemoryPropertyFlags) -> u32 { + let properties = unsafe { + self.instance + .get_physical_device_memory_properties(self.physical) + }; + (0..properties.memory_type_count) + .find(|index| { + bits & (1 << index) != 0 + && properties.memory_types[*index as usize] + .property_flags + .contains(flags) + }) + .expect("device-local memory type") + } + + fn image( + &self, + size: Size, + usage: vk::ImageUsageFlags, + export: bool, + ) -> (vk::Image, vk::DeviceMemory, u64, u32) { + let mut external = vk::ExternalMemoryImageCreateInfo::default() + .handle_types(vk::ExternalMemoryHandleTypeFlags::OPAQUE_FD); + let mut info = vk::ImageCreateInfo::default() + .image_type(vk::ImageType::TYPE_2D) + .format(vk::Format::B8G8R8A8_UNORM) + .extent(vk::Extent3D { + width: size.width, + height: size.height, + depth: 1, + }) + .mip_levels(1) + .array_layers(1) + .samples(vk::SampleCountFlags::TYPE_1) + .tiling(vk::ImageTiling::OPTIMAL) + .usage(usage) + .sharing_mode(vk::SharingMode::EXCLUSIVE) + .initial_layout(vk::ImageLayout::UNDEFINED); + if export { + info = info.push_next(&mut external); + } + let image = unsafe { self.device.create_image(&info, None) }.expect("create image"); + let requirements = unsafe { self.device.get_image_memory_requirements(image) }; + let memory_type = self.memory_type( + requirements.memory_type_bits, + vk::MemoryPropertyFlags::DEVICE_LOCAL, + ); + let mut exported = vk::ExportMemoryAllocateInfo::default() + .handle_types(vk::ExternalMemoryHandleTypeFlags::OPAQUE_FD); + let mut dedicated = vk::MemoryDedicatedAllocateInfo::default().image(image); + let mut allocation = vk::MemoryAllocateInfo::default() + .allocation_size(requirements.size) + .memory_type_index(memory_type) + .push_next(&mut dedicated); + if export { + allocation = allocation.push_next(&mut exported); + } + let memory = + unsafe { self.device.allocate_memory(&allocation, None) }.expect("allocate image"); + unsafe { self.device.bind_image_memory(image, memory, 0) }.expect("bind image"); + (image, memory, requirements.size, memory_type) + } +} + +impl Drop for Gpu { + fn drop(&mut self) { + unsafe { + let _ = self.device.device_wait_idle(); + self.device.destroy_device(None); + self.instance.destroy_instance(None); + } + } +} + +struct Slot { + image: vk::Image, + memory: vk::DeviceMemory, + timeline: vk::Semaphore, + /// The last recording into this slot, freed when the bridge hands the slot + /// back: by then the encoder has waited on its signal, so it has executed. + commands: Option, + written: bool, +} + +/// One camera: a render target, its slots, and the bridge source they feed. +pub struct Camera<'a> { + gpu: &'a Gpu, + pub size: Size, + source: Option, + target: vk::Image, + target_memory: vk::DeviceMemory, + pool: vk::CommandPool, + slots: Vec, + pub rendered: u64, +} + +impl<'a> Camera<'a> { + pub fn new(gpu: &'a Gpu, bridge: &Bridge, stream: &str, size: Size, slots: usize) -> Self { + let source = bridge + .source(stream, gpu.identity, size, Format::Bgra8, slots) + .expect("register the camera"); + let (target, target_memory, ..) = gpu.image( + size, + vk::ImageUsageFlags::TRANSFER_SRC | vk::ImageUsageFlags::TRANSFER_DST, + false, + ); + let pool = unsafe { + gpu.device.create_command_pool( + &vk::CommandPoolCreateInfo::default().queue_family_index(gpu.queue_family), + None, + ) + } + .expect("command pool"); + let memory_fd = ash::khr::external_memory_fd::Device::new(&gpu.instance, &gpu.device); + let semaphore_fd = ash::khr::external_semaphore_fd::Device::new(&gpu.instance, &gpu.device); + let mut ring = Vec::with_capacity(slots); + for index in 0..slots { + let (image, memory, allocation_size, memory_type) = gpu.image( + size, + vk::ImageUsageFlags::TRANSFER_DST + | vk::ImageUsageFlags::SAMPLED + | vk::ImageUsageFlags::STORAGE, + true, + ); + let mut kind = vk::SemaphoreTypeCreateInfo::default() + .semaphore_type(vk::SemaphoreType::TIMELINE) + .initial_value(0); + let mut export = vk::ExportSemaphoreCreateInfo::default() + .handle_types(vk::ExternalSemaphoreHandleTypeFlags::OPAQUE_FD); + let timeline = unsafe { + gpu.device.create_semaphore( + &vk::SemaphoreCreateInfo::default() + .push_next(&mut kind) + .push_next(&mut export), + None, + ) + } + .expect("timeline semaphore"); + let (memory_handle, timeline_handle) = unsafe { + ( + memory_fd + .get_memory_fd( + &vk::MemoryGetFdInfoKHR::default() + .memory(memory) + .handle_type(vk::ExternalMemoryHandleTypeFlags::OPAQUE_FD), + ) + .expect("export slot memory"), + semaphore_fd + .get_semaphore_fd( + &vk::SemaphoreGetFdInfoKHR::default() + .semaphore(timeline) + .handle_type(vk::ExternalSemaphoreHandleTypeFlags::OPAQUE_FD), + ) + .expect("export slot timeline"), + ) + }; + // SAFETY: Vulkan returned fresh descriptors owned by the caller. + let (memory_handle, timeline_handle) = unsafe { + ( + OwnedFd::from_raw_fd(memory_handle), + OwnedFd::from_raw_fd(timeline_handle), + ) + }; + source + .import( + index, + memory_handle, + timeline_handle, + allocation_size, + memory_type, + ) + .expect("import the slot"); + ring.push(Slot { + image, + memory, + timeline, + commands: None, + written: false, + }); + } + Self { + gpu, + size, + source: Some(source), + target, + target_memory, + pool, + slots: ring, + rendered: 0, + } + } + + pub fn source(&self) -> &Source { + self.source.as_ref().expect("camera closed") + } + + pub fn wanted(&self) -> bool { + self.source().wanted() + } + + /// Render one solid `rgb` frame and hand it to the bridge. Returns false + /// when every slot is still in use. + pub fn render(&mut self, rgb: [u8; 3], frame: u64, timestamp_us: u64) -> bool { + let Some((index, ready)) = self.source().acquire().expect("acquire a slot") else { + return false; + }; + let device = &self.gpu.device; + let slot = &mut self.slots[index]; + if let Some(previous) = slot.commands.take() { + unsafe { device.free_command_buffers(self.pool, &[previous]) }; + } + let commands = unsafe { + device.allocate_command_buffers( + &vk::CommandBufferAllocateInfo::default() + .command_pool(self.pool) + .level(vk::CommandBufferLevel::PRIMARY) + .command_buffer_count(1), + ) + } + .expect("command buffer")[0]; + let range = vk::ImageSubresourceRange { + aspect_mask: vk::ImageAspectFlags::COLOR, + base_mip_level: 0, + level_count: 1, + base_array_layer: 0, + layer_count: 1, + }; + let layers = vk::ImageSubresourceLayers { + aspect_mask: vk::ImageAspectFlags::COLOR, + mip_level: 0, + base_array_layer: 0, + layer_count: 1, + }; + let barrier = |image, old, new, src_access, dst_access| { + vk::ImageMemoryBarrier::default() + .old_layout(old) + .new_layout(new) + .src_access_mask(src_access) + .dst_access_mask(dst_access) + .src_queue_family_index(vk::QUEUE_FAMILY_IGNORED) + .dst_queue_family_index(vk::QUEUE_FAMILY_IGNORED) + .image(image) + .subresource_range(range) + }; + // Component order, whatever the image's byte order. + let [r, g, b] = rgb.map(|channel| f32::from(channel) / 255.0); + let clear = vk::ClearColorValue { + float32: [r, g, b, 1.0], + }; + unsafe { + device + .begin_command_buffer( + commands, + &vk::CommandBufferBeginInfo::default() + .flags(vk::CommandBufferUsageFlags::ONE_TIME_SUBMIT), + ) + .expect("begin"); + // "Render": after the previous frame's copy read the target. + device.cmd_pipeline_barrier( + commands, + vk::PipelineStageFlags::ALL_COMMANDS, + vk::PipelineStageFlags::TRANSFER, + vk::DependencyFlags::empty(), + &[], + &[], + &[barrier( + self.target, + vk::ImageLayout::UNDEFINED, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + vk::AccessFlags::TRANSFER_READ, + vk::AccessFlags::TRANSFER_WRITE, + )], + ); + device.cmd_clear_color_image( + commands, + self.target, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + &clear, + &[range], + ); + // The plugin's part: the render target into the slot, after every + // earlier use of the slot on this queue. + device.cmd_pipeline_barrier( + commands, + vk::PipelineStageFlags::ALL_COMMANDS, + vk::PipelineStageFlags::TRANSFER, + vk::DependencyFlags::empty(), + &[], + &[], + &[ + barrier( + self.target, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + vk::ImageLayout::TRANSFER_SRC_OPTIMAL, + vk::AccessFlags::TRANSFER_WRITE, + vk::AccessFlags::TRANSFER_READ, + ), + barrier( + slot.image, + if slot.written { + vk::ImageLayout::GENERAL + } else { + vk::ImageLayout::UNDEFINED + }, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + vk::AccessFlags::MEMORY_READ | vk::AccessFlags::MEMORY_WRITE, + vk::AccessFlags::TRANSFER_WRITE, + ), + ], + ); + device.cmd_copy_image( + commands, + self.target, + vk::ImageLayout::TRANSFER_SRC_OPTIMAL, + slot.image, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + &[vk::ImageCopy::default() + .src_subresource(layers) + .dst_subresource(layers) + .extent(vk::Extent3D { + width: self.size.width, + height: self.size.height, + depth: 1, + })], + ); + device.cmd_pipeline_barrier( + commands, + vk::PipelineStageFlags::TRANSFER, + vk::PipelineStageFlags::ALL_COMMANDS, + vk::DependencyFlags::empty(), + &[], + &[], + &[barrier( + slot.image, + vk::ImageLayout::TRANSFER_DST_OPTIMAL, + vk::ImageLayout::GENERAL, + vk::AccessFlags::TRANSFER_WRITE, + vk::AccessFlags::MEMORY_READ, + )], + ); + device.end_command_buffer(commands).expect("end"); + } + let signals = [ready]; + let mut timeline = + vk::TimelineSemaphoreSubmitInfo::default().signal_semaphore_values(&signals); + let buffers = [commands]; + let semaphores = [slot.timeline]; + let submit = vk::SubmitInfo::default() + .push_next(&mut timeline) + .command_buffers(&buffers) + .signal_semaphores(&semaphores); + unsafe { device.queue_submit(self.gpu.queue, &[submit], vk::Fence::null()) } + .expect("submit"); + slot.commands = Some(commands); + slot.written = true; + self.rendered += 1; + self.source() + .submit(index, frame, timestamp_us) + .expect("submit to the bridge"); + true + } +} + +impl Drop for Camera<'_> { + fn drop(&mut self) { + // The plugin's teardown order: close the source, which returns once the + // GPU is done with every slot, then free the images. + drop(self.source.take()); + let device = &self.gpu.device; + unsafe { + let _ = device.queue_wait_idle(self.gpu.queue); + for slot in self.slots.drain(..) { + device.destroy_semaphore(slot.timeline, None); + device.destroy_image(slot.image, None); + device.free_memory(slot.memory, None); + } + device.destroy_command_pool(self.pool, None); + device.destroy_image(self.target, None); + device.free_memory(self.target_memory, None); + } + } +} diff --git a/demo/carla/justfile b/demo/carla/justfile new file mode 100644 index 00000000000..ffbceb1e9a5 --- /dev/null +++ b/demo/carla/justfile @@ -0,0 +1,122 @@ +#!/usr/bin/env just --justfile +# +# The CARLA demo: simulated trucks in one CARLA world, watched and steered from +# a browser over MoQ. See README.md. + +set fallback + +# The simulator is not included: start `just carla server` once and leave it +# running across restarts of everything else. +# +# Run a local relay, both trucks, and the web player. +default: + bun install + bun run concurrently --kill-others --names relay,trucks,web --prefix-colors auto \ + "just relay" \ + "just wait && just wait-port 127.0.0.1 2000 && just worker" \ + "just wait && just web" + +# Run the Python worker: every truck in one shared CARLA world. It reads the +# relay from MOQ_URL itself, so a production token never reaches argv. +worker *ARGS: + @sim/python.sh -m moq_carla_sim.main {{ ARGS }} + +# Run the web player against a relay. +web url='http://localhost:4443': + cd web && VITE_RELAY_URL="{{ url }}" bun --bun vite --open + +# Build the player for carla.moq.dev, against the public demo relay. +build-web: + bun install --frozen-lockfile + cd web && VITE_RELAY_URL=https://cdn.moq.dev/demo bun run build + +# Deploy the player to carla.moq.dev. +deploy: build-web + cd web && bun wrangler deploy + +# Build the worker's virtualenv, with moq's Python bindings from this checkout. +venv: + sim/venv.sh + +# Run the CARLA simulator on the local NVIDIA GPU, in podman. +server: + sim/server.sh + +# Generate the rootless NVIDIA CDI device the simulator runs with. +cdi: + sim/cdi.sh + +# Disable rendering on a simulator no worker owns, so an idle simulator stays idle. +park *ARGS: + @sim/python.sh -m moq_carla_sim.park {{ ARGS }} + +# Measure camera capture capacity on a separate, idle simulator. +benchmark *ARGS: + @sim/python.sh -m moq_carla_sim.benchmark {{ ARGS }} + +# Bake the running town's road network into the player's map; commit the result. +map *ARGS: + @sim/python.sh -m moq_carla_sim.roadmap --out web/src/maps {{ ARGS }} + +# Install the simulator and worker as rootless systemd user units. +service-install: + sim/systemd/install-user-service.sh + +# Start the units now and at login (at boot with `loginctl enable-linger`). +service-enable: + systemctl --user enable --now carla-worker.service + +# Stop the units and remove them from startup. +service-disable: + systemctl --user disable --now carla-worker.service carla-sim.service + +# Show the units' status. +service-status: + systemctl --user status carla-worker.service carla-sim.service + +# Follow the units' logs. +service-logs: + journalctl --user -u carla-worker.service -u carla-sim.service -f + +# None of these need CARLA or a GPU. `just js` and `just rs` cover the player and +# the bridge. +# +# Lint and test the worker and its launchers. +check: + uv sync --project ../.. --locked --no-install-workspace + cd sim && uv run --project ../../.. --no-sync ruff check . + cd sim && uv run --project ../../.. --no-sync ruff format --check . + cd sim && uv run --project ../../.. --no-sync pyright + cd sim && uv run --project ../../.. --no-sync pytest -q + sim/python.test.sh + sim/server.test.sh + sim/systemd/install-user-service.test.sh + bridge/gpu.test.sh + +# Auto-fix the worker's lint and formatting. +fix: + uv sync --project ../.. --no-install-workspace + cd sim && uv run --project ../../.. --no-sync ruff check --fix . + cd sim && uv run --project ../../.. --no-sync ruff format . + +# A Vulkan stand-in plays the CARLA plugin. A host with no NVIDIA GPU skips, and +# one whose NVIDIA driver is not loaded fails. +# +# Run the GPU bridge end to end on this host's NVIDIA GPU. +bridge-gpu: + cd ../.. && demo/carla/bridge/gpu.sh cargo test --locked -p moq-carla-bridge --test gpu -- --ignored + +# Wait for a TCP port to accept connections. bash's /dev/tcp rather than nc, +# because the user units gate on this at boot and nc is not in the devshell. +[private] +wait-port host port tries="60": + #!/usr/bin/env bash + set -euo pipefail + for _ in $(seq 1 {{ tries }}); do + if (exec 3<>/dev/tcp/{{ host }}/{{ port }}) 2>/dev/null; then + exit 0 + fi + sleep 0.5 + done + echo "timed out waiting for {{ host }}:{{ port }}" >&2 + exit 1 diff --git a/demo/carla/sim/.gitignore b/demo/carla/sim/.gitignore new file mode 100644 index 00000000000..c373c7551a1 --- /dev/null +++ b/demo/carla/sim/.gitignore @@ -0,0 +1,7 @@ +# The runtime venv, rebuilt by venv.sh. +.venv/ +__pycache__/ +*.egg-info/ +# Left behind by the Python checks. +.pytest_cache/ +.ruff_cache/ diff --git a/demo/carla/sim/cdi.sh b/demo/carla/sim/cdi.sh new file mode 100755 index 00000000000..8cd879f0571 --- /dev/null +++ b/demo/carla/sim/cdi.sh @@ -0,0 +1,26 @@ +#!/usr/bin/env bash +# Generate a rootless Podman CDI spec whose hook survives Nix garbage collection. +set -euo pipefail + +state_dir="${XDG_STATE_HOME:-$HOME/.local/state}/moq" +cdi_dir="${XDG_CONFIG_HOME:-$HOME/.config}/cdi" +toolkit="$state_dir/nvidia-container-toolkit" +hook="$toolkit/bin/nvidia-ctk" + +mkdir -p "$state_dir" "$cdi_dir" + +# The CDI spec embeds the hook's absolute path. A bare `nix shell` leaves that +# path unrooted, so a later Nix GC breaks every container start. The out-link is +# both the stable path written into the spec and the GC root that keeps it alive. +nix build --out-link "$toolkit" nixpkgs#nvidia-container-toolkit + +tmp="$(mktemp "$cdi_dir/nvidia.XXXXXX.yaml")" +trap 'rm -f "$tmp"' EXIT +"$hook" cdi generate \ + --mode=nvml \ + --nvidia-cdi-hook-path="$hook" \ + --output="$tmp" +mv "$tmp" "$cdi_dir/nvidia.yaml" +trap - EXIT + +printf 'carla: wrote %s with rooted hook %s\n' "$cdi_dir/nvidia.yaml" "$hook" >&2 diff --git a/demo/carla/sim/moq_carla_sim/__init__.py b/demo/carla/sim/moq_carla_sim/__init__.py new file mode 100644 index 00000000000..096a8dad796 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/__init__.py @@ -0,0 +1,12 @@ +"""Simulated trucks in one shared CARLA world, published over MoQ. + +The trucks drive themselves under Traffic Manager and can be taken over from +the browser. Every driver command passes the safety gate in `drive` before it +reaches CARLA. + +Python rather than Rust because the `carla` crate's C++ bindings (`carla-sys`, +via autocxx) only build against LLVM 11-13, which neither nixpkgs nor Fedora +still ship. CARLA's own client is a Python wheel, and moq's Python bindings +cover publishing and the JSON control tracks, so the whole worker fits in one +process with no FFI of ours in the middle. +""" diff --git a/demo/carla/sim/moq_carla_sim/aerial.py b/demo/carla/sim/moq_carla_sim/aerial.py new file mode 100644 index 00000000000..8fac0fc7340 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/aerial.py @@ -0,0 +1,155 @@ +"""Capture a calibrated aerial image without changing the running CARLA world.""" + +from __future__ import annotations + +import argparse +import json +import math +import queue +import time +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Any + + +@dataclass(frozen=True) +class Camera: + x: float + y: float + z: float + pitch: float = -90.0 + yaw: float = -90.0 + fov: float = 20.0 + + +def calibration( + bbox: list[float], padding: float = 30.0, ground_z: float = 0.0, fov: float = 20.0 +) -> tuple[Camera, list[float]]: + """Fit a square pinhole footprint; image right is +x and image down is +y. + + The footprint is exact on the chosen ground plane. Elevated roofs and + uneven terrain retain perspective displacement, so this is not an orthophoto. + """ + if len(bbox) != 4 or not all(math.isfinite(value) for value in bbox): + raise ValueError("bbox must contain four finite world coordinates") + left, top, right, bottom = bbox + if right <= left or bottom <= top: + raise ValueError("bbox must have positive width and height") + if not math.isfinite(padding) or padding < 0: + raise ValueError("padding must be finite and nonnegative") + if not math.isfinite(ground_z) or not math.isfinite(fov) or not 1 <= fov <= 90: + raise ValueError("ground height must be finite and field of view must be 1..90 degrees") + side = max(right - left, bottom - top) + 2 * padding + x, y = (left + right) / 2, (top + bottom) / 2 + half = side / 2 + camera = Camera(x=x, y=y, z=ground_z + half / math.tan(math.radians(fov / 2)), fov=fov) + return camera, [x - half, y - half, x + half, y + half] + + +def capture(world: Any, camera: Camera, resolution: int, timeout: float) -> Any: + """Own just one temporary camera; the existing simulator owns all ticks.""" + import carla + + if resolution < 256 or resolution > 4096: + raise ValueError("resolution must be 256..4096 pixels") + if not math.isfinite(timeout) or timeout <= 0: + raise ValueError("capture timeout must be finite and positive") + if world.get_settings().no_rendering_mode: + raise RuntimeError("CARLA rendering is parked; open a truck camera in the demo and rerun") + blueprint = world.get_blueprint_library().find("sensor.camera.rgb") + for name, value in { + "image_size_x": str(resolution), + "image_size_y": str(resolution), + "fov": str(camera.fov), + "sensor_tick": "0.5", + "lens_k": "0", + "lens_kcube": "0", + "motion_blur_intensity": "0", + }.items(): + blueprint.set_attribute(name, value) + transform = carla.Transform( + carla.Location(x=camera.x, y=camera.y, z=camera.z), + carla.Rotation(pitch=camera.pitch, yaw=camera.yaw, roll=0), + ) + frames: queue.Queue[Any] = queue.Queue(maxsize=1) + + def receive(frame: Any) -> None: + try: + frames.put_nowait(frame) + except queue.Full: + # A capture only needs one pending frame, never a backlog of images. + return + + sensor = world.spawn_actor(blueprint, transform) + try: + sensor.listen(receive) + deadline = time.monotonic() + timeout + + def next_frame() -> Any: + remaining = deadline - time.monotonic() + if remaining <= 0: + raise TimeoutError("no aerial image before capture deadline") + try: + return frames.get(timeout=remaining) + except queue.Empty as error: + raise TimeoutError("no aerial image before capture deadline") from error + + # Discard initial exposure frames without driving or changing world time. + for _ in range(3): + next_frame() + return next_frame() + finally: + try: + sensor.stop() + finally: + if not sensor.destroy(): + raise RuntimeError(f"failed to destroy temporary aerial camera {sensor.id}") + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--carla-host", default="127.0.0.1") + parser.add_argument("--carla-port", type=int, default=2000) + parser.add_argument("--timeout", type=float, default=30.0) + parser.add_argument("--resolution", type=int, default=2048) + parser.add_argument("--padding", type=float, default=30.0, help="metres outside the baked roads") + parser.add_argument("--ground-z", type=float, default=0.0, help="calibration plane in world metres") + parser.add_argument("--fov", type=float, default=20.0) + parser.add_argument("--out", type=Path, default=Path(__file__).resolve().parents[2] / "web/src/maps") + args = parser.parse_args(argv) + if not math.isfinite(args.timeout) or args.timeout <= 0: + parser.error("--timeout must be finite and positive") + + import carla + + client = carla.Client(args.carla_host, args.carla_port) + client.set_timeout(args.timeout) + world = client.get_world() + name = world.get_map().name.rsplit("/", 1)[-1] + roads = json.loads((args.out / f"{name}.json").read_text()) + if roads["name"] != name: + raise ValueError("baked road map name does not match the running world") + camera, bbox = calibration(roads["bbox"], args.padding, args.ground_z, args.fov) + frame = capture(world, camera, args.resolution, args.timeout) + image = args.out / f"{name}.aerial.png" + frame.save_to_disk(str(image)) + if not image.is_file() or image.stat().st_size == 0: + raise RuntimeError(f"CARLA did not save an image to {image}") + manifest = { + "name": name, + "image": image.name, + "bbox": bbox, + "width": frame.width, + "height": frame.height, + "groundZ": args.ground_z, + "camera": asdict(camera), + "frame": frame.frame, + } + path = args.out / f"{name}.aerial.json" + path.write_text(json.dumps(manifest, indent=2) + "\n") + print(f"Wrote {image} and {path}; verify road alignment before shipping") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/demo/carla/sim/moq_carla_sim/benchmark.py b/demo/carla/sim/moq_carla_sim/benchmark.py new file mode 100644 index 00000000000..1ce091f6c94 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/benchmark.py @@ -0,0 +1,189 @@ +"""Measure camera throughput on an otherwise idle CARLA server (no MoQ required).""" + +from __future__ import annotations + +import argparse +import json +import math +import statistics +import threading +import time +from contextlib import ExitStack + + +class Frames: + """Keep frame identities, not images, so delivery cannot hide behind tick FPS.""" + + def __init__(self) -> None: + self.condition = threading.Condition() + self.received: dict[int, float] = {} + self.duplicates = 0 + + def receive(self, image) -> None: + with self.condition: + if image.frame in self.received: + self.duplicates += 1 + else: + self.received[image.frame] = time.monotonic() + self.condition.notify_all() + + def finish(self, expected: dict[int, float], deadline: float) -> dict: + with self.condition: + self.condition.wait_for( + lambda: expected.keys() <= self.received.keys(), max(0, deadline - time.monotonic()) + ) + missing = sorted(expected.keys() - self.received.keys()) + # Same client monotonic clock on both sides: delay from requesting + # this world tick to receiving its image, not glass-to-glass latency. + delays = sorted( + (self.received[frame] - started) * 1000 for frame, started in expected.items() if frame in self.received + ) + return { + "delivered": len(expected) - len(missing), + "missing": missing, + "duplicates": self.duplicates, + "callback_delay_p95_ms": delays[math.ceil(len(delays) * 0.95) - 1] if delays else None, + "callback_delay_max_ms": max(delays) if delays else None, + } + + +def summary(durations: list[float]) -> dict: + ordered = sorted(durations) + return { + "tick_fps": len(durations) / sum(durations), + "mean_ms": statistics.mean(durations) * 1000, + "p50_ms": statistics.median(durations) * 1000, + "p95_ms": ordered[math.ceil(len(ordered) * 0.95) - 1] * 1000, + "max_ms": max(durations) * 1000, + "over_30fps_budget": sum(duration > 1 / 30 for duration in durations), + } + + +def require_idle(world, timeout: float) -> None: + if world.get_settings().synchronous_mode: + raise RuntimeError("benchmark requires an idle asynchronous server; another client may own its ticks") + # A new client can see an empty actor cache before its first world snapshot. + # Receive that snapshot without taking ownership of the simulation clock. + world.wait_for_tick(timeout) + actors = world.get_actors() + if any(actors.filter(pattern) for pattern in ("vehicle.*", "walker.*", "sensor.*")): + raise RuntimeError("benchmark requires an empty server; use a separate CARLA instance") + + +def destroy(actor) -> None: + if not actor.destroy(): + raise RuntimeError(f"failed to destroy benchmark actor {actor.id}") + + +def measure( + client, cameras: int, width: int, height: int, ticks: int, warmup: int, timeout: float, listen: bool +) -> dict: + import carla + + world = client.get_world() + require_idle(world, timeout) + original = world.get_settings() + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1 / 30 + settings.no_rendering_mode = False + # ExitStack attempts every cleanup even if one actor RPC fails. Restore the + # clock last, after removing only actors this measurement created. + with ExitStack() as cleanup: + cleanup.callback(world.apply_settings, original) + world.apply_settings(settings) + library = world.get_blueprint_library() + vehicle = world.spawn_actor( + library.find("vehicle.carlamotors.european_hgv"), world.get_map().get_spawn_points()[0] + ) + cleanup.callback(destroy, vehicle) + vehicle.set_simulate_physics(False) + collectors = [] + for index in range(cameras): + blueprint = library.find("sensor.camera.rgb") + blueprint.set_attribute("image_size_x", str(width)) + blueprint.set_attribute("image_size_y", str(height)) + blueprint.set_attribute("sensor_tick", "0.0") + sensor = world.spawn_actor( + blueprint, carla.Transform(carla.Location(x=-6, z=3 + index * 0.1)), attach_to=vehicle + ) + cleanup.callback(destroy, sensor) + if listen: + frames = Frames() + collectors.append(frames) + sensor.listen(frames.receive) + cleanup.callback(sensor.stop) + for _ in range(warmup): + world.tick() + durations = [] + expected = {} + for _ in range(ticks): + started = time.monotonic() + frame = world.tick() + durations.append(time.monotonic() - started) + expected[frame] = started + if len(expected) != ticks or max(expected) - min(expected) != ticks - 1: + raise RuntimeError("nonconsecutive world frames; another client may be ticking this server") + # GPU cameras are pipelined. Waiting for each same-tick image deadlocks + # that pipeline and measures the client barrier instead of capacity. + # Drain extra world ticks without counting their images or durations. + for _ in range(8): + world.tick() + deadline = time.monotonic() + timeout + delivery = [collector.finish(expected, deadline) for collector in collectors] + result = { + "cameras": cameras, + "width": width, + "height": height, + "listening": listen, + "ticks": ticks, + "warmup": warmup, + **summary(durations), + "delivery": delivery, + } + print(json.dumps(result), flush=True) + if any(item["missing"] or item["duplicates"] for item in delivery): + raise RuntimeError("camera delivery was incomplete or duplicated; this is not a passing throughput result") + return result + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--host", default="127.0.0.1") + parser.add_argument("--port", type=int, default=2200) + parser.add_argument("--ticks", type=int, default=300) + parser.add_argument("--warmup", type=int, default=60) + parser.add_argument("--timeout", type=float, default=30) + parser.add_argument("--cameras", type=int, nargs="+", default=[0, 1, 2, 3, 4]) + parser.add_argument("--sizes", nargs="+", default=["320x180"]) + parser.add_argument("--no-listen", action="store_true", help="control: spawn cameras without activating capture") + args = parser.parse_args(argv) + if args.ticks < 1 or args.warmup < 1 or any(count < 0 for count in args.cameras): + parser.error("ticks and warmup must be positive, camera counts nonnegative") + if not math.isfinite(args.timeout) or args.timeout <= 0 or not 1 <= args.port <= 65535: + parser.error("timeout must be finite and positive, port must be 1..65535") + sizes = [] + for size in args.sizes: + try: + width, height = map(int, size.split("x")) + if not (1 <= width <= 4096 and 1 <= height <= 4096): + raise ValueError + except ValueError: + parser.error("sizes must be WIDTHxHEIGHT with dimensions 1..4096") + sizes.append((width, height)) + + import carla + + client = carla.Client(args.host, args.port) + client.set_timeout(args.timeout) + if client.get_client_version() != client.get_server_version(): + raise RuntimeError("CARLA client/server versions differ") + print(json.dumps({"server": client.get_server_version(), "map": client.get_world().get_map().name}), flush=True) + for width, height in sizes: + for cameras in args.cameras: + measure(client, cameras, width, height, args.ticks, args.warmup, args.timeout, not args.no_listen) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/demo/carla/sim/moq_carla_sim/bridge.py b/demo/carla/sim/moq_carla_sim/bridge.py new file mode 100644 index 00000000000..019e4ef13ab --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/bridge.py @@ -0,0 +1,457 @@ +"""The worker's side of the native CARLA video bridge. + +The bridge (`demo/carla/bridge`) runs inside CARLA: it copies each camera's render +target on the GPU, converts and scales it with CUDA, and encodes it with NVENC. +This module speaks its Unix-socket protocol, mirrored from +`demo/carla/bridge/src/protocol.rs`, and republishes the compressed frames on the +truck's broadcast. Raw pixels never reach Python. + +Each message is `u32 length | u8 kind | body`: little-endian integers, a string +as `u16 length | UTF-8`, a blob as `u32 length | bytes`. +""" + +from __future__ import annotations + +import asyncio +import logging +import struct +from dataclasses import dataclass +from datetime import timedelta +from typing import Callable + +import moq + +log = logging.getLogger(__name__) + +VERSION = 1 +MAX_MESSAGE = 16 << 20 +MAX_RENDITIONS = 8 + +_HELLO = 0x01 +_OPEN = 0x02 +_DEMAND = 0x03 +_KEYFRAME = 0x04 +_CLOSE = 0x05 + +_EVENT_HELLO = 0x81 +_EVENT_CONFIG = 0x82 +_EVENT_FRAME = 0x83 +_EVENT_ERROR = 0x84 + + +class BridgeError(Exception): + """The bridge reported a failure, broke the protocol, or went away.""" + + +@dataclass(frozen=True) +class Rendition: + """One encoded size of a stream, published as track `name`.""" + + name: str + width: int + height: int + bitrate: int + framerate: int + + +@dataclass(frozen=True) +class Hello: + version: int + + +@dataclass(frozen=True) +class Config: + """A rendition's Annex-B parameter sets, sent before any of its frames.""" + + id: int + rendition: int + init: bytes + + +@dataclass(frozen=True) +class Frame: + """One encoded access unit, stamped with CARLA's frame and simulation time.""" + + id: int + rendition: int + frame: int + timestamp_us: int + keyframe: bool + payload: bytes + + +@dataclass(frozen=True) +class Error: + """Stream 0 is the connection itself; any other stream has stopped.""" + + id: int + message: str + + +Event = Hello | Config | Frame | Error + + +def _message(kind: int, body: bytes) -> bytes: + return struct.pack(" bytes: + encoded = value.encode() + return struct.pack(" bytes: + return _message(_HELLO, struct.pack(" bytes: + body = struct.pack(" bytes: + return _message(_DEMAND, struct.pack(" bytes: + return _message(_KEYFRAME, struct.pack(" bytes: + return _message(_CLOSE, struct.pack(" None: + self._data = memoryview(data) + self._offset = 0 + + def take(self, size: int) -> bytes: + if self._offset + size > len(self._data): + raise BridgeError(f"truncated message: need {size} bytes, have {len(self._data) - self._offset}") + value = bytes(self._data[self._offset : self._offset + size]) + self._offset += size + return value + + def unpack(self, fmt: str) -> tuple[int, ...]: + return struct.unpack(fmt, self.take(struct.calcsize(fmt))) + + def string(self) -> str: + (size,) = self.unpack(" bytes: + (size,) = self.unpack(" None: + if self._offset != len(self._data): + raise BridgeError(f"{len(self._data) - self._offset} trailing bytes") + + +def decode_event(kind: int, body: bytes) -> Event: + reader = _Body(body) + event: Event + if kind == _EVENT_HELLO: + (version,) = reader.unpack(" Event | None: + """The next event, or None when the bridge closed the socket between messages.""" + try: + header = await reader.readexactly(4) + except asyncio.IncompleteReadError as err: + if err.partial: + raise BridgeError("bridge closed mid-message") from err + return None + (length,) = struct.unpack(" MAX_MESSAGE: + raise BridgeError(f"message length {length}") + try: + message = await reader.readexactly(length) + except asyncio.IncompleteReadError as err: + raise BridgeError("bridge closed mid-message") from err + return decode_event(message[0], message[1:]) + + +class Bridge: + """One connection to the bridge, shared by every truck's stream. + + `run` is the only reader: it must be running while streams open, since the + parameter sets each `open` waits for arrive through it. + """ + + def __init__(self, reader: asyncio.StreamReader, writer: asyncio.StreamWriter) -> None: + self._reader = reader + self._writer = writer + self._streams: dict[int, BridgeStream] = {} + # Ids are never reused, so a closed one stays known: the bridge may + # still deliver frames it encoded before it saw the close. + self._closed: set[int] = set() + self._next_id = 1 + # Why `run` ended; every stream opened after that fails with it. + self._failure: BaseException | None = None + + @classmethod + async def connect(cls, path: str) -> Bridge: + reader, writer = await asyncio.open_unix_connection(path, limit=MAX_MESSAGE) + bridge = cls(reader, writer) + bridge._send(encode_hello()) + hello = await read_event(reader) + if hello != Hello(VERSION): + raise BridgeError(f"expected hello {VERSION}, got {hello!r}") + return bridge + + def disconnect(self) -> None: + """Close the connection; the bridge stops every stream on it.""" + self._writer.close() + + def _send(self, message: bytes) -> None: + # Control messages are tiny and rare; the transport buffers them. + self._writer.write(message) + + async def open(self, name: str, renditions: list[Rendition], broadcast: moq.BroadcastProducer) -> BridgeStream: + """Encode the cameras registered as `name` into `broadcast`'s tracks. + + Returns once every rendition is advertised, before anyone subscribes. + """ + if not renditions or len(renditions) > MAX_RENDITIONS: + raise ValueError(f"{name}: {len(renditions)} renditions") + if self._failure is not None: + raise BridgeError(f"{name}: the bridge connection has ended") from self._failure + id = self._next_id + self._next_id += 1 + stream = BridgeStream(self, id, name, renditions, broadcast) + self._streams[id] = stream + self._send(encode_open(id, name, renditions)) + await stream.advertised() + return stream + + async def run(self) -> None: + """Deliver events until the connection ends. Always raises: the bridge + going away, or any error it reports, ends the worker's video.""" + try: + await self._deliver() + except BaseException as err: + # Nothing else answers an `open` still waiting for its parameter + # sets, so it would wait forever; later ones fail straight away. + self._failure = err + for stream in self._streams.values(): + stream.fail(err) + raise + + async def _deliver(self) -> None: + while True: + event = await read_event(self._reader) + if event is None: + raise BridgeError("bridge closed the connection") + if isinstance(event, Error): + stream = self._streams.get(event.id) + where = stream.name if stream else "bridge" + raise BridgeError(f"{where}: {event.message}") + if isinstance(event, Hello): + raise BridgeError("unexpected hello") + stream = self._streams.get(event.id) + if stream is None: + if event.id in self._closed: + continue + raise BridgeError(f"event for unknown stream {event.id}") + if isinstance(event, Config): + stream.configure(event) + else: + stream.publish(event) + + def demand(self, id: int, renditions: int) -> None: + self._send(encode_demand(id, renditions)) + + def keyframe(self, id: int, rendition: int) -> None: + self._send(encode_keyframe(id, rendition)) + + def close(self, id: int) -> None: + self._streams.pop(id, None) + self._closed.add(id) + self._send(encode_close(id)) + + +class BridgeStream: + """One broadcast's video tracks, fed by the bridge. + + Each rendition's subscribers drive the bridge's demand, which decides what + CARLA renders: the smallest camera covering the largest demanded rendition, + scaled on the GPU for the rest, and nothing at all without subscribers. + """ + + def __init__( + self, + bridge: Bridge, + id: int, + name: str, + renditions: list[Rendition], + broadcast: moq.BroadcastProducer, + ) -> None: + self._bridge = bridge + self.id = id + self.name = name + self._renditions = renditions + self._broadcast = broadcast + self._tracks: list[moq.media.TrackProducer | None] = [None] * len(renditions) + self._advertised: asyncio.Future[None] = asyncio.get_running_loop().create_future() + self._demand = 0 + self._on_demand: Callable[[bool], None] | None = None + self._finished = False + # Simulation time of the first frame, shared by every rendition so their + # timelines line up. + self._origin_us: int | None = None + self.published = [0] * len(renditions) + + async def advertised(self) -> None: + await self._advertised + + @property + def rendition_names(self) -> list[str]: + return [rendition.name for rendition in self._renditions] + + def fail(self, err: BaseException) -> None: + """End a pending `advertised` with the connection's failure.""" + if not self._advertised.done(): + failure = BridgeError(f"{self.name}: the bridge connection ended before the stream was advertised") + failure.__cause__ = err + self._advertised.set_exception(failure) + + def configure(self, config: Config) -> None: + if config.rendition >= len(self._renditions) or self._tracks[config.rendition] is not None: + raise BridgeError(f"{self.name}: unexpected config for rendition {config.rendition}") + rendition = self._renditions[config.rendition] + # The parameter sets resolve the catalog entry up front, so a viewer can + # subscribe before the first frame exists, which is what starts it. + self._tracks[config.rendition] = moq.media.TrackProducer.video( + self._broadcast, + moq.media.VideoInit( + format=moq.media.VideoFormat.AVC3, + data=config.init, + hint=moq.media.VideoHint( + coded=moq.media.Dimensions(width=rendition.width, height=rendition.height), + bitrate=rendition.bitrate, + framerate=float(rendition.framerate), + ), + ), + target=moq.media.Named(rendition.name), + ) + if all(track is not None for track in self._tracks) and not self._advertised.done(): + self._advertised.set_result(None) + + def publish(self, frame: Frame) -> None: + if frame.rendition >= len(self._tracks): + raise BridgeError(f"{self.name}: frame for rendition {frame.rendition}") + track = self._tracks[frame.rendition] + if track is None: + raise BridgeError(f"{self.name}: frame before config for rendition {frame.rendition}") + if self._origin_us is None: + self._origin_us = frame.timestamp_us + timestamp = timedelta(microseconds=max(0, frame.timestamp_us - self._origin_us)) + track.write_frame(frame.payload, timestamp) + # Live encoder output: records the handoff for the catalog's jitter. + track.flush(timestamp) + self.published[frame.rendition] += 1 + + async def watch_demand(self, on_demand: Callable[[bool], None] | None = None) -> None: + """Forward each rendition's subscribers to the bridge until cancelled. + + `on_demand` hears whether any rendition is wanted, before the bridge + starts rendering and after it stops, so what it prepares (the + simulator's renderer) is ready for the first picture. + """ + self._on_demand = on_demand + async with asyncio.TaskGroup() as group: + for index, track in enumerate(self._tracks): + assert track is not None, "watch_demand before advertised" + group.create_task(self._watch(index, track.demand()), name=f"demand:{self.name}:{index}") + + async def _watch(self, index: int, demand: moq.TrackDemand) -> None: + while True: + while await demand.wanted_changed() is None: + pass + self._set_demand(index, True) + try: + while await demand.wanted_changed() is not None: + pass + finally: + self._set_demand(index, False) + + def _set_demand(self, index: int, used: bool) -> None: + # A watcher cancelled before `finish` may still clear its bit; the bridge + # would refuse demand for a closed stream and drop the connection. + if self._finished: + return + mask = self._demand | (1 << index) if used else self._demand & ~(1 << index) + if mask == self._demand: + return + # A failed `on_demand` commits nothing and fails the watcher. + if not self._demand and self._on_demand is not None: + self._on_demand(True) + self._demand = mask + self._bridge.demand(self.id, mask) + if not mask and self._on_demand is not None: + self._on_demand(False) + + def keyframe(self, rendition: int) -> None: + self._bridge.keyframe(self.id, rendition) + + def finish(self) -> None: + """Stop encoding and end the tracks cleanly for subscribers. Idempotent: + the bridge refuses a second Close, which would drop the connection + every truck's video shares.""" + if self._finished: + return + self._finished = True + self._bridge.close(self.id) + for track in self._tracks: + if track is None: + continue + try: + track.finish() + # uniffi's stubs declare the error a plain class; at runtime it is an Exception. + except moq.Error as err: # pyright: ignore[reportGeneralTypeIssues] + log.debug("%s: finishing a video track: %s", self.name, err) + + +__all__ = [ + "Bridge", + "BridgeError", + "BridgeStream", + "Config", + "Error", + "Frame", + "Hello", + "Rendition", + "decode_event", + "encode_close", + "encode_demand", + "encode_hello", + "encode_keyframe", + "encode_open", + "read_event", +] diff --git a/demo/carla/sim/moq_carla_sim/drive.py b/demo/carla/sim/moq_carla_sim/drive.py new file mode 100644 index 00000000000..e764c1219d5 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/drive.py @@ -0,0 +1,245 @@ +"""The control safety gate. + +Every driver command passes through here: sanitize the axes, check the +single-driver lease, honour the latched e-stop. Nothing downstream may act on a +command this gate did not accept, so CARLA's actuation sits strictly below it, +exactly where a real vehicle-bus backend would. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field, replace +from enum import Enum +from typing import Literal + +SHIFT_MAX_THROTTLE = 0.01 + + +class Gear(str, Enum): + """Transmission gear, as the `control` and `status` tracks spell it.""" + + NEUTRAL = "neutral" + FORWARD = "forward" + REVERSE = "reverse" + + +def _clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) + + +@dataclass(frozen=True) +class DriveState: + """The actuation request: what the truck has been asked to do right now. + + The defaults are the safe resting state: stopped, brake on, neutral. + """ + + steering: float = 0.0 + throttle: float = 0.0 + brake: float = 1.0 + gear: Gear = Gear.NEUTRAL + + def sanitized(self) -> DriveState: + """Clamp analog axes into range. + + A misbehaving or malicious driver must never be able to request + out-of-range actuation. + """ + return replace( + self, + steering=_clamp(self.steering, -1.0, 1.0), + throttle=_clamp(self.throttle, 0.0, 1.0), + brake=_clamp(self.brake, 0.0, 1.0), + ) + + @staticmethod + def stopped() -> DriveState: + """The all-stop state used for e-stop and when no driver is connected.""" + return DriveState() + + +@dataclass(frozen=True) +class Accepted: + """A command the truck has accepted: sanitized, lease-checked, e-stop aware. + + This is the only thing an actuator may act on. + """ + + state: DriveState = field(default_factory=DriveState.stopped) + estopped: bool = False + + +@dataclass(frozen=True) +class TimestampEntry: + """One media-clock timestamp from the driver, in milliseconds.""" + + label: str + ts: float + + +CommandKind = Literal[ + "drive", + "estop", + "reset_estop", + "respawn", + "driver_left", + "watchdog_expired", +] + + +@dataclass(frozen=True) +class Command: + """A parsed driver command with the originating driver attached. + + `drive`, `estop`, `reset_estop`, and `respawn` arrive on the wire; + `driver_left` and `watchdog_expired` are synthesized locally. + + `driver_id` is the driver's path segment, which `status` echoes. `epoch` is + the route epoch its broadcast was announced under: the same id announced + under another epoch is another run of the driver, so it is a different + driver for the lease. + """ + + kind: CommandKind + driver_id: str = "" + epoch: str | None = None + sequence: int = 0 + state: DriveState | None = None + timestamps: tuple[TimestampEntry, ...] = () + + +class Drive: + """The truck's live control state machine. + + The first driver to send a command owns the truck until it disconnects or + its command lease expires. Other drivers cannot silently steal control. + + The lease binds to the driver's broadcast path and the epoch it was + announced under, so a driver whose broadcast ends or restarts loses it. + It is still cooperative, not enforced: the holder's id is echoed on the + public `status` track, so anyone allowed to publish there can announce at + that path under a newer epoch, which ends the holder's broadcast and frees + the lease for the newcomer. This stops two honest drivers colliding; it + does not stop a deliberate takeover. + """ + + def __init__(self) -> None: + self._driver: str | None = None + self._epoch: str | None = None + self._sequence: int | None = None + self._state = DriveState.stopped() + self._estopped = False + self._estop_by: str | None = None + + @property + def driver(self) -> str | None: + return self._driver + + @property + def sequence(self) -> int | None: + return self._sequence + + @property + def state(self) -> DriveState: + return self._state + + @property + def estopped(self) -> bool: + return self._estopped + + @property + def estop_by(self) -> str | None: + return self._estop_by + + def holds(self, command: Command) -> bool: + """Whether `command` comes from the run of the driver holding the lease.""" + return self._driver is not None and (self._driver, self._epoch) == (command.driver_id, command.epoch) + + def accepted(self) -> Accepted: + return Accepted(state=self._state, estopped=self._estopped) + + def synchronize(self, state: DriveState) -> bool: + """Adopt live vehicle feedback before an unclaimed manual handoff.""" + if self._driver is not None or self._estopped: + return False + self._state = state.sanitized() + return True + + def apply(self, command: Command) -> bool: + """Apply a command, returning whether it changed or refreshed state.""" + if command.kind == "drive": + if self._estopped or not (self._driver is None or self.holds(command)): + return False + claiming = self._driver is None + self._driver = command.driver_id + self._epoch = command.epoch + self._sequence = command.sequence + state = (command.state or DriveState.stopped()).sanitized() + if ( + state.gear is not self._state.gear + and not self._can_shift(state) + and not (claiming and self._can_initialize_takeover(self._state, state)) + ): + state = replace(state, gear=self._state.gear) + self._state = state + + elif command.kind == "estop": + was_estopped = self._estopped + # E-stop is a safety control anyone may trigger, so it always + # latches and brings the truck to a stop, but it must never transfer + # the driving lease to its sender. Otherwise a bystander could + # e-stop, then reset_estop, and drive off with a lease they never + # legitimately claimed. Only refresh the acknowledgement sequence + # when the sender already holds the lease, so a bystander's e-stop + # cannot masquerade as the authoritative command either. + if self.holds(command): + self._sequence = command.sequence + self._state = DriveState.stopped() + self._estopped = True + if not was_estopped: + self._estop_by = command.driver_id + + elif command.kind == "reset_estop": + if not self._estopped: + return False + # Reset clears the global latch without claiming or transferring the + # lease. Only the holder's sequence is authoritative for that lease. + if self.holds(command): + self._sequence = command.sequence + self._state = DriveState.stopped() + self._estopped = False + self._estop_by = None + + elif command.kind == "driver_left": + if not self.holds(command): + return False + self._clear_driver() + + elif command.kind == "watchdog_expired": + if self._driver is None: + return False + self._clear_driver() + + return True + + @staticmethod + def _can_shift(state: DriveState) -> bool: + """Require some brake and a released throttle for a gear change.""" + return state.brake > 0.0 and state.throttle <= SHIFT_MAX_THROTTLE + + @staticmethod + def _can_initialize_takeover(current: DriveState, requested: DriveState) -> bool: + """Let a new CARLA lease synchronize into forward without touching either pedal.""" + return ( + current.gear is Gear.NEUTRAL + and requested.gear is Gear.FORWARD + and requested.steering == 0.0 + and requested.throttle == 0.0 + and requested.brake == 0.0 + ) + + def _clear_driver(self) -> None: + self._driver = None + self._epoch = None + self._sequence = None + self._state = DriveState.stopped() diff --git a/demo/carla/sim/moq_carla_sim/handback.py b/demo/carla/sim/moq_carla_sim/handback.py new file mode 100644 index 00000000000..861595eeca8 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/handback.py @@ -0,0 +1,146 @@ +"""Steering a released truck onto a lane Traffic Manager will follow. + +Traffic Manager seeds a vehicle it takes back from the waypoint nearest its +position, heading be damned, and steers hard for it (`LocalizationStage::Update` +via `InMemoryMap::GetWaypoint`; the client API has no say in that seed). A truck +centred on a lane that runs its way is handed over without a twitch. One +straddling the centre line, angled, or inside a junction is aimed at the +oncoming or crossing lane and swerves at whatever speed it has. So a released +truck first drives itself onto such a lane, coasting, and only then lets Traffic +Manager have it. + +Pure geometry in CARLA's left-handed frame (x forward, y right, z up): steer is +positive to the right, and the sign of a 2D cross product says which side of the +truck a point lies on. Nothing here touches the simulator, so it is testable +without one. +""" + +from __future__ import annotations + +import math + +import carla + +# Traffic Manager takes a truck this close to a lane's centre line, and this well +# aligned with it, without a visible correction: the nearest waypoint is then on +# that lane, and the ones ahead of the truck are the ones it steers for. +LATERAL_TOLERANCE = 0.5 +HEADING_TOLERANCE = math.radians(8.0) + +# Pure pursuit aims at the lane two seconds ahead, bounded so a crawling truck +# still looks past its bumper and a fast one does not cut corners. Two seconds +# rather than one keeps the merge gentle: a 30 degree heading error at 36 km/h +# asks for about a third of the steering lock, not two thirds. The lane is +# walked to the target in steps, because `Waypoint.next` forks where the lane +# enters a junction and any one fork is an arbitrary exit; a target closer than +# the minimum means the junction is too near to merge before it. +LOOKAHEAD_SECONDS = 2.0 +LOOKAHEAD_MIN = 8.0 +LOOKAHEAD_MAX = 30.0 +LOOKAHEAD_STEP = 2.0 + +# The HGV, near enough: pure pursuit needs a wheelbase and the road-wheel angle +# that `steer=1.0` commands. +WHEELBASE = 4.5 +MAX_STEER = math.radians(35.0) + +# Below walking pace Traffic Manager's correction is the same slow pull-away +# every truck makes from its spawn point, so a truck this slow is handed over +# wherever it stands rather than driven around looking for a lane. +WALKING_PACE = 1.0 + +# How many driving lanes to consider either side of the nearest one. +LANE_SEARCH = 6 + + +def _dot(a, b) -> float: + return a.x * b.x + a.y * b.y + + +def _cross(a, b) -> float: + return a.x * b.y - a.y * b.x + + +def _length(a) -> float: + return math.hypot(a.x, a.y) + + +def aligned_lane(road_map: carla.Map, location: carla.Location, forward: carla.Vector3D) -> carla.Waypoint | None: + """The driving lane under `location` that runs closest to `forward`. + + `get_waypoint` returns the nearest lane by position, which for a truck near + the centre line is the oncoming one as often as its own. Walk the road's + driving lanes both ways from it and keep the best aligned. + """ + nearest = road_map.get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Driving) + if nearest is None: + return None + lanes = [nearest] + for sideways in ("get_left_lane", "get_right_lane"): + lane = getattr(nearest, sideways)() + while lane is not None and lane.lane_type == carla.LaneType.Driving and len(lanes) < LANE_SEARCH: + lanes.append(lane) + lane = getattr(lane, sideways)() + return max(lanes, key=lambda lane: _dot(lane.transform.get_forward_vector(), forward)) + + +def pursuit_steer(location: carla.Location, forward: carla.Vector3D, target: carla.Location) -> float: + """Pure-pursuit steering toward `target`, in CARLA's [-1, 1].""" + to_target = carla.Vector3D(x=target.x - location.x, y=target.y - location.y, z=0.0) + distance = math.hypot(to_target.x, to_target.y) + if distance == 0.0: + return 0.0 + alpha = math.atan2(_cross(forward, to_target), _dot(forward, to_target)) + delta = math.atan2(2.0 * WHEELBASE * math.sin(alpha), distance) + return max(-1.0, min(1.0, delta / MAX_STEER)) + + +def plan( + road_map: carla.Map, + transform: carla.Transform, + velocity: carla.Vector3D, + measured_steer: float, + *, + reversing: bool, +) -> carla.VehicleControl | None: + """The released truck's control for this tick, or None once it can be handed over.""" + speed = math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + if speed <= WALKING_PACE: + return None + location = transform.location + forward = transform.get_forward_vector() + if reversing or _dot(velocity, forward) < 0.0: + # Nothing here merges backwards. Stop, then hand over from rest. + return carla.VehicleControl(brake=1.0, steer=measured_steer, reverse=reversing) + + lane = aligned_lane(road_map, location, forward) + if lane is None or lane.is_junction: + # Lanes cross inside a junction, so the nearest one means as little to + # this as it does to Traffic Manager. Coast through on the driver's + # steering and merge on the far side; the same below, when the junction + # is too close ahead to merge before it. + return carla.VehicleControl(throttle=0.0, brake=0.0, steer=measured_steer) + + lane_forward = lane.transform.get_forward_vector() + offset = carla.Vector3D(x=location.x - lane.transform.location.x, y=location.y - lane.transform.location.y, z=0.0) + lateral = _cross(lane_forward, offset) + # Forward vectors are unit length in 3D; on a grade their 2D shadows are not. + cosine = _dot(forward, lane_forward) / (_length(forward) * _length(lane_forward)) + heading = math.acos(max(-1.0, min(1.0, cosine))) + if abs(lateral) <= LATERAL_TOLERANCE and heading <= HEADING_TOLERANCE: + return None + + lookahead = max(LOOKAHEAD_MIN, min(LOOKAHEAD_MAX, speed * LOOKAHEAD_SECONDS)) + target = lane + walked = 0.0 + while walked < lookahead: + ahead = target.next(LOOKAHEAD_STEP) + if len(ahead) != 1 or ahead[0].is_junction: + break + target = ahead[0] + walked += LOOKAHEAD_STEP + if walked < LOOKAHEAD_MIN: + return carla.VehicleControl(throttle=0.0, brake=0.0, steer=measured_steer) + return carla.VehicleControl( + throttle=0.0, brake=0.0, steer=pursuit_steer(location, forward, target.transform.location) + ) diff --git a/demo/carla/sim/moq_carla_sim/main.py b/demo/carla/sim/moq_carla_sim/main.py new file mode 100644 index 00000000000..ac9642ea115 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/main.py @@ -0,0 +1,306 @@ +"""CLI entrypoint: one process, one CARLA world, every truck in it. + +Deliberately one process for every truck rather than one per truck. CARLA +advises a single command-sending client, one owner keeps Traffic Manager state +coherent, and the trucks have to share a world to see each other at all. +""" + +from __future__ import annotations + +import argparse +import asyncio +import contextlib +import fcntl +import json +import logging +import os +import signal +import sys +import tempfile +from pathlib import Path +from typing import IO + +import moq + +from .bridge import Bridge, Rendition +from .runtime import TruckSession +from .watchdog import Watchdog +from .world import CameraConfig, Clock, World, bridge_stream + +log = logging.getLogger("moq_carla_sim") + +# The trucks this worker runs without `--truck`, which is how +# carla-worker.service runs it: the one list of trucks the demo is expected to +# show. +TRUCKS_FILE = Path(__file__).resolve().parent.parent / "trucks.json" + +# A local development relay, which needs no token. +DEFAULT_URL = "http://localhost:4443" + + +def default_trucks() -> list[str]: + trucks = json.loads(TRUCKS_FILE.read_text()) + if not isinstance(trucks, list) or not trucks or not all(isinstance(truck, str) and truck for truck in trucks): + raise ValueError(f"{TRUCKS_FILE} must be a non-empty list of truck names") + return trucks + + +def parse_args(argv: list[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser(prog="moq-carla-sim", description=__doc__) + parser.add_argument( + "--url", + default=os.environ.get("MOQ_URL", DEFAULT_URL), + # A hosted URL carries its JWT in the query, so prefer MOQ_URL over the + # flag: a command line is visible to every user on the host. + help=f"relay URL, with ?jwt=... when hosted (default: MOQ_URL, else {DEFAULT_URL})", + ) + parser.add_argument( + "--truck", + action="append", + dest="trucks", + default=None, + help="truck name; repeat for more than one (default: the list in trucks.json)", + ) + parser.add_argument("--camera", default="front", help="camera name in the broadcast path") + parser.add_argument("--location", default=None, help="human-readable label echoed in telemetry") + parser.add_argument("--carla-host", default="127.0.0.1") + parser.add_argument("--carla-port", type=int, default=2000) + parser.add_argument("--carla-timeout", type=float, default=30.0) + parser.add_argument("--traffic-manager-port", type=int, default=8000) + parser.add_argument("--width", type=int, default=1280) + parser.add_argument("--height", type=int, default=720) + parser.add_argument("--fps", type=int, default=30) + parser.add_argument( + "--bridge-socket", + default=os.environ.get("CARLA_BRIDGE_SOCKET") or default_bridge_socket(), + help="the simulator's GPU video bridge (default: CARLA_BRIDGE_SOCKET, or carla-bridge/bridge.sock " + "under XDG_RUNTIME_DIR, where server.sh puts it)", + ) + parser.add_argument("--log-level", default="info") + args = parser.parse_args(argv) + if not args.trucks: + args.trucks = default_trucks() + return args + + +def default_bridge_socket() -> str: + runtime_dir = os.environ.get("XDG_RUNTIME_DIR") or tempfile.gettempdir() + return str(Path(runtime_dir) / "carla-bridge" / "bridge.sock") + + +def renditions(camera: CameraConfig) -> list[Rendition]: + return [ + Rendition(rendition.name, rendition.width, rendition.height, rendition.bitrate, rendition.fps) + for rendition in camera.renditions() + ] + + +async def connect_bridge(path: str) -> Bridge: + """Connect to the bridge the simulator started when the cameras spawned.""" + try: + return await Bridge.connect(path) + except (FileNotFoundError, ConnectionRefusedError) as err: + raise RuntimeError( + f"no GPU video bridge at {path}: the cameras spawned, so is the simulator running with " + "-moq-bridge-socket at that path (`just carla server`)?" + ) from err + + +def broadcast_path(truck: str, camera: str) -> str: + return f"carla/{truck}/{camera}.hang" + + +def driver_prefix(truck: str) -> str: + """Where a truck's drivers publish, each at `{prefix}/{driverId}`.""" + return f"viewer/carla/{truck}" + + +# The one task allowed to end the run without failing it. +SHUTDOWN = "shutdown" + + +def claim_simulator(host: str, port: int) -> IO[str]: + """Hold a lock naming this simulator for as long as the returned file is open. + + One worker owns the world: `sweep` removes every vehicle in it, so a second + worker on the same simulator would destroy the first one's trucks, and the + first would then fail and restart into the same sweep. A manual `just carla + worker` beside the systemd unit is the usual way to get there. The lock is + per host so it covers only that case; it says nothing about a worker on + another machine, and neither does anything else here. + """ + runtime_dir = os.environ.get("XDG_RUNTIME_DIR") or tempfile.gettempdir() + path = Path(runtime_dir) / f"moq-carla-{host}-{port}.lock" + lock = path.open("a+") + try: + fcntl.flock(lock, fcntl.LOCK_EX | fcntl.LOCK_NB) + except BlockingIOError: + lock.close() + raise RuntimeError( + f"another worker already owns the simulator at {host}:{port} (lock {path}); " + "stop it first, it would otherwise lose its trucks to this one's sweep" + ) from None + return lock + + +def terminate(_signum: int, _frame: object) -> None: + """Turn SIGTERM into the KeyboardInterrupt SIGINT already is. + + Until `supervise` installs the loop's own handlers, the run is in blocking + CARLA calls: connecting, sweeping, spawning. Python's default SIGTERM + action ends the process there with no `finally`, leaving the freshly + spawned trucks in the world for the next run to sweep. Raising instead + unwinds through `run`'s cleanup, the same way Ctrl-C does. + """ + raise KeyboardInterrupt + + +async def session_closed(client: moq.Client) -> None: + """Raise once the relay session is over. + + `Session.closed()` resolves when the connection stops for good, by raising + its error or by returning after a local shutdown, which nothing here + performs while the trucks are running. Either way the broadcasts are gone + from the relay and the run has to end. + """ + session = client.session + if session is None: + raise RuntimeError("relay session is not connected") + await session.closed() + raise RuntimeError("relay session closed") + + +async def supervise(client: moq.Client, tasks: list[asyncio.Task[None]], clock: Clock, watchdog: Watchdog) -> None: + """Run until a truck stops, the relay session or the clock dies, or a signal arrives. + + The session is watched alongside the trucks because its death is invisible + from inside them: their tracks keep accepting frames, so the process goes on + looking healthy while the relay announces nothing, and systemd has no + failure to restart. Only raising gets us out of that state. The clock is + the same shape of failure: a world nobody ticks is frozen, and every truck + keeps looking healthy while publishing nothing. + + All of that assumes this loop still turns. The watchdog's pulse is what + proves it does; see `watchdog` for the hang it exists to catch. + """ + stop = asyncio.Event() + loop = asyncio.get_running_loop() + for sig in (signal.SIGINT, signal.SIGTERM): + loop.add_signal_handler(sig, stop.set) + + watchers = [ + asyncio.create_task(stop.wait(), name=SHUTDOWN), + asyncio.create_task(session_closed(client), name="session"), + asyncio.create_task(clock.wait_failed(), name="clock"), + asyncio.create_task(watchdog.pulse(), name="pulse"), + ] + + done, pending = await asyncio.wait([*tasks, *watchers], return_when=asyncio.FIRST_COMPLETED) + # Awaited, not just cancelled: each session releases its driver watcher on + # the way out, and an un-awaited cancel would leave that to loop teardown, + # after the world in `run` is already gone. + for task in pending: + task.cancel() + await asyncio.gather(*pending, return_exceptions=True) + # Surface whatever ended the process rather than exiting cleanly on a truck + # or a session that died: neither must read as a normal stop. + for task in done: + if task.get_name() != SHUTDOWN and not task.cancelled(): + task.result() + + +async def run(args: argparse.Namespace) -> None: + camera = CameraConfig(width=args.width, height=args.height, fps=args.fps) + + lock = claim_simulator(args.carla_host, args.carla_port) + world = World.connect(args.carla_host, args.carla_port, args.carla_timeout, args.traffic_manager_port, camera.fps) + sessions: list[TruckSession] = [] + # The RPC timeout bounds every blocking CARLA call, so a thread that has + # not moved in twice that long is hung, not slow. The clock idles a frame + # period between ticks on purpose, so that is not counted against it. + watchdog = Watchdog(2 * args.carla_timeout + 1 / camera.fps, world.clock.heartbeat) + # Spawning is inside the try: a spawn that fails partway has already put + # actors in the world, and the server outlives us, so they must still be + # removed or the next run spawns beside them. The clock starts first + # because a synchronous sweep and spawn only land on a tick. + try: + world.clock.start() + watchdog.start() + world.sweep() + world.spawn(args.trucks, camera, args.camera) + map_name = world.map_name() + # Every truck is in the world, so the clock no longer needs to run for + # its own sake: from here it follows viewer and driver demand. + world.ready() + bridge = await connect_bridge(args.bridge_socket) + # The only reader of the bridge's events, so it runs before any stream + # opens. It ending, for any reason, ends the run: every truck's video + # shares the connection. + events = asyncio.create_task(bridge.run(), name="bridge") + try: + async with moq.connect(args.url) as client: + try: + # One epoch for the run: a restarted worker announces every + # truck under a new one, which viewers see as a restart. + route = moq.Route(epoch=moq.mint_epoch()) + for truck in world.trucks: + broadcast = client.create_broadcast(broadcast_path(truck.name, args.camera)) + video = await bridge.open(bridge_stream(truck.name, args.camera), renditions(camera), broadcast) + session = TruckSession(truck, broadcast, video, args.location, map_name, world.clock, route) + sessions.append(session) + log.info( + "publishing %s, consuming %s/*", + broadcast_path(truck.name, args.camera), + driver_prefix(truck.name), + ) + + tasks = [ + asyncio.create_task( + session.run(client, driver_prefix(truck.name)), + name=f"truck:{truck.name}", + ) + for session, truck in zip(sessions, world.trucks) + ] + + await supervise(client, [*tasks, events], world.clock, watchdog) + finally: + # Inside the session, so the tracks end cleanly for + # subscribers rather than with the connection. + for session in sessions: + session.stop_video() + # Also inside the session, ahead of its close: the actors are + # the one thing that outlives this process, so they go before + # anything that could hang on a relay that has already gone, + # and before systemd's stop timeout turns that hang into a kill. + world.destroy() + finally: + events.cancel() + await asyncio.gather(events, return_exceptions=True) + bridge.disconnect() + finally: + # A no-op on the way out of a completed run; this is for a sweep, a + # spawn, or a connect that failed before the session was ever entered. + world.destroy() + # Armed through both destroys: a teardown that hangs is the same hang, + # and after a failure nothing but the watchdog would end it. + watchdog.stop() + lock.close() + + +def main(argv: list[str] | None = None) -> int: + args = parse_args(argv) + logging.basicConfig( + level=args.log_level.upper(), + format="%(asctime)s %(levelname)-5s %(name)s: %(message)s", + ) + # The bindings log from Rust, which Python logging does not see; keep both + # layers at the same level. + moq.log_level(args.log_level.lower()) + signal.signal(signal.SIGTERM, terminate) + with contextlib.suppress(asyncio.CancelledError, KeyboardInterrupt): + asyncio.run(run(args)) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/demo/carla/sim/moq_carla_sim/park.py b/demo/carla/sim/moq_carla_sim/park.py new file mode 100644 index 00000000000..f17ac321879 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/park.py @@ -0,0 +1,74 @@ +"""Park the renderer of a simulator no worker owns. + +The server outlives every client and boots with rendering on, so a simulator +sitting between workers draws frames nobody asked for. Measured on an +RTX 3070 Ti against an empty world with nothing connected: 174 W and 100% GPU +before, 26 W and 44% after, with process CPU down about half a core. The worker +asserts the same state on connect (see `World.__init__`); this is that +assertion for the gap before one arrives. + +Only `no_rendering_mode`. Synchronous mode is deliberately untouched: a +synchronous world nobody ticks is frozen for whatever connects next, and +`just carla benchmark` refuses a world that is already synchronous. +""" + +from __future__ import annotations + +import argparse +import logging +import sys + +from .main import claim_simulator +from .world import connect_client + +log = logging.getLogger(__name__) + + +def parse_args(argv: list[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser(prog="carla-park", description=__doc__) + parser.add_argument("--carla-host", default="127.0.0.1") + parser.add_argument("--carla-port", type=int, default=2000) + parser.add_argument("--carla-timeout", type=float, default=30.0) + parser.add_argument("--log-level", default="info") + return parser.parse_args(argv) + + +def park(host: str, port: int, timeout: float) -> bool: + """Disable rendering on an unowned simulator. True when it parked one. + + The worker's own lock decides ownership: a worker holding it is already + driving demand, and parking under it would black out a live viewer between + its `select_camera` and the sensor's next frame. + """ + try: + lock = claim_simulator(host, port) + except RuntimeError: + log.info("a worker owns %s:%d; leaving its rendering alone", host, port) + return False + + try: + world = connect_client(host, port, timeout).get_world() + settings = world.get_settings() + if settings.no_rendering_mode: + log.info("rendering already parked") + return True + settings.no_rendering_mode = True + world.apply_settings(settings) + log.info("rendering parked until a worker or a viewer needs it") + return True + finally: + lock.close() + + +def main(argv: list[str] | None = None) -> int: + args = parse_args(argv) + logging.basicConfig( + level=args.log_level.upper(), + format="%(asctime)s %(levelname)-5s %(name)s: %(message)s", + ) + park(args.carla_host, args.carla_port, args.carla_timeout) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/demo/carla/sim/moq_carla_sim/publish.py b/demo/carla/sim/moq_carla_sim/publish.py new file mode 100644 index 00000000000..b5607b62fb7 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/publish.py @@ -0,0 +1,385 @@ +"""MoQ wiring: video and status out, driver control in. + +The broadcast layout is the one `demo/carla/README.md` documents and the web +player speaks: + + carla/{truck}/{camera}.hang video (sd, hd) + status + viewer/carla/{truck}/{driverId} control +""" + +from __future__ import annotations + +import asyncio +import logging +import math +from dataclasses import dataclass +from typing import Any + +import moq + +from .drive import Command, DriveState, Gear, TimestampEntry + +log = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class Pose: + """Where a truck is, in CARLA world meters. + + Deliberately the simulator's own top-down axes (x east, y south, yaw + clockwise from +x) rather than latitude and longitude: the shipped towns + carry a null OpenDRIVE georeference, so `transform_to_geolocation` returns + real WGS84 numbers for a meaningless place in the Gulf of Guinea. The road + map baked into the web player is in these same meters, so the two line up. + """ + + x: float + y: float + yaw: float + speed: float + + +@dataclass(frozen=True) +class Motion: + """How the truck actually responded, in its own frame. + + Acceleration in m/s^2 (longitudinal positive forward, lateral positive + right) and yaw rate in deg/s. Lateral acceleration is the number that rolls + a loaded truck over, and none of these can be derived from the command: + they are what the physics did with it. + """ + + yaw_rate: float + accel_lon: float + accel_lat: float + + +@dataclass(frozen=True) +class Collision: + """The most recent contact the vehicle's collision sensor reported. + + `at` is unix milliseconds at the instant contact began, and is what gives a + collision an identity on a snapshot track: without one, the retained + snapshot a late subscriber receives is indistinguishable from a fresh + impact, and a restarted producer reuses its predecessor's numbering. + + An absolute instant rather than an age, and the distinction is the whole + design. An age would redirty the snapshot on every publish for a parked + truck; this stays fixed until the next collision. It also means a viewer + who opened the page long afterwards ages it correctly, instead of timing it + from when that particular browser happened to first see it. + """ + + at: float + other: str + impulse: float + + +@dataclass(frozen=True) +class Telemetry: + """One consistent read of the vehicle: where it is, and how it responded. + + Grouped because they come from a single read of the actor and always travel + together. The wire keeps them as separate top-level `status` fields, which + is a contract; this struct is only the plumbing behind it. + """ + + pose: Pose + measured: DriveState + motion: Motion + collision: Collision | None + + +@dataclass(frozen=True) +class Status: + accepted: DriveState + estopped: bool + estop_by: str | None + autonomous: bool + driver: str | None + sequence: int | None + media_pipeline: dict[str, list[dict[str, Any]]] + location: str | None + map_name: str | None + telemetry: Telemetry | None + + +def status_payload(status: Status) -> dict[str, Any]: + """Build the `status` track payload. + + `mode` is `simulated` because accepted commands actuate a CARLA vehicle; + the player treats it as an open wire enum, so another value degrades to + `unknown` instead of blanking the tile. Who is steering is a separate + question and rides on `autonomy`. + + `accepted` is the command the gate let through; `measured`, `motion`, and + `collision` are what the vehicle did with it, and are present whatever is + driving. + """ + payload: dict[str, Any] = { + "accepted": { + "steering": status.accepted.steering, + "throttle": status.accepted.throttle, + "brake": status.accepted.brake, + "gear": status.accepted.gear.value, + }, + "mode": "simulated", + "capabilities": ["respawn"], + "autonomy": "autonomous" if status.autonomous else "manual", + "estopped": status.estopped, + "media_pipeline": status.media_pipeline, + } + # Omitted rather than null when absent; the player reads a missing field + # as unset. + if status.driver is not None: + payload["driver"] = status.driver + if status.estop_by is not None: + payload["estop_by"] = status.estop_by + if status.sequence is not None: + payload["sequence"] = status.sequence + if status.location is not None: + payload["location"] = status.location + if status.map_name is not None: + payload["map"] = status.map_name + if status.telemetry is not None: + # Everything below is rounded for the same reason: unrounded float noise + # from the physics engine would republish a "changed" snapshot for a + # parked truck forever. Centimetres and tenths of a degree for the pose, + # a tenth of a percent for a normalized control axis. + pose = status.telemetry.pose + payload["pose"] = { + "x": round(pose.x, 2), + "y": round(pose.y, 2), + "yaw": round(pose.yaw, 1), + "speed": round(pose.speed, 2), + } + measured = status.telemetry.measured + payload["measured"] = { + "steering": round(measured.steering, 3), + "throttle": round(measured.throttle, 3), + "brake": round(measured.brake, 3), + "gear": measured.gear.value, + } + motion = status.telemetry.motion + payload["motion"] = { + "yaw_rate": round(motion.yaw_rate, 1), + "accel_lon": round(motion.accel_lon, 2), + "accel_lat": round(motion.accel_lat, 2), + } + if status.telemetry.collision is not None: + collision = status.telemetry.collision + payload["collision"] = { + "at": round(collision.at), + "other": collision.other, + "impulse": round(collision.impulse, 1), + } + return payload + + +class StatusPublisher: + """The `status` track: DEFLATE-compressed JSON snapshots. + + Snapshot rather than stream, because a late joiner wants the truck's current + state, not the history of every command it ever accepted. + """ + + def __init__(self, broadcast: moq.BroadcastProducer) -> None: + self._producer = moq.json.SnapshotProducer(broadcast, broadcast.publish_track("status"), compression=True) + # Weak: watching demand neither keeps the track open nor locks it. + self._demand = self._producer.demand() + + def publish(self, payload: dict[str, Any]) -> None: + """Publish the latest status, failing the session if the track is gone. + + Deliberately not swallowed. `update` fails only on a producer that has + been closed, which never reopens, and status republishes on a 100 ms + timer, so catching here would retry a dead track ten times a second per + truck, forever, logging as it went. Status is the control echo, so a + truck that cannot answer is not one anybody should be steering: let it + reach the session, exactly as the video pump does with its own track. + """ + self._producer.update(payload) + + async def used(self) -> None: + while await self._demand.wanted_changed() is None: + pass + + async def unused(self) -> None: + while await self._demand.wanted_changed() is not None: + pass + + +# The player's pipeline stages. Anything else a driver sends is dropped, so an +# anonymous lease holder cannot grow the status the truck echoes. +TIMESTAMP_LABELS = ("received", "decoded", "rendered") + + +def _parse_timestamps(raw: Any) -> tuple[TimestampEntry, ...]: + if not isinstance(raw, list): + return () + entries: dict[str, TimestampEntry] = {} + for item in raw: + if not isinstance(item, dict): + continue + label = item.get("label") + if label not in TIMESTAMP_LABELS or label in entries: + continue + timestamp = _number_or_none(item.get("ts")) + if timestamp is not None: + entries[label] = TimestampEntry(label=label, ts=timestamp) + return tuple(entries.values()) + + +def parse_command(driver_id: str, message: Any, epoch: str | None = None) -> Command | None: + """Turn one `control` track message from a driver run into a Command. + + Returns None for anything unrecognized. A driver is an untrusted publisher, + so a malformed message is dropped here rather than reaching the gate: the + axes are clamped downstream, but a wrong *shape* must not raise into the + control loop and take the truck's whole session with it. + """ + if not isinstance(message, dict): + return None + + kind = message.get("type") + sequence = int(_number(message.get("sequence"))) + timestamps = _parse_timestamps(message.get("timestamps")) + + if kind == "drive": + gear_raw = message.get("gear", Gear.NEUTRAL.value) + try: + gear = Gear(gear_raw) + except ValueError: + # An unknown gear is not a reason to drop the whole command; neutral + # is the safe reading of "we do not know what they asked for". + log.warning("%s: unknown gear %r, treating as neutral", driver_id, gear_raw) + gear = Gear.NEUTRAL + return Command( + kind="drive", + driver_id=driver_id, + epoch=epoch, + sequence=sequence, + state=DriveState( + steering=_number(message.get("steering")), + throttle=_number(message.get("throttle")), + brake=_number(message.get("brake")), + gear=gear, + ), + timestamps=timestamps, + ) + + if kind in ("estop", "reset_estop", "respawn"): + return Command(kind=kind, driver_id=driver_id, epoch=epoch, sequence=sequence, timestamps=timestamps) + + return None + + +def _number(value: Any) -> float: + """A JSON number, or 0.0 for anything else. + + `bool` is excluded explicitly because it is an `int` subclass in Python, so + a driver sending `true` would otherwise read as 1. + """ + return _number_or_none(value) or 0.0 + + +def _number_or_none(value: Any) -> float | None: + if not isinstance(value, (int, float)) or isinstance(value, bool): + return None + number = float(value) + return number if math.isfinite(number) else None + + +async def watch_drivers( + client: moq.Client, + prefix: str, + commands: asyncio.Queue[Command], +) -> None: + """Discover driver broadcasts at `{prefix}/{driverId}` and fan their commands in. + + Runs until cancelled. Each run of a driver, its path plus the route epoch it + announced under, gets its own task, so one slow or stuck driver cannot hold + up the others. A broadcast that ends, or restarts under another epoch, ends + its task, and with it the lease of that run; a restart starts a new task. + """ + drivers: dict[str, tuple[str | None, asyncio.Task[None]]] = {} + + def forget(driver_id: str, task: asyncio.Task[None]) -> None: + # Only this run's entry: a restart has already put its successor there. + current = drivers.get(driver_id) + if current is not None and current[1] is task: + del drivers[driver_id] + + try: + async with client.announced(prefix, filter="*") as announced: + async for event in announced: + # Every variant carries `announce`; uniffi's stubs type only the enum. + announce = event.announce # pyright: ignore[reportAttributeAccessIssue] + path = announce.prefix + driver_id = path.removeprefix(prefix + "/") + # The filter admits one segment below the prefix; anything else + # is a route that only overlaps it, not a driver's broadcast. + if driver_id == path or not driver_id or "/" in driver_id: + continue + epoch = announce.route.epoch + current = drivers.get(driver_id) + if isinstance(event, moq.AnnounceEventEnd): + if current is not None: + del drivers[driver_id] + current[1].cancel() + continue + if current is not None: + # Same run: the route only changed hops or cost. + if current[0] == epoch and not isinstance(event, moq.AnnounceEventRestart): + continue + current[1].cancel() + task = asyncio.create_task( + _pump_driver(driver_id, epoch, client, path, commands), + name=f"driver:{driver_id}", + ) + drivers[driver_id] = (epoch, task) + task.add_done_callback(lambda task, driver_id=driver_id: forget(driver_id, task)) + finally: + for _, task in drivers.values(): + task.cancel() + + +async def _pump_driver( + driver_id: str, + epoch: str | None, + client: moq.Client, + path: str, + commands: asyncio.Queue[Command], +) -> None: + """Feed one driver run's `control` track into the command queue. + + Always emits `driver_left` on the way out, however it leaves, so the gate + releases a lease even when the track ends abnormally. + """ + log.info("driver %s connected (epoch %s)", driver_id, epoch) + try: + broadcast = await client.request_broadcast(path) + control = moq.json.SnapshotConsumer(await broadcast.subscribe_track("control"), compression=True) + async for message in control: + command = parse_command(driver_id, message, epoch) + if command is not None: + await commands.put(command) + # uniffi's stubs declare the error a plain class; at runtime it is an Exception. + except moq.Error as err: # pyright: ignore[reportGeneralTypeIssues] + log.info("driver %s track ended: %s", driver_id, err) + finally: + log.info("driver %s disconnected (epoch %s)", driver_id, epoch) + await commands.put(Command(kind="driver_left", driver_id=driver_id, epoch=epoch)) + + +__all__ = [ + "StatusPublisher", + "Collision", + "Motion", + "Pose", + "Status", + "Telemetry", + "parse_command", + "status_payload", + "watch_drivers", +] diff --git a/demo/carla/sim/moq_carla_sim/roadmap.py b/demo/carla/sim/moq_carla_sim/roadmap.py new file mode 100644 index 00000000000..73f536be116 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/roadmap.py @@ -0,0 +1,177 @@ +"""Bake the running CARLA town's road network into the web player's map. + +Run once per town, against a running simulator, and commit the result: + + just carla map + +The output is a GeoJSON FeatureCollection of lane centrelines, but its +coordinates are CARLA world METERS (x east, y south), not WGS84 degrees. That is +deliberate: the shipped towns carry a null OpenDRIVE georeference, so +`transform_to_geolocation` would hand back real coordinates for a meaningless +place, and the trucks' `pose` telemetry is in these same meters. The player +projects both with one transform. Reprojecting onto a real basemap is a decision +about where the demo pretends to be, and belongs with whoever makes it. + +Lane centrelines rather than road edges because that is what `get_topology` +gives directly, and at town scale a bundle of centrelines reads as a road. +""" + +from __future__ import annotations + +import argparse +import json +import logging +import math +import sys +from pathlib import Path +from typing import Any + +log = logging.getLogger(__name__) + +# Metres between sampled waypoints along a lane, and the corridor within which +# the simplifier may then drop them again. 2 m tracks a tight junction curve; +# 0.5 m of error is invisible on a town-sized map but collapses straight roads +# from hundreds of points to two. +STEP = 2.0 +TOLERANCE = 0.5 + +# Waypoints to walk before abandoning a lane. `next` follows a ring road back +# into the same lane forever, so the walk needs a bound rather than a rule. +MAX_LANE_POINTS = 4000 + +Point = tuple[float, float] + + +def _segment_distance(point: Point, start: Point, end: Point) -> float: + """Perpendicular distance from `point` to the segment `start`-`end`.""" + dx, dy = end[0] - start[0], end[1] - start[1] + span = math.hypot(dx, dy) + if span == 0.0: + return math.hypot(point[0] - start[0], point[1] - start[1]) + return abs(dy * point[0] - dx * point[1] + end[0] * start[1] - end[1] * start[0]) / span + + +def simplify(points: list[Point], tolerance: float) -> list[Point]: + """Ramer-Douglas-Peucker: drop points no further than `tolerance` off the line. + + Iterative rather than recursive so a long straight lane cannot bottom out + the interpreter on its own geometry. + """ + if len(points) < 3: + return list(points) + + keep = [False] * len(points) + keep[0] = keep[-1] = True + stack = [(0, len(points) - 1)] + while stack: + first, last = stack.pop() + if last <= first + 1: + continue + index, worst = max( + ((i, _segment_distance(points[i], points[first], points[last])) for i in range(first + 1, last)), + key=lambda pair: pair[1], + ) + if worst > tolerance: + keep[index] = True + stack.append((first, index)) + stack.append((index, last)) + return [point for point, keep_it in zip(points, keep) if keep_it] + + +def lane_points(start: Any, step: float, limit: int = MAX_LANE_POINTS) -> list[Point]: + """Walk one lane from `start` until it ends or turns into a different lane.""" + lane = (start.road_id, start.section_id, start.lane_id) + waypoint = start + points = [(waypoint.transform.location.x, waypoint.transform.location.y)] + for _ in range(limit): + following = waypoint.next(step) + if not following: + break + waypoint = following[0] + if (waypoint.road_id, waypoint.section_id, waypoint.lane_id) != lane: + break + points.append((waypoint.transform.location.x, waypoint.transform.location.y)) + return points + + +def road_lines(road_map: Any, step: float = STEP, tolerance: float = TOLERANCE) -> list[list[Point]]: + """Every drivable lane in the map, as a simplified polyline. + + `get_topology` yields one entry per lane, so walking from each start covers + the network exactly once. + """ + lines = [] + for start, _end in road_map.get_topology(): + points = lane_points(start, step) + if len(points) < 2: + continue + lines.append(simplify(points, tolerance)) + return lines + + +def feature_collection(name: str, lines: list[list[Point]]) -> dict[str, Any]: + """Wrap lane polylines as GeoJSON, in CARLA meters (see the module docstring).""" + if not lines: + raise RuntimeError(f"{name} produced no drivable lanes") + xs = [x for line in lines for x, _ in line] + ys = [y for line in lines for _, y in line] + return { + "type": "FeatureCollection", + "name": name, + # Decimetres: finer than the map is ever drawn, and it halves the file. + "bbox": [round(min(xs), 1), round(min(ys), 1), round(max(xs), 1), round(max(ys), 1)], + "features": [ + { + "type": "Feature", + "properties": {"kind": "lane"}, + "geometry": { + "type": "MultiLineString", + "coordinates": [[[round(x, 1), round(y, 1)] for x, y in line] for line in lines], + }, + } + ], + } + + +def parse_args(argv: list[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser(prog="carla-roadmap", description=__doc__) + parser.add_argument("--carla-host", default="127.0.0.1") + parser.add_argument("--carla-port", type=int, default=2000) + parser.add_argument("--carla-timeout", type=float, default=30.0) + parser.add_argument("--step", type=float, default=STEP, help="metres between sampled waypoints") + parser.add_argument("--tolerance", type=float, default=TOLERANCE, help="metres of simplification error") + parser.add_argument( + "--out", + type=Path, + default=Path(__file__).resolve().parents[2] / "web/src/maps", + help="directory to write .json into", + ) + parser.add_argument("--log-level", default="info") + return parser.parse_args(argv) + + +def main(argv: list[str] | None = None) -> int: + args = parse_args(argv) + logging.basicConfig(level=args.log_level.upper(), format="%(levelname)-5s %(name)s: %(message)s") + + # Imported here rather than at module scope so the pure geometry above stays + # testable without the CARLA wheel. + from .world import connect_client + + client = connect_client(args.carla_host, args.carla_port, args.carla_timeout) + road_map = client.get_world().get_map() + name = road_map.name.rsplit("/", 1)[-1] + + lines = road_lines(road_map, args.step, args.tolerance) + collection = feature_collection(name, lines) + + args.out.mkdir(parents=True, exist_ok=True) + path = args.out / f"{name}.json" + path.write_text(json.dumps(collection, separators=(",", ":")) + "\n") + points = sum(len(line) for line in lines) + log.info("wrote %s: %d lanes, %d points, %d KiB", path, len(lines), points, path.stat().st_size // 1024) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/demo/carla/sim/moq_carla_sim/runtime.py b/demo/carla/sim/moq_carla_sim/runtime.py new file mode 100644 index 00000000000..eb380f17075 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/runtime.py @@ -0,0 +1,355 @@ +"""The per-truck control loop. + +One `TruckSession` per truck: it owns the safety gate, the MoQ broadcast, and +the truck's bridge stream, whose compressed video the bridge publishes into +that broadcast. +""" + +from __future__ import annotations + +import asyncio +import logging +import math +from typing import TYPE_CHECKING, Any + +import moq + +from .drive import Accepted, Command, Drive, TimestampEntry +from .publish import Pose, Status, StatusPublisher, Telemetry, status_payload, watch_drivers +from .world import drive_hold, status_hold + +if TYPE_CHECKING: + from .bridge import BridgeStream + from .world import Clock, Truck + +log = logging.getLogger(__name__) + +# How long the truck waits for the lease holder before releasing the lease. +# The web player heartbeats at 250 ms. +COMMAND_TIMEOUT = 1.0 + +# How often a session reports its published frame rate. +REPORT_INTERVAL = 10.0 + +# How often status republishes the vehicle's own telemetry, so map markers keep +# moving and measured feedback keeps arriving while nobody is driving. +TELEMETRY_INTERVAL = 0.1 + +# Pending driver commands held before publishers are made to wait. +COMMAND_QUEUE_DEPTH = 64 + +# An autonomous truck that has not moved this far in this long is wedged, not +# waiting: Traffic Manager would have deleted it, which is what the hero role +# forbids, so this is where a stuck truck gets unstuck. Town10's lights cycle in +# well under a minute, and the trucks queued behind a wedged one respawn with it +# rather than sit there once it is gone. Displacement rather than speed, because +# a truck grinding against a barrier reports velocity while going nowhere. +STUCK_TIMEOUT = 300.0 +STUCK_DISTANCE = 2.0 + + +class StuckDetector: + """Notices a truck that has stopped going anywhere. + + `update` anchors on the first pose it sees and reports stuck once the truck + has stayed within `distance` of that anchor for `timeout`. Moving that far + re-anchors and restarts the clock. + """ + + def __init__(self, timeout: float = STUCK_TIMEOUT, distance: float = STUCK_DISTANCE) -> None: + self._timeout = timeout + self._distance = distance + self._anchor: Pose | None = None + self._since = 0.0 + + def reset(self) -> None: + self._anchor = None + + def update(self, pose: Pose, now: float) -> bool: + anchor = self._anchor + if anchor is None or math.hypot(pose.x - anchor.x, pose.y - anchor.y) >= self._distance: + self._anchor = pose + self._since = now + return False + return now - self._since >= self._timeout + + +# Longer than any real pipeline stage. A driver's timestamps are any finite +# floats, whose difference can be infinite, so it is clamped before `int`. +MAX_STAGE_MS = 60_000 + + +def latency_breakdown(timestamps: tuple[TimestampEntry, ...]) -> list[dict[str, Any]]: + """Per-stage deltas from the driver's media timestamps, in milliseconds.""" + return [ + {"label": f"{a.label}→{b.label}", "ms": int(min(max(b.ts - a.ts, 0), MAX_STAGE_MS))} + for a, b in zip(timestamps, timestamps[1:]) + ] + + +class TruckSession: + """One truck's gate, broadcast, and control loop.""" + + def __init__( + self, + truck: Truck, + broadcast: moq.BroadcastProducer, + video: BridgeStream, + location: str | None, + map_name: str | None, + clock: Clock, + route: moq.Route, + ) -> None: + self._truck = truck + self._video = video + self._clock = clock + self._location = location + self._map_name = map_name + self._drive = Drive() + self._status = StatusPublisher(broadcast) + # Bounded: drivers are untrusted publishers, so an unbounded queue is a + # memory-growth path under one that floods; a full queue instead blocks + # that driver's own task. + self._commands: asyncio.Queue[Command] = asyncio.Queue(maxsize=COMMAND_QUEUE_DEPTH) + self._media_pipeline: dict[str, list[dict[str, Any]]] = {} + self._deadline: float | None = None + self._stuck = StuckDetector() + # A broadcast starts unadvertised; the player discovers trucks by + # announcement, so advertise once the status and video tracks exist. + # The stream's are there already: opening it advertised them. The + # route carries the run's epoch, so viewers see a restarted worker as + # a restart rather than a stalled broadcast. + broadcast.announce(route) + + def stop_video(self) -> None: + """End the video tracks cleanly and stop the bridge encoding them.""" + self._video.finish() + + @property + def commands(self) -> asyncio.Queue[Command]: + return self._commands + + async def run(self, client: moq.Client, drivers: str) -> None: + """Consume driver commands and actuate until cancelled. + + Driver discovery runs in a task group rather than a bare task, so a + failure there fails the session. A detached task's exception is never + retrieved: the truck would keep publishing video and status while + silently accepting no driver ever again, which reads as healthy. + """ + self._publish() + async with asyncio.TaskGroup() as group: + watcher = group.create_task( + watch_drivers(client, drivers, self._commands), name=f"drivers:{self._truck.name}" + ) + # Video demand is what renders the world for this truck, and the + # bridge follows the same demand to pick its camera. + demand = group.create_task( + self._video.watch_demand(self._truck.set_video_demand), name=f"demand:{self._truck.name}" + ) + video = group.create_task(self._report_video(), name=f"video:{self._truck.name}") + telemetry = group.create_task(self._publish_telemetry(), name=f"telemetry:{self._truck.name}") + viewers = group.create_task(self._watch_status_demand(), name=f"viewers:{self._truck.name}") + try: + await self._loop() + finally: + # Before the tasks, so a truck on its way out cannot leave the + # world stepping for a driver it no longer has. + self._clock.release(drive_hold(self._truck.name)) + watcher.cancel() + video.cancel() + demand.cancel() + telemetry.cancel() + viewers.cancel() + + async def _loop(self) -> None: + while True: + command = await self._next_command() + if command.kind == "respawn": + # Respawn changes the simulated actor rather than the shared + # drive contract. Only the lease holder may move the truck. + if not self._drive.holds(command) or self._drive.estopped: + continue + self._truck.respawn() + self._drive = Drive() + self._deadline = None + self._media_pipeline.clear() + self._stuck.reset() + self._hold_clock() + self._publish() + continue + if command.kind == "drive" and self._drive.driver is None and self._truck.autonomous: + # The browser seeds its first command from the latest status, but + # the gate still holds its no-driver resting state. Synchronize the + # gear from fresher local feedback so the shift interlock does not + # turn a moving takeover into a neutral coast. + self._drive.synchronize(self._truck.telemetry().measured) + accepted = self._drive.apply(command) + changed = accepted + self._hold_clock() + + if command.kind in ("drive", "estop", "reset_estop"): + if accepted and self._drive.holds(command): + # Only the lease holder's accepted traffic refreshes the + # watchdog and its pipeline timing. A bystander's e-stop + # still latches, but must not extend the holder's lease, and + # an e-stopped holder's refused drive frames must not keep + # the lease alive forever. + self._media_pipeline[command.driver_id] = latency_breakdown(command.timestamps) + self._deadline = asyncio.get_running_loop().time() + COMMAND_TIMEOUT + elif command.kind == "driver_left" and accepted: + # Only the holder's run has a pipeline entry, so another run of + # the same driver id leaving must not drop it. + self._media_pipeline.pop(command.driver_id, None) + self._deadline = None + elif command.kind == "watchdog_expired" and accepted: + self._deadline = None + self._media_pipeline.clear() + log.warning("%s: control watchdog expired, returning to autonomy", self._truck.name) + + if not changed: + continue + + # Actuate before publishing, so the world and the status echo can + # never disagree about what was accepted. + self._actuate() + self._publish() + + async def _report_video(self) -> None: + """Log the published frame rate per rendition. + + A truck whose simulator cannot keep up still publishes, just at a lower + rate, so nothing else would say so. + """ + loop = asyncio.get_running_loop() + published = list(self._video.published) + since = loop.time() + while True: + await asyncio.sleep(REPORT_INTERVAL) + now = loop.time() + current = list(self._video.published) + rates = [(count - before) / (now - since) for count, before in zip(current, published)] + if any(rates): + log.info( + "%s: publishing %s", + self._truck.name, + ", ".join(f"{name}={rate:.1f} fps" for name, rate in zip(self._video.rendition_names, rates)), + ) + published, since = current, now + + async def _publish_telemetry(self) -> None: + """Republish on a timer so a truck nobody is driving still reports. + + The drive loop publishes only when the gate's accepted state changes, + and the vehicle's own telemetry moves independently of it: without this + an autonomous truck would pin its map marker wherever the last driver + let go of it, and report the pedals it was applying at that moment. + + Unbounded on purpose: a publish that fails raises out of this task and + into the session's task group rather than being retried, so the timer + cannot outlive the track it publishes to. + + The stuck watchdog rides on the same tick. It only ever fires for a + truck nobody holds: a stationary truck under a driver is the driver's + business, and an e-stopped one is latched on purpose. So does the + handback: a released truck steers itself onto a lane before Traffic + Manager takes it, and nothing else runs often enough to drive it. + """ + while True: + await asyncio.sleep(TELEMETRY_INTERVAL) + telemetry = self._truck.telemetry() + if self._drive.driver is None and not self._drive.estopped: + self._truck.release() + if ( + self._drive.driver is not None + or self._drive.estopped + or not self._truck.autonomous + or not self._clock.running + ): + # A parked world is not a stuck truck: nothing is meant to be + # moving, and respawning on that would fire every STUCK_TIMEOUT + # for as long as nobody watches. + self._stuck.reset() + elif self._stuck.update(telemetry.pose, asyncio.get_running_loop().time()): + log.warning("%s: no movement in %.0f s while autonomous, respawning", self._truck.name, STUCK_TIMEOUT) + self._truck.respawn() + self._stuck.reset() + self._publish(telemetry) + + async def _next_command(self) -> Command: + """The next driver command, or a synthesized watchdog expiry.""" + if self._deadline is None: + return await self._commands.get() + + timeout = self._deadline - asyncio.get_running_loop().time() + if timeout <= 0: + return Command(kind="watchdog_expired") + try: + return await asyncio.wait_for(self._commands.get(), timeout=timeout) + except asyncio.TimeoutError: + return Command(kind="watchdog_expired") + + async def _watch_status_demand(self) -> None: + """Step the world while anyone subscribes to this truck's status. + + The map draws every truck from `status`, not video, so parking on video + demand alone froze every marker under an open map. The release is in a + `finally` so a cancelled watcher cannot leave the world stepping. + """ + reason = status_hold(self._truck.name) + while True: + await self._status.used() + self._clock.hold(reason) + try: + await self._status.unused() + finally: + self._clock.release(reason) + + def _hold_clock(self) -> None: + """Step the world while this truck has a driver, whoever is watching. + + Video demand parks the clock, and the two do not have to agree: a driver + whose viewer has gone would otherwise steer a frozen world, with every + command accepted and nothing moving. An e-stop is deliberately not a + hold, even under a driver. It latches until reset, so holding for it + would keep the world stepping indefinitely for a truck that is meant to + be standing still. + """ + reason = drive_hold(self._truck.name) + if self._drive.driver is not None and not self._drive.estopped: + self._clock.hold(reason) + else: + self._clock.release(reason) + + def _actuate(self) -> None: + self._truck.actuate(self._drive.accepted(), has_driver=self._drive.driver is not None) + + def _publish(self, telemetry: Telemetry | None = None) -> None: + self._status.publish( + status_payload( + Status( + accepted=self._drive.state, + estopped=self._drive.estopped, + estop_by=self._drive.estop_by, + autonomous=self._truck.autonomous, + driver=self._drive.driver, + sequence=self._drive.sequence, + media_pipeline=self._media_pipeline, + location=self._location, + map_name=self._map_name, + telemetry=self._truck.telemetry() if telemetry is None else telemetry, + ) + ) + ) + + +__all__ = [ + "COMMAND_TIMEOUT", + "STUCK_DISTANCE", + "STUCK_TIMEOUT", + "TELEMETRY_INTERVAL", + "Accepted", + "StuckDetector", + "TruckSession", + "latency_breakdown", +] diff --git a/demo/carla/sim/moq_carla_sim/watchdog.py b/demo/carla/sim/moq_carla_sim/watchdog.py new file mode 100644 index 00000000000..e2246773154 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/watchdog.py @@ -0,0 +1,108 @@ +"""End the worker when it stops making progress without failing. + +CARLA's synchronous-mode Traffic Manager can deadlock `world.tick()` inside the +client library (carla-simulator/carla#9172), before the RPC whose timeout would +otherwise raise. The clock thread then blocks forever, the world freezes, and +the next blocking CARLA call from the event loop never returns either. Every +asyncio watcher in `supervise` is stuck behind that call, so the relay session +dies unnoticed while the video threads go on reporting 0 fps and systemd sees a +healthy process. Nothing in-process recovers from a deadlocked client library, +hence a thread of its own that trusts neither the clock nor the loop, and an +exit that skips every cleanup that could hang: systemd restarts the worker and +the next run sweeps whatever this one left in the world. +""" + +from __future__ import annotations + +import asyncio +import faulthandler +import logging +import os +import sys +import threading +import time +from typing import Callable + +log = logging.getLogger(__name__) + +# How often the event loop proves it is still turning. +PULSE_INTERVAL = 1.0 + + +class Heartbeat: + """A timestamp one thread refreshes and the watchdog reads. + + Unarmed until its first beat: a heartbeat nobody has started yet is not + stale, it is not being watched. + """ + + def __init__(self, name: str) -> None: + self.name = name + self._last: float | None = None + + def beat(self) -> None: + self._last = time.monotonic() + + def age(self) -> float: + if self._last is None: + return 0.0 + return time.monotonic() - self._last + + +def die(reason: str) -> None: + """Exit without unwinding, so nothing that hangs gets a say in the shutdown. + + The stacks are the only record of where the hang was, so they go out first. + """ + log.critical("%s; dumping every thread and exiting for systemd to restart", reason) + for handler in logging.getLogger().handlers: + handler.flush() + faulthandler.dump_traceback(all_threads=True) + sys.stderr.flush() + os._exit(1) + + +class Watchdog: + """Fail the process once any heartbeat is older than `stall` seconds. + + Watches its own event loop heartbeat alongside the ones handed in. Those + are armed at `start`; the loop's is armed by its first `pulse`, so a + startup spent in blocking CARLA calls before `supervise` runs, each bounded + by the RPC timeout but unbounded in number, counts against nobody. + """ + + def __init__(self, stall: float, *heartbeats: Heartbeat, fail: Callable[[str], None] = die) -> None: + self.loop = Heartbeat("event loop") + self._armed = heartbeats + self._heartbeats = (self.loop, *heartbeats) + self._stall = stall + # Check often enough that a stall is caught soon after the threshold, + # not up to a whole interval late. + self._interval = min(PULSE_INTERVAL, stall / 4) + self._fail = fail + self._stop = threading.Event() + self._thread = threading.Thread(target=self._run, name="watchdog", daemon=True) + + def start(self) -> None: + for heartbeat in self._armed: + heartbeat.beat() + self._thread.start() + + def stop(self) -> None: + self._stop.set() + if self._thread.is_alive(): + self._thread.join(timeout=5.0) + + async def pulse(self) -> None: + """Refresh the event loop heartbeat until cancelled.""" + while True: + self.loop.beat() + await asyncio.sleep(self._interval) + + def _run(self) -> None: + while not self._stop.wait(self._interval): + for heartbeat in self._heartbeats: + age = heartbeat.age() + if age > self._stall: + self._fail(f"{heartbeat.name} has not advanced in {age:.1f} s (limit {self._stall:.1f} s)") + return diff --git a/demo/carla/sim/moq_carla_sim/world.py b/demo/carla/sim/moq_carla_sim/world.py new file mode 100644 index 00000000000..91b82ad2514 --- /dev/null +++ b/demo/carla/sim/moq_carla_sim/world.py @@ -0,0 +1,976 @@ +"""The CARLA side: one world, several trucks, demand-gated cameras. + +Cameras never hand pixels to this process. Each feeds the GPU video bridge +inside the simulator (`demo/carla/bridge`), which renders them only while the +worker's stream wants video; see `bridge`. + +A single client process owns the world. CARLA advises one command-sending +client, and one owner keeps Traffic Manager state coherent, so every truck lives +here rather than in a process each. That is also the point of the demo: they +share a world and can see each other. + +Autonomy is the resting state. A truck nobody is driving runs on Traffic +Manager. Takeover adopts its latest measured controls so switching to manual +does not change steering or pedal demand underneath the driver. +""" + +from __future__ import annotations + +import asyncio +import logging +import math +import threading +import time +from dataclasses import dataclass +from typing import Callable + +import carla + +from . import handback +from .drive import Accepted, DriveState, Gear +from .publish import Collision, Motion, Pose, Telemetry +from .watchdog import Heartbeat + +log = logging.getLogger(__name__) + + +def connect_client(host: str, port: int, timeout: float) -> carla.Client: + """Connect to the simulator, refusing a client the server cannot speak to. + + Fail here rather than at the first spawn: a version mismatch produces + confusing missing-blueprint errors much further along. + """ + client = carla.Client(host, port) + client.set_timeout(timeout) + server = client.get_server_version() + local = client.get_client_version() + if server != local: + raise RuntimeError( + f"CARLA version mismatch: server {server}, client {local}. " + "The python wheel and the server image must match." + ) + log.info("connected to CARLA %s", server) + return client + + +# CARLA's HGV truck, shipped from 0.9.15 on. +TRUCK_BLUEPRINT = "vehicle.carlamotors.european_hgv" + +# Cab-height forward camera. The longitudinal position comes from each +# blueprint's bounding box: the HGV's origin is near its center, so a small +# fixed x offset would leave the sensor inside the cab. +CAMERA_HEIGHT = 2.8 +CAMERA_STANDOFF = 0.25 +CONVOY_SPACING = 12.0 + +# The preview rendition every truck marker on the map plays at once. +SD_HEIGHT = 240 + +# The camera attribute naming the bridge stream a camera renders for. Only the +# patched simulator build has it (https://github.com/moq-dev/carla). +BRIDGE_ATTRIBUTE = "moq_bridge_stream" + +# How often the clock reports its tick durations, matching the video pump's +# throughput line so the two can be read together. +CLOCK_REPORT_INTERVAL = 10.0 + +# How long a spawn waits for the tick that lands its actor. The clock ticks at +# the frame period, so anything near this means it is not running. +SPAWN_TICK_TIMEOUT = 5.0 + +# CARLA reports a collision event for every frame contact persists, so a truck +# resting against a barrier would otherwise report thirty collisions a second. +# Continuing contact with the SAME actor inside this window is coalesced into +# the one collision it is, keeping its peak impulse. Striking something else is +# a new collision however soon it follows: scraping a barrier and then hitting a +# truck half a second later is two events, and folding the second into the first +# would hide it behind the first one's count and actor. +COLLISION_WINDOW = 1.0 + + +def bridge_stream(truck: str, camera: str) -> str: + """The bridge stream a truck's cameras render for, e.g. `truck1/front`.""" + return f"{truck}/{camera}" + + +def camera_transform(vehicle: carla.Vehicle) -> carla.Transform: + box = vehicle.bounding_box + front = box.location.x + box.extent.x + return carla.Transform(carla.Location(x=front + CAMERA_STANDOFF, z=CAMERA_HEIGHT)) + + +def nearby_spawn_points(points: list[carla.Transform], origin: carla.Location) -> list[carla.Transform]: + """Order map spawn points from nearest to farthest from `origin`.""" + return sorted( + points, + key=lambda point: ( + (point.location.x - origin.x) ** 2 + (point.location.y - origin.y) ** 2 + (point.location.z - origin.z) ** 2 + ), + ) + + +def free_spawn_point(points: list[carla.Transform], vehicles: list[carla.Vehicle], own_id: int) -> carla.Transform: + """Return the first candidate with a truck-length gap from every other vehicle.""" + + def separated(vehicle: carla.Vehicle, point: carla.Transform) -> bool: + if vehicle.id == own_id: + return True + location = vehicle.get_transform().location + distance = ( + (location.x - point.location.x) ** 2 + + (location.y - point.location.y) ** 2 + + (location.z - point.location.z) ** 2 + ) + return distance >= CONVOY_SPACING**2 + + for point in points: + if all(separated(vehicle, point) for vehicle in vehicles): + return point + raise RuntimeError(f"every one of the {len(points)} respawn points is occupied") + + +def _waypoint_key(waypoint: carla.Waypoint) -> tuple[int, int, int, float]: + return waypoint.road_id, waypoint.section_id, waypoint.lane_id, waypoint.s + + +def convoy_transform(road_map: carla.Map, lead: carla.Transform, position: int) -> carla.Transform: + """A road-aligned spawn `position` truck lengths behind the lead.""" + lead_waypoint = road_map.get_waypoint(lead.location) + choices = [ + waypoint + for waypoint in lead_waypoint.previous(CONVOY_SPACING * position) + if waypoint.road_id == lead_waypoint.road_id and waypoint.lane_id == lead_waypoint.lane_id + ] + if not choices: + raise RuntimeError("the lead truck is too close to a junction for the convoy") + return min(choices, key=_waypoint_key).transform + + +def convoy_anchor_points( + road_map: carla.Map, + points: list[carla.Transform], + followers: int, +) -> list[carla.Transform]: + """Spawn points with enough uninterrupted same-lane road behind them.""" + if followers == 0: + return points + anchors = [] + for point in points: + try: + convoy_transform(road_map, point, followers) + except RuntimeError: + continue + anchors.append(point) + if not anchors: + raise RuntimeError(f"the map has no spawn point with room for {followers + 1} trucks") + return anchors + + +@dataclass(frozen=True) +class CameraConfig: + """The encoded video ladder. + + The sd rendition is capped at `SD_HEIGHT` pixels high while preserving the + camera's aspect ratio. Both dimensions stay even because the encoder's + 4:2:0 planes require it. Bitrates are explicit because the encoder's resolution-derived + defaults are tuned for efficiency, while this teleoperation demo needs road + detail to survive motion, and every truck's sd plays at once on the + map, so this is the rendition that is always on screen. + """ + + width: int = 1280 + height: int = 720 + fps: int = 30 + sd_bitrate: int = 800_000 + hd_bitrate: int = 4_000_000 + + def renditions(self) -> tuple[RenditionConfig, ...]: + scale = min(1.0, SD_HEIGHT / self.height) + sd_width = max(2, int(self.width * scale) // 2 * 2) + sd_height = max(2, int(self.height * scale) // 2 * 2) + return ( + RenditionConfig( + name="sd", + width=sd_width, + height=sd_height, + fps=self.fps, + bitrate=self.sd_bitrate, + ), + RenditionConfig( + name="hd", + width=self.width, + height=self.height, + fps=self.fps, + bitrate=self.hd_bitrate, + ), + ) + + +@dataclass(frozen=True) +class RenditionConfig: + """One named camera rendition.""" + + name: str + width: int + height: int + fps: int + bitrate: int + + +class Truck: + """One vehicle plus its cameras, and the autonomy/manual switch between them. + + The cameras, one per rendition size, feed the bridge rather than this + process, and the bridge picks which of them renders. What the truck + controls is whether the shared world renders at all for it. + """ + + def __init__( + self, + name: str, + vehicle: carla.Vehicle, + cameras: list[carla.Sensor], + collision: carla.Sensor, + traffic_manager_port: int, + render_demand: Callable[[str, bool], None] | None = None, + *, + road_map: carla.Map, + ) -> None: + self.name = name + self._vehicle = vehicle + self._road_map = road_map + self._cameras = cameras + self._video = False + self._render_demand = render_demand + self._tm_port = traffic_manager_port + self._autonomous = False + self._spawn_transform = vehicle.get_transform() + + # Unlike the cameras, the collision sensor listens for the truck's whole + # life rather than following viewer demand. It emits only on contact, so + # it costs nothing while nothing happens, and the one thing an operator + # must not miss is a truck nobody was watching hitting something. + self._collision_sensor = collision + self._collision_lock = threading.Lock() + self._collision: Collision | None = None + self._collision_at = 0.0 + self._collision_actor: int | None = None + collision.listen(self._on_collision) + + self.engage_autonomy() + + def set_video_demand(self, demanded: bool) -> None: + """Render the shared world for this truck while anyone wants its video. + + CARLA captures nothing in no-rendering mode, so the stream turns this on + before telling the bridge and off after. + """ + if demanded == self._video: + return + if self._render_demand is not None: + self._render_demand(self.name, demanded) + self._video = demanded + log.info("%s: video %s", self.name, "wanted" if demanded else "parked") + + def _on_collision(self, event: carla.CollisionEvent) -> None: + """Record contact, on CARLA's sensor thread. + + Continuing contact with the same actor inside `COLLISION_WINDOW` is + coalesced into one collision carrying its peak impulse, and keeps + extending the window, so grinding along a barrier stays one event. + Anything else struck is a new collision immediately. + + Keyed on the actor's id rather than its `type_id`, which is a blueprint + name: two of the same model of car are one type and two collisions. + """ + impulse = event.normal_impulse + magnitude = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + actor = event.other_actor + # Monotonic for the coalescing window, which is a duration and must not + # move when the host clock is stepped; wall clock for the wire, which is + # an instant a browser on another machine has to age. + now = time.monotonic() + with self._collision_lock: + previous = self._collision + if ( + previous is not None + and self._collision_actor == actor.id + and now - self._collision_at < COLLISION_WINDOW + ): + # The timestamp stays at the start of contact: it identifies the + # collision, so moving it would republish one event as many. + self._collision = Collision( + at=previous.at, + other=previous.other, + impulse=max(previous.impulse, magnitude), + ) + else: + self._collision = Collision( + at=time.time() * 1000.0, + other=actor.type_id, + impulse=magnitude, + ) + log.warning("%s: collision with %s", self.name, self._collision.other) + self._collision_actor = actor.id + self._collision_at = now + + def telemetry(self) -> Telemetry: + """One consistent read of the vehicle: where it is, and how it responded. + + Every getter here reads the state the client received on the last tick + rather than issuing an RPC, so polling every truck at the status rate + costs the simulator nothing. + + `get_control` is the control the physics actually applied, which is + feedback rather than the command echo, and is populated whoever is + driving - including Traffic Manager. Acceleration and yaw rate come out + of the world frame and are projected onto the vehicle's own axes, since + what matters to a heavy truck is how hard it is being pushed sideways. + """ + transform = self._vehicle.get_transform() + velocity = self._vehicle.get_velocity() + control = self._vehicle.get_control() + acceleration = self._vehicle.get_acceleration() + angular = self._vehicle.get_angular_velocity() + # CARLA's own basis vectors rather than trigonometry on yaw: the world is + # left-handed with y pointing south, so hand-rolling the rotation is an + # easy way to publish lateral acceleration with the wrong sign. + forward = transform.get_forward_vector() + right = transform.get_right_vector() + + def project(vector: carla.Vector3D, axis: carla.Vector3D) -> float: + """Component of `vector` along a unit `axis`, in all three dimensions. + + The z terms are not optional padding: pitch tilts `forward` out of + the horizontal plane and roll tilts `right`, so dropping them + underreports exactly on the graded, banked roads where the + numbers matter. + """ + return vector.x * axis.x + vector.y * axis.y + vector.z * axis.z + + with self._collision_lock: + collision = self._collision + return Telemetry( + pose=Pose( + x=transform.location.x, + y=transform.location.y, + yaw=transform.rotation.yaw, + speed=math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2), + ), + measured=DriveState( + steering=control.steer, + throttle=control.throttle, + brake=control.brake, + # CARLA has no neutral - `actuate` expresses it as zero throttle - + # so the measured gear is the direction the physics drove in. + gear=Gear.REVERSE if control.reverse else Gear.FORWARD, + ), + motion=Motion( + yaw_rate=angular.z, + accel_lon=project(acceleration, forward), + accel_lat=project(acceleration, right), + ), + collision=collision, + ) + + def engage_autonomy(self) -> None: + """Hand the truck to Traffic Manager where it stands. Idempotent.""" + if self._autonomous: + return + self._vehicle.set_autopilot(True, self._tm_port) + self._autonomous = True + log.info("%s: autonomy engaged", self.name) + + def release(self) -> None: + """Return a driverless truck to autonomy, merging onto a lane first. + + Idempotent: the drive loop calls this when the driver lets go, and the + telemetry timer keeps calling it until the truck is autonomous again. + Until then the truck coasts under `handback`'s steering; see that + module for why Traffic Manager cannot simply be given a moving truck. + """ + if self._autonomous: + return + measured = self._vehicle.get_control() + control = handback.plan( + self._road_map, + self._vehicle.get_transform(), + self._vehicle.get_velocity(), + measured.steer, + reversing=measured.reverse, + ) + if control is None: + self.engage_autonomy() + else: + self._vehicle.apply_control(control) + + def respawn(self) -> None: + """Return this actor to a free spawn point without breaking its sensors.""" + world = self._vehicle.get_world() + map_points = self._road_map.get_spawn_points() + candidates = [ + self._spawn_transform, + *nearby_spawn_points(map_points, self._spawn_transform.location), + ] + transform = free_spawn_point( + candidates, + list(world.get_actors().filter("vehicle.*")), + self._vehicle.id, + ) + if self._autonomous: + self._vehicle.set_autopilot(False, self._tm_port) + self._autonomous = False + self._vehicle.apply_control(carla.VehicleControl()) + self._vehicle.set_transform(transform) + self._vehicle.set_target_velocity(carla.Vector3D()) + self._vehicle.set_target_angular_velocity(carla.Vector3D()) + with self._collision_lock: + self._collision = None + self._collision_actor = None + self._collision_at = 0.0 + self.engage_autonomy() + log.info("%s: respawned", self.name) + + @property + def autonomous(self) -> bool: + return self._autonomous + + def _disengage_autonomy(self) -> None: + if not self._autonomous: + return + # Traffic Manager releases the vehicle where it is, still carrying its + # velocity. The takeover command preserves its latest measured controls. + self._vehicle.set_autopilot(False, self._tm_port) + self._autonomous = False + log.info("%s: autonomy disengaged, driver has control", self.name) + + def actuate(self, accepted: Accepted, *, has_driver: bool) -> None: + """Apply accepted state to the vehicle, or return it to autonomy. + + Only the gate's accepted state reaches CARLA. The three cases are + deliberately different, because they are not equally urgent: + + - E-stop is the one hard latch: full brake, autonomy off, and it stays + that way until an explicit reset. + - No driver means autonomy resumes, once the truck is somewhere Traffic + Manager can take it (see `release`). A lapsed lease or a closed tab + is a truck nobody is steering, not an emergency. + - A claim carrying only the resting state leaves Traffic Manager in + control rather than braking the truck during takeover. + - A driver that supplied input drives the accepted command state. + """ + if accepted.estopped: + self._disengage_autonomy() + self._vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=1.0, steer=0.0, hand_brake=True)) + return + + if not has_driver: + self.release() + return + + # A player without feedback yet claims with the resting state. Applying + # that frame would make every click brake an autonomous truck. + if self._autonomous and accepted.state == DriveState.stopped(): + return + + self._disengage_autonomy() + state = accepted.state + # CARLA has no neutral, so it is expressed as zero throttle: a truck in + # neutral coasts and still brakes and steers, rather than driving. + throttle = 0.0 if state.gear is Gear.NEUTRAL else state.throttle + self._vehicle.apply_control( + carla.VehicleControl( + throttle=throttle, + brake=state.brake, + steer=state.steering, + reverse=state.gear is Gear.REVERSE, + ) + ) + + def destroy(self) -> None: + """Tear down every sensor before the vehicle they are attached to. + + Cleanup keeps walking after a failed actor RPC so one sensor the server + already culled cannot leave the other sensor or vehicle behind. + """ + failure: RuntimeError | None = None + for sensor in (*self._cameras, self._collision_sensor): + for cleanup in (sensor.stop, sensor.destroy): + try: + cleanup() + except RuntimeError as err: + failure = failure or err + try: + self._vehicle.destroy() + except RuntimeError as err: + failure = failure or err + if failure is not None: + raise failure + + +def next_deadline(deadline: float, now: float, period: float) -> float: + """When the tick after the one just issued is due. + + Deadlines advance by the period from each other, not from `now`, so the + cadence does not drift by the tick's own duration. A tick that overran by + more than a whole period (a server hitch) re-anchors on `now` instead: + catching up would fire the missed ticks back to back, and a burst of frames + stamped a step apart is the very judder a fixed step exists to avoid. Sim + time simply runs that much behind the wall clock, which nothing depends on. + """ + deadline += period + if deadline < now - period: + return now + return deadline + + +# Why the clock is being stepped. `Clock` holds are keyed by reason so the +# startup gate, a camera, a map viewer and a driver can each keep the world +# running without one of them releasing on another's behalf. +STARTUP_HOLD = "startup" +RENDER_HOLD = "render" + + +def drive_hold(truck: str) -> str: + return f"drive:{truck}" + + +def status_hold(truck: str) -> str: + return f"status:{truck}" + + +class Clock: + """Drives the simulation one fixed step per wall-clock period. + + CARLA's own free-running mode stamps each camera frame with however long the + previous server tick took, and with `sensor_tick` quantizing on top of that + a nominal 30 fps camera delivered frames 11 ms and 53 ms apart in pairs. + The player paints by those stamps, so the judder is on screen whatever the + encoder or the network do. Synchronous mode with a fixed step makes every + frame exactly a period apart, and this thread supplies the ticks at that + period so sim time keeps pace with the wall clock. + + A tick failure is fatal for the session, the same as a lost video track: in + synchronous mode a world nobody ticks is frozen, and every truck would go on + looking healthy while publishing nothing. A tick that never returns is the + same frozen world with no failure to report, so each one that does return + beats `heartbeat` for the watchdog. + + Ticking is held rather than unconditional. Stepping physics and Traffic + Manager costs a full core even with rendering parked, so a world no viewer + and no driver needs stops stepping until one arrives. `hold` and `release` + name their reason so two holders cannot cancel each other out; see + `World.ready` for the one that covers startup. + """ + + def __init__(self, world: carla.World, fps: int) -> None: + self._world = world + self._period = 1.0 / fps + self.heartbeat = Heartbeat("simulation clock") + self._stop = threading.Event() + self._thread = threading.Thread(target=self._run, name="carla-clock", daemon=True) + self._failure_loop: asyncio.AbstractEventLoop | None = None + self._failure: asyncio.Future[BaseException] | None = None + # Held through `sweep` and `spawn`, which wait on ticks. `World.ready` + # drops it once every truck is in the world. + self._holds: set[str] = {STARTUP_HOLD} + self._holds_lock = threading.Lock() + self._wanted = threading.Event() + self._wanted.set() + + @property + def running(self) -> bool: + """Whether the world is currently being stepped.""" + return self._wanted.is_set() + + def hold(self, reason: str) -> None: + """Step the world until `reason` is released. Idempotent.""" + with self._holds_lock: + self._holds.add(reason) + self._wanted.set() + + def release(self, reason: str) -> None: + """Drop one hold, parking the world once the last one goes. Idempotent.""" + with self._holds_lock: + self._holds.discard(reason) + if not self._holds: + self._wanted.clear() + + def start(self) -> None: + self._failure_loop = asyncio.get_running_loop() + self._failure = self._failure_loop.create_future() + self._thread.start() + + async def wait_failed(self) -> None: + """Raise when the clock can no longer tick the world.""" + if self._failure is None: + raise RuntimeError("clock was not started") + failure = await self._failure + raise RuntimeError("simulation clock stopped") from failure + + def stop(self) -> None: + self._stop.set() + if self._thread.is_alive(): + self._thread.join(timeout=5.0) + if self._failure is not None and not self._failure.done(): + self._failure.cancel() + + def _run(self) -> None: + deadline = time.monotonic() + # Tick durations since the last report. A tick longer than the period is + # the server failing to render the active cameras in time, which shows + # up as a slow-motion truck and nothing else would name the cause. + ticks: list[float] = [] + reported = deadline + parked = False + while not self._stop.is_set(): + if not self._wanted.is_set(): + if not parked: + log.info("clock parked") + parked = True + # The thread is idle by design, not wedged, so it keeps beating: + # the watchdog exists for a `tick` that never returns, and would + # otherwise kill the worker a minute into every parked world. + # Waking each period rather than blocking also keeps `stop` + # responsive during shutdown. + self.heartbeat.beat() + self._wanted.wait(self._period) + # Whatever the pause cost, the next tick is due now rather than + # owed a burst of catch-up steps; `next_deadline` collapses a + # deadline this far behind to the current time. + reported = time.monotonic() + continue + if parked: + log.info("clock running") + parked = False + started = time.monotonic() + try: + self._world.tick() + except RuntimeError as err: + if self._failure_loop is not None: + self._failure_loop.call_soon_threadsafe(self._record_failure, err) + log.error("simulation tick failed, stopping: %s", err) + return + now = time.monotonic() + self.heartbeat.beat() + ticks.append(now - started) + if now - reported >= CLOCK_REPORT_INTERVAL: + late = sum(1 for tick in ticks if tick > self._period) + log.info( + "clock: %d ticks, mean %.1f ms, max %.1f ms, %d over the %.1f ms period", + len(ticks), + 1000 * sum(ticks) / len(ticks), + 1000 * max(ticks), + late, + 1000 * self._period, + ) + ticks.clear() + reported = now + deadline = next_deadline(deadline, now, self._period) + self._stop.wait(max(0.0, deadline - now)) + + def _record_failure(self, failure: BaseException) -> None: + if self._failure is not None and not self._failure.done(): + self._failure.set_result(failure) + + +class World: + """The shared CARLA world and the trucks spawned into it. + + The world runs in synchronous mode at a fixed step of `1 / fps`, ticked by + `clock`; see `Clock` for why. The mode is restored on `destroy`, because a + synchronous world with no client ticking it is frozen for the next one. + """ + + def __init__(self, client: carla.Client, traffic_manager_port: int, fps: int) -> None: + self._client = client + self._world = client.get_world() + self._tm = client.get_trafficmanager(traffic_manager_port) + self._tm_port = traffic_manager_port + self._map: carla.Map | None = None + self._rendering_trucks: set[str] = set() + self.trucks: list[Truck] = [] + # The server outlives this worker and may have been left rendering, or + # free-running, by a hard crash. Assert the idle state and the fixed + # step before doing any other world work. Traffic Manager setup can + # still fail after apply_settings has already frozen the world, so + # roll that back rather than leave the next client a world nobody ticks. + settings = self._world.get_settings() + settings.no_rendering_mode = True + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1.0 / fps + self._world.apply_settings(settings) + try: + # Traffic Manager steps with the world, so a stalled tick stalls the + # autonomous trucks rather than leaving them to drive through a frozen + # frame. + self._tm.set_synchronous_mode(True) + except BaseException: + try: + settings = self._world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = 0.0 + self._world.apply_settings(settings) + except RuntimeError as err: + log.warning("failed to release synchronous mode after setup failed: %s", err) + raise + self.clock = Clock(self._world, fps) + + @classmethod + def connect(cls, host: str, port: int, timeout: float, traffic_manager_port: int, fps: int) -> World: + return cls(connect_client(host, port, timeout), traffic_manager_port, fps) + + def road_map(self) -> carla.Map: + """The town, fetched once. + + `get_map` downloads and parses the whole OpenDRIVE, and both spawning and + the map name want it, so hold on to it rather than paying twice at start. + """ + if self._map is None: + self._map = self._world.get_map() + return self._map + + def map_name(self) -> str: + """The loaded map's short name, e.g. `Town10HD_Opt`. + + CARLA reports it as a content path (`Carla/Maps/Town10HD_Opt`). The last + segment is what `just carla map` names the road file it bakes, + and what the player matches before drawing those roads under the trucks. + """ + return self.road_map().name.rsplit("/", 1)[-1] + + def _set_render_demand(self, name: str, demanded: bool) -> None: + """Render the shared world while at least one truck needs a camera.""" + next_trucks = set(self._rendering_trucks) + if demanded: + next_trucks.add(name) + else: + next_trucks.discard(name) + if next_trucks == self._rendering_trucks: + return + + was_rendering = bool(self._rendering_trucks) + will_render = bool(next_trucks) + if was_rendering != will_render: + self._set_rendering(will_render) + # Parking the renderer leaves physics and Traffic Manager stepping, + # which is most of a core on its own, so the clock follows the same + # demand. A driver or a map viewer holds it separately: see + # `TruckSession`. + if will_render: + self.clock.hold(RENDER_HOLD) + else: + self.clock.release(RENDER_HOLD) + self._rendering_trucks = next_trucks + + def ready(self) -> None: + """Let the world park now that every truck is spawned. + + `sweep` and `spawn` wait on ticks, so the clock is held from + construction until this drops the gate. Nothing has demanded a camera + yet at that point, so the usual next step is straight to parked. + """ + self.clock.release(STARTUP_HOLD) + + def _set_rendering(self, enabled: bool) -> None: + settings = self._world.get_settings() + no_rendering = not enabled + if settings.no_rendering_mode == no_rendering: + return + settings.no_rendering_mode = no_rendering + self._world.apply_settings(settings) + log.info("CARLA rendering %s", "enabled" if enabled else "parked") + + def sweep(self) -> None: + """Remove every vehicle a previous run left in the world, sensors first. + + The server outlives its clients, and Traffic Manager runs inside the + client, so a worker that died without `destroy` leaves its trucks frozen + wherever they were, holding whatever control they last had. They sit on + the roads the next run spawns onto, and a fresh truck drives up behind + one and waits there forever. This process is the world's only owner, so + every vehicle in it is ours to remove. + """ + actors = self._world.get_actors() + vehicles = list(actors.filter("vehicle.*")) + if not vehicles: + return + stale = {vehicle.id for vehicle in vehicles} + sensors = [ + sensor for sensor in actors.filter("sensor.*") if sensor.parent is not None and sensor.parent.id in stale + ] + for sensor in sensors: + for cleanup in (sensor.stop, sensor.destroy): + try: + cleanup() + except RuntimeError as err: + log.warning("failed to remove stale sensor %d: %s", sensor.id, err) + # Every vehicle gets its attempt before the first failure is raised, so + # one the server refuses does not leave the rest behind. A stale truck + # still in the world is the jam this is here to clear, so spawning + # beside it is not an option. + stuck: list[str] = [] + for vehicle in vehicles: + try: + if not vehicle.destroy(): + stuck.append(f"{vehicle.id}: server refused") + except RuntimeError as err: + stuck.append(f"{vehicle.id}: {err}") + if stuck: + raise RuntimeError("failed to remove stale vehicles: " + "; ".join(stuck)) + log.warning("removed %d vehicles and %d sensors left behind by a previous run", len(vehicles), len(sensors)) + # Destroys land on the next tick in synchronous mode. Spawn walks the + # map immediately after, so wait for that tick or it still sees them. + self._world.wait_for_tick(SPAWN_TICK_TIMEOUT) + + def spawn(self, names: list[str], camera: CameraConfig, camera_name: str) -> None: + """Spawn one truck per name into the shared world, cameras attached. + + The first truck anchors a same-lane convoy. Every later truck first tries + its position directly behind the lead, then falls back to the closest map + spawn points if the road is obstructed. CARLA rejects physical overlap. + + Each truck's cameras render for its bridge stream, `truck/camera_name`. + The simulator registers them with the bridge as they spawn, which is + what starts it listening for this worker. + """ + library = self._world.get_blueprint_library() + if not library.find("sensor.camera.rgb").has_attribute(BRIDGE_ATTRIBUTE): + raise RuntimeError( + f"the simulator's cameras have no {BRIDGE_ATTRIBUTE} attribute: the simulator " + "lacks the GPU video bridge. Build it with https://github.com/moq-dev/carla and pin CARLA_IMAGE." + ) + truck_bp = library.find(TRUCK_BLUEPRINT) + # Traffic Manager deletes stuck non-hero vehicles. Every truck must + # survive a jam until an operator takes over or respawns it; deleting + # one also invalidates its sensors and telemetry. Hero still supports + # autopilot and anchors streamed map tiles around the vehicle. + truck_bp.set_attribute("role_name", "hero") + + road_map = self.road_map() + points = road_map.get_spawn_points() + if len(points) < len(names): + raise RuntimeError(f"map has {len(points)} spawn points, need {len(names)}") + anchors = convoy_anchor_points(road_map, points, len(names) - 1) + + lead_transform: carla.Transform | None = None + for index, name in enumerate(names): + if lead_transform is None: + candidates = anchors + else: + convoy_point = convoy_transform(road_map, lead_transform, index) + candidates = [convoy_point, *nearby_spawn_points(points, convoy_point.location)] + + vehicle = self._spawn_vehicle(truck_bp, candidates) + cameras: list[carla.Sensor] = [] + # Every sensor spawned so far, in spawn order, so the failure paths + # below can walk them without caring which kind each one is. + sensors: list[carla.Sensor] = [] + try: + # A synchronous spawn lands on the next tick, and the transform the + # convoy is laid out from is read back from that tick's snapshot. + self._world.wait_for_tick(SPAWN_TICK_TIMEOUT) + if index == 0: + lead_transform = vehicle.get_transform() + for rendition in camera.renditions(): + camera_bp = library.find("sensor.camera.rgb") + camera_bp.set_attribute("image_size_x", str(rendition.width)) + camera_bp.set_attribute("image_size_y", str(rendition.height)) + # Every tick: the fixed step already is the frame period, and a + # tick-length `sensor_tick` on top would skip frames on rounding. + camera_bp.set_attribute("sensor_tick", "0.0") + camera_bp.set_attribute(BRIDGE_ATTRIBUTE, bridge_stream(name, camera_name)) + # The simulator refuses the spawn when the camera cannot + # reach the bridge, naming why in its log. + cameras.append(self._world.spawn_actor(camera_bp, camera_transform(vehicle), attach_to=vehicle)) + sensors.append(cameras[-1]) + # Attached at the vehicle origin: a collision sensor reports the + # actor's contacts, so where on it the sensor sits is irrelevant. + collision = self._world.spawn_actor( + library.find("sensor.other.collision"), carla.Transform(), attach_to=vehicle + ) + sensors.append(collision) + except BaseException: + # The vehicle is in the world already and `destroy` only walks + # the trucks it finished building, so failed sensors would + # otherwise leave it parked there for the next run. Every exit + # counts, a SIGTERM mid-RPC included, not just a refused spawn. + for sensor in sensors: + try: + sensor.destroy() + except RuntimeError as err: + log.warning("failed to clean up sensor after %s sensor spawn failed: %s", name, err) + try: + vehicle.destroy() + except RuntimeError as err: + log.warning("failed to clean up vehicle after %s spawn failed: %s", name, err) + raise + try: + truck = Truck( + name, + vehicle, + cameras, + collision, + self._tm_port, + self._set_render_demand, + road_map=road_map, + ) + except BaseException: + # Construction enables autopilot before the Truck reaches + # `self.trucks`, so clean up every actor here rather than leave + # them invisible to `destroy()` on the persistent server. + cleanups = [ + *(cleanup for sensor in sensors for cleanup in (sensor.stop, sensor.destroy)), + vehicle.destroy, + ] + for cleanup in cleanups: + try: + cleanup() + except RuntimeError as err: + log.warning("failed to clean up after %s construction failed: %s", name, err) + raise + self.trucks.append(truck) + log.info("spawned %s (actor %d)", name, vehicle.id) + + def _spawn_vehicle(self, blueprint: carla.ActorBlueprint, points: list[carla.Transform]): + """Spawn at the first free point in the caller's preferred order. + + `try_spawn_actor` returns None when the point is occupied. `sweep` has + cleared the last run's trucks by now, so that is a convoy follower whose + spot is blocked; walking the ordered candidates finds it the nearest + free road, and still fails loudly if the map is genuinely full. + """ + for point in points: + vehicle = self._world.try_spawn_actor(blueprint, point) + if vehicle is not None: + return vehicle + raise RuntimeError(f"every one of the {len(points)} candidate spawn points is occupied") + + def destroy(self) -> None: + """Remove every actor this process spawned and hand the world back. + + The server outlives the client, so anything left behind stays in the + world and the next run spawns beside it. The world goes back to + free-running first: with the clock stopped nothing else would apply the + destroys, and a synchronous world with no client is frozen for whatever + connects next (`just carla map`, a hand-run client). + """ + self.clock.stop() + try: + self._tm.set_synchronous_mode(False) + settings = self._world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = 0.0 + self._world.apply_settings(settings) + except RuntimeError as err: + # A server already gone cannot be handed anything; the actors below + # are gone with it, and their own errors say so. + log.warning("failed to release synchronous mode: %s", err) + for truck in self.trucks: + try: + truck.destroy() + except RuntimeError as err: + # A truck already gone (server restarted, actor culled) must not + # stop the rest from being cleaned up. + log.warning("failed to destroy %s: %s", truck.name, err) + self.trucks.clear() diff --git a/demo/carla/sim/pyproject.toml b/demo/carla/sim/pyproject.toml new file mode 100644 index 00000000000..505c49c58f3 --- /dev/null +++ b/demo/carla/sim/pyproject.toml @@ -0,0 +1,47 @@ +# The CARLA worker. Not published: a demo, run from the repo. +# +# moq's bindings are deliberately NOT listed here. venv.sh installs them from +# this checkout's py/ (moq-ffi built by maturin, moq-rs editable), so the +# worker always runs the bindings of the tree it ships in, never a PyPI release. +[project] +name = "moq-carla-sim" +version = "0.1.0" +description = "A CARLA world of simulated trucks published over MoQ, steerable from the browser." +requires-python = ">=3.12,<3.13" +dependencies = [ + # CARLA ships wheels for cp310-cp312 only, which is what pins the + # interpreter above. + "carla==0.9.16", +] + +[build-system] +requires = ["setuptools>=68"] +build-backend = "setuptools.build_meta" + +[tool.setuptools] +packages = ["moq_carla_sim"] + +[tool.ruff] +line-length = 120 +target-version = "py312" + +[tool.ruff.lint] +select = ["E", "F", "W", "I"] + +[tool.ruff.format] +quote-style = "double" + +# Collected from the root uv workspace's dev tools rather than the CARLA venv: +# tests/conftest.py stubs `moq` and `carla`, so the package imports from source. +[tool.pytest.ini_options] +testpaths = ["tests"] +pythonpath = ["."] + +# Type-checks the worker against the in-tree moq wrapper source, so an API +# change there fails the gate instead of crashing at runtime. carla exists only +# in the CARLA venv; typings/ stands in for it. +[tool.pyright] +include = ["moq_carla_sim"] +extraPaths = ["../../../py/moq-rs"] +pythonVersion = "3.12" +reportMissingModuleSource = false diff --git a/demo/carla/sim/python.sh b/demo/carla/sim/python.sh new file mode 100755 index 00000000000..40df128b089 --- /dev/null +++ b/demo/carla/sim/python.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash +# +# Run the CARLA worker's venv interpreter with an environment its wheels can +# actually load. Every entry point that runs the worker or a CARLA client goes +# through here, so the two fixes below live in one place. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +venv="$here/.venv" + +if [[ ! -x "$venv/bin/python" ]]; then + echo "python.sh: no venv; run 'just carla venv' first." >&2 + exit 1 +fi + +# 1. The devshell exports a PYTHONPATH into its own Python's site-packages. A +# venv does not override it, so a 3.12 interpreter would import packages +# built for another version and fail somewhere unrelated. +export PYTHONPATH= + +# 2. manylinux wheels (CARLA's client and moq's own cdylib) link libstdc++ and +# libz, which under Nix are in no default loader path. Ask the compiler on +# PATH where they are rather than hardcoding store hashes that move with +# every nixpkgs bump. A library it cannot place is left to the host's loader. +libs=() +for lib in libstdc++.so.6 libz.so.1; do + found="$(g++ -print-file-name="$lib")" + # g++ echoes the bare name back when it cannot place the library. + [[ "$found" == /* ]] && libs+=("$(dirname "$found")") +done +if [[ ${#libs[@]} -eq 0 ]]; then + echo "python.sh: g++ cannot locate libstdc++ or libz; run inside the devshell." >&2 + exit 1 +fi + +# No NVIDIA driver: the simulator encodes, and the worker only republishes. +LD_LIBRARY_PATH="$( + IFS=: + echo "${libs[*]}" +)${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +export LD_LIBRARY_PATH + +exec "$venv/bin/python" "$@" diff --git a/demo/carla/sim/python.test.sh b/demo/carla/sim/python.test.sh new file mode 100755 index 00000000000..ef7c00b3825 --- /dev/null +++ b/demo/carla/sim/python.test.sh @@ -0,0 +1,47 @@ +#!/usr/bin/env bash +# Offline regression for python.sh's loader environment. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +scratch="$(mktemp -d)" +trap 'rm -rf "$scratch"' EXIT + +cp "$here/python.sh" "$scratch/python.sh" +mkdir -p "$scratch/.venv/bin" "$scratch/bin" + +cat >"$scratch/.venv/bin/python" <<'EOF' +#!/usr/bin/env bash +printf '%s|%s\n' "$PYTHONPATH" "$LD_LIBRARY_PATH" +EOF +chmod +x "$scratch/.venv/bin/python" + +# A compiler that places libstdc++ but not libz, as the devshell's does. +cat >"$scratch/bin/g++" <<'EOF' +#!/usr/bin/env bash +case "$1" in +-print-file-name=libstdc++.so.6) echo /gcc/lib/libstdc++.so.6 ;; +*) echo "${1#-print-file-name=}" ;; +esac +EOF +chmod +x "$scratch/bin/g++" + +# The libraries the compiler found go first; nothing else, the NVIDIA driver +# included, since the worker never encodes. The devshell's PYTHONPATH is cleared. +actual="$(PATH="$scratch/bin:$PATH" PYTHONPATH=/devshell LD_LIBRARY_PATH=/existing "$scratch/python.sh")" +expected="|/gcc/lib:/existing" +if [[ "$actual" != "$expected" ]]; then + echo "python.test: expected $expected, got $actual" >&2 + exit 1 +fi + +# A compiler that places neither is refused rather than run against the host's. +cat >"$scratch/bin/g++" <<'EOF' +#!/usr/bin/env bash +echo "${1#-print-file-name=}" +EOF +if PATH="$scratch/bin:$PATH" "$scratch/python.sh" >/dev/null 2>&1; then + echo "python.test: ran without locating libstdc++ or libz" >&2 + exit 1 +fi + +echo "python.test: ok" diff --git a/demo/carla/sim/server.sh b/demo/carla/sim/server.sh new file mode 100755 index 00000000000..a2387b0e41d --- /dev/null +++ b/demo/carla/sim/server.sh @@ -0,0 +1,130 @@ +#!/usr/bin/env bash +# +# Run the CARLA simulator on the local GPU, in podman. +# +# CARLA is distributed as an Unreal Engine build against Ubuntu's driver stack, +# so a container is the only sane way to run it on Fedora/NixOS. It renders +# offscreen and speaks its RPC on :2000; the worker (`just carla worker`) is a +# client and can be restarted independently. +set -euo pipefail + +# Stock upstream by default. The worker needs the patched build (the GPU video +# bridge and the RPC work guard), a local image built by +# https://github.com/moq-dev/carla, so it is selected by pinning CARLA_IMAGE +# rather than shipped here; put +# that pin in ~/.config/moq/carla.env, which carla-sim.service reads. The +# resolved image is logged below, so a pin that went missing is visible. The +# worker refuses a stock simulator by name. +image="${CARLA_IMAGE:-docker.io/carlasim/carla:0.9.16}" +name="${CARLA_NAME:-carla}" +port="${CARLA_PORT:-2000}" +# Where the bridge inside the simulator listens for the worker; the worker's +# --bridge-socket defaults to the same path. +bridge_dir="${CARLA_BRIDGE_DIR:-${XDG_RUNTIME_DIR:-/tmp}/carla-bridge}" + +if ! command -v podman >/dev/null 2>&1; then + echo "carla: podman not found." >&2 + exit 1 +fi + +# The GPU reaches the container through a CDI device generated by +# nvidia-container-toolkit. Check its referenced entries and hook before pulling +# the 8.6 GB image: a spec generated through a bare `nix shell` embeds an +# unrooted store path that breaks after Nix GC even though the YAML still exists. +# nvidia-ctk writes YAML by default, while NixOS's declarative generator writes +# JSON. Accept both without depending on jq at service runtime. +if [[ -n "${CARLA_CDI_SPEC:-}" ]]; then + specs=("$CARLA_CDI_SPEC") +else + shopt -s nullglob + specs=(/etc/cdi/*nvidia* /var/run/cdi/*nvidia* "${XDG_CONFIG_HOME:-$HOME/.config}"/cdi/*nvidia*) + shopt -u nullglob +fi + +valid_spec= +for spec in "${specs[@]}"; do + valid=1 + entry_seen=0 + hook_seen=0 + while IFS= read -r host_path; do + case "$host_path" in + *nvidia-ctk | *nvidia-cdi-hook) ;; + *) entry_seen=1 ;; + esac + if [[ ! -e "$host_path" ]]; then + valid=0 + break + fi + done < <(sed -nE \ + -e 's/^[[:space:]]*(- )?(path|hostPath):[[:space:]]+([^[:space:]]+)[[:space:]]*$/\3/p' \ + -e 's/^[[:space:]]*"(path|hostPath)":[[:space:]]*"([^"]+)",?[[:space:]]*$/\2/p' \ + "$spec") + while IFS= read -r hook; do + hook_seen=1 + if [[ ! -x "$hook" ]]; then + valid=0 + break + fi + done < <(sed -nE \ + -e 's/^[[:space:]]*(- )?path:[[:space:]]+([^[:space:]]*nvidia-(ctk|cdi-hook))[[:space:]]*$/\2/p' \ + -e 's/^[[:space:]]*"path":[[:space:]]*"([^"]*nvidia-(ctk|cdi-hook))",?[[:space:]]*$/\1/p' \ + "$spec") + if [[ "$valid" -eq 1 && "$entry_seen" -eq 1 && "$hook_seen" -eq 1 ]]; then + valid_spec="$spec" + break + fi +done + +if [[ -z "$valid_spec" ]]; then + cat >&2 <<-'EOF' + carla: the nvidia.com/gpu CDI device is missing or stale. Set it up with: + + just carla cdi + + Then point Podman at it by adding to ~/.config/containers/containers.conf: + + [engine] + cdi_spec_dirs = ["/etc/cdi", "/var/run/cdi", "/.config/cdi"] + EOF + exit 1 +fi + +# Name the resolved image in the journal, so a pin that has quietly gone back +# to stock is visible rather than showing up only as idle CPU. +digest="$(podman image inspect --format '{{.Digest}}' "$image" 2>/dev/null || true)" +echo "carla: image $image (${digest:-not pulled yet})" >&2 + +# --security-opt label=disable: SELinux denies a container access to +# /dev/nvidia* even though the nodes are mode 0666, and the failure looks like a +# missing driver rather than a denial. The alternative is a host-wide +# `setsebool -P container_use_devices 1`, which needs root. +# +# --net=host: the worker connects to the RPC port and the traffic manager port, +# and CARLA hands out further ports at runtime. +# +# --userns=keep-id: the image's carla user is UID 1000, and keeping IDs makes +# it this user, so the bridge's socket in the shared directory is one the +# worker may connect to. The directory stays private to this user. +# The name is the service's ownership boundary. --replace lets systemd adopt a +# manually started or orphaned simulator container instead of retrying forever on a +# name collision; it never touches another container name. +# +# The engine binary directly, not the image's CarlaUE4.sh: that wrapper runs +# the binary without exec, leaving a shell as container PID 1 that ignores +# SIGTERM, so a stop would sit through systemd's timeout and leave the +# simulator running. As PID 1 the engine gets the signal and exits in a few +# seconds. +# Override the image entrypoint too: wrappers can silently append quality or +# experimental flags after the launcher's arguments. +mkdir -p "$bridge_dir" +chmod 700 "$bridge_dir" +exec podman run --rm --replace --name "$name" \ + --security-opt label=disable \ + --device nvidia.com/gpu=all \ + --net=host \ + --userns=keep-id \ + -v "$bridge_dir:$bridge_dir" \ + --entrypoint /workspace/CarlaUE4/Binaries/Linux/CarlaUE4-Linux-Shipping \ + "$image" \ + CarlaUE4 -RenderOffScreen -nosound -carla-rpc-port="$port" -quality-level=Epic \ + -moq-bridge-socket="$bridge_dir/bridge.sock" diff --git a/demo/carla/sim/server.test.sh b/demo/carla/sim/server.test.sh new file mode 100755 index 00000000000..453333ed3f1 --- /dev/null +++ b/demo/carla/sim/server.test.sh @@ -0,0 +1,91 @@ +#!/usr/bin/env bash +# Offline regression for the CARLA container lifecycle contract. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +tmp="$(mktemp -d)" +trap 'rm -rf "$tmp"' EXIT + +mkdir -p "$tmp/bin" "$tmp/config/cdi" +cat >"$tmp/bin/podman" <<'EOF' +#!/usr/bin/env bash +set -euo pipefail +printf '%s\n' "$@" >"${CARLA_TEST_PODMAN_ARGS:?}" +EOF +chmod +x "$tmp/bin/podman" + +: >"$tmp/config/cdi/nvidia.yaml" +if PATH="$tmp/bin:$PATH" CARLA_CDI_SPEC="$tmp/config/cdi/nvidia.yaml" bash "$here/server.sh" >"$tmp/empty.out" 2>&1; then + echo "empty CDI spec unexpectedly accepted" >&2 + exit 1 +fi +grep -Fq 'just carla cdi' "$tmp/empty.out" + +touch "$tmp/device" +# The fixtures mirror nvidia-ctk's layout: entries are YAML list items, the +# hook's path is a bare key under `- hookName:`. +printf -- '- hostPath: %s\n path: %s\n' "$tmp/device" "$tmp/missing/nvidia-ctk" >"$tmp/config/cdi/nvidia.yaml" +if PATH="$tmp/bin:$PATH" CARLA_CDI_SPEC="$tmp/config/cdi/nvidia.yaml" bash "$here/server.sh" >"$tmp/stale.out" 2>&1; then + echo "stale CDI hook unexpectedly accepted" >&2 + exit 1 +fi +grep -Fq 'just carla cdi' "$tmp/stale.out" + +touch "$tmp/bin/nvidia-ctk" +chmod +x "$tmp/bin/nvidia-ctk" +printf -- '- hostPath: %s\n path: %s\n' "$tmp/device" "$tmp/bin/nvidia-ctk" >"$tmp/config/cdi/nvidia.yaml" +PATH="$tmp/bin:$PATH" \ + CARLA_CDI_SPEC="$tmp/config/cdi/nvidia.yaml" \ + XDG_RUNTIME_DIR="$tmp/run" \ + CARLA_TEST_PODMAN_ARGS="$tmp/podman.args" \ + bash "$here/server.sh" + +# NixOS's declarative CDI generator emits JSON instead of nvidia-ctk's default +# YAML. It receives the same stale-path validation before the launcher uses it. +cat >"$tmp/config/cdi/nvidia.yaml" <&2 + exit 1 +fi + +# The bridge listens in a directory the worker shares, as the same user, at the +# path the worker defaults to. +grep -Fxq -- '--userns=keep-id' "$tmp/podman.args" +grep -Fxq -- "$tmp/run/carla-bridge:$tmp/run/carla-bridge" "$tmp/podman.args" +grep -Fxq -- "-moq-bridge-socket=$tmp/run/carla-bridge/bridge.sock" "$tmp/podman.args" +[[ "$(stat -c %a "$tmp/run/carla-bridge")" == 700 ]] + +printf 'CARLA server lifecycle test: ok\n' diff --git a/demo/carla/sim/systemd/carla-sim.service b/demo/carla/sim/systemd/carla-sim.service new file mode 100644 index 00000000000..b03808429c5 --- /dev/null +++ b/demo/carla/sim/systemd/carla-sim.service @@ -0,0 +1,31 @@ +[Unit] +Description=CARLA simulator +Documentation=file://@CARLA_REPO@/demo/carla/README.md + +[Service] +Type=simple +Environment=CARLA_REPO=@CARLA_REPO@ +# Optional, and the same file the worker reads. CARLA_IMAGE belongs here rather +# than in a systemd drop-in, so reinstalling the units cannot silently return +# the simulator to the stock image and its idle game-thread spin. +EnvironmentFile=-%E/moq/carla.env +ExecStart=@NIX_BIN@ develop --accept-flake-config ${CARLA_REPO} --command just --justfile ${CARLA_REPO}/justfile carla server +# The server boots with rendering on and outlives every client, so a simulator +# waiting for its worker draws frames nobody asked for: 174 W and 100% GPU on +# an RTX 3070 Ti, against 26 W and 44% parked. `carla park` no-ops when a worker +# already owns the world. Both lines are `-`: this is an optimization, never a +# verdict on the simulator, and an unparked one still works. Failing the unit +# here would restart-loop the container on a slow boot instead. +ExecStartPost=-@NIX_BIN@ develop --accept-flake-config ${CARLA_REPO} --command just --justfile ${CARLA_REPO}/justfile carla wait-port 127.0.0.1 2000 600 +ExecStartPost=-@NIX_BIN@ develop --accept-flake-config ${CARLA_REPO} --command just --justfile ${CARLA_REPO}/justfile carla park +# just reports a recipe killed by SIGTERM as exit 143 rather than dying of +# the signal, which systemd would otherwise log as a failed stop. +SuccessExitStatus=143 +Restart=on-failure +RestartSec=10s +RestartSteps=4 +RestartMaxDelaySec=2min +# The gate above runs inside the start timeout, which defaults to 90s, and also +# covers the one-time 8.6 GB image pull. +TimeoutStartSec=6min +TimeoutStopSec=45s diff --git a/demo/carla/sim/systemd/carla-worker.service b/demo/carla/sim/systemd/carla-worker.service new file mode 100644 index 00000000000..3edbc45bc8c --- /dev/null +++ b/demo/carla/sim/systemd/carla-worker.service @@ -0,0 +1,31 @@ +[Unit] +Description=CARLA MoQ worker +Documentation=file://@CARLA_REPO@/demo/carla/README.md +Wants=carla-sim.service +After=carla-sim.service + +[Service] +Type=simple +Environment=CARLA_REPO=@CARLA_REPO@ +# MOQ_URL lives here, so a JWT in it stays off the command line. +EnvironmentFile=%E/moq/carla.env +# After= only orders the container's *start*; the simulator answers RPC seconds +# to a minute later. Without this gate the worker spends a 30s client timeout +# and a restart backoff on every cold boot. It also covers the one-time 8.6 GB +# image pull, up to the start timeout; Restart=on-failure retries if that +# expires anyway. +ExecStartPre=@NIX_BIN@ develop --accept-flake-config ${CARLA_REPO} --command just --justfile ${CARLA_REPO}/justfile carla wait-port 127.0.0.1 2000 600 +ExecStart=@NIX_BIN@ develop --accept-flake-config ${CARLA_REPO} --command just --justfile ${CARLA_REPO}/justfile carla worker +# just reports a recipe killed by SIGTERM as exit 143 rather than dying of +# the signal, which systemd would otherwise log as a failed stop. +SuccessExitStatus=143 +Restart=on-failure +RestartSec=10s +RestartSteps=4 +RestartMaxDelaySec=2min +# The gate above runs inside the start timeout, which defaults to 90s. +TimeoutStartSec=6min +TimeoutStopSec=30s + +[Install] +WantedBy=default.target diff --git a/demo/carla/sim/systemd/install-user-service.sh b/demo/carla/sim/systemd/install-user-service.sh new file mode 100755 index 00000000000..129ea0f4f3c --- /dev/null +++ b/demo/carla/sim/systemd/install-user-service.sh @@ -0,0 +1,52 @@ +#!/usr/bin/env bash +# Install the CARLA simulator and worker as rootless systemd user services. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo="$(git -C "$here" rev-parse --show-toplevel)" +unit_dir="${CARLA_SYSTEMD_USER_DIR:-${XDG_CONFIG_HOME:-$HOME/.config}/systemd/user}" +env_dir="${CARLA_CONFIG_DIR:-${XDG_CONFIG_HOME:-$HOME/.config}/moq}" +systemctl_bin="${SYSTEMCTL:-systemctl}" + +if [[ ! "$repo" =~ ^/[A-Za-z0-9._/-]+$ ]]; then + echo "service install: unsupported repository path: $repo" >&2 + exit 1 +fi +nix_bin="$(command -v nix)" || { + echo "service install: nix is not available in PATH" >&2 + exit 1 +} +if [[ ! "$nix_bin" =~ ^/[A-Za-z0-9._/-]+$ ]]; then + echo "service install: unsupported nix path: $nix_bin" >&2 + exit 1 +fi + +install -d -m 0700 "$unit_dir" "$env_dir" + +for name in carla-sim.service carla-worker.service; do + template="$(<"$here/$name")" + template="${template//@CARLA_REPO@/$repo}" + template="${template//@NIX_BIN@/$nix_bin}" + install -m 0600 /dev/null "$unit_dir/$name" + printf '%s\n' "$template" >"$unit_dir/$name" +done + +"$systemctl_bin" --user daemon-reload + +cat < + CARLA_IMAGE=localhost/moq-carla: + +The token is a demo-project JWT scoped to publish carla/** and subscribe +viewer/carla/**. For local development, MOQ_URL=http://localhost:4443 needs +no token. + +Then run: + just carla service-enable +EOF diff --git a/demo/carla/sim/systemd/install-user-service.test.sh b/demo/carla/sim/systemd/install-user-service.test.sh new file mode 100755 index 00000000000..fcd3de472dd --- /dev/null +++ b/demo/carla/sim/systemd/install-user-service.test.sh @@ -0,0 +1,84 @@ +#!/usr/bin/env bash +# Offline regression for the rootless CARLA systemd installer. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo="$(git -C "$here" rev-parse --show-toplevel)" +tmp="$(mktemp -d)" +trap 'rm -rf "$tmp"' EXIT + +mkdir -p "$tmp/bin" +cat >"$tmp/bin/systemctl" <<'EOF' +#!/usr/bin/env bash +set -euo pipefail +printf '%s\n' "$*" >"${CARLA_SYSTEMCTL_LOG:?}" +EOF +cat >"$tmp/bin/nix" <<'EOF' +#!/usr/bin/env bash +exit 1 +EOF +chmod +x "$tmp/bin/systemctl" "$tmp/bin/nix" +nix_bin="$tmp/bin/nix" + +space_repo="$tmp/repo space" +git init -q "$space_repo" +cp -R "$here" "$space_repo/systemd" +if CARLA_SYSTEMD_USER_DIR="$tmp/space-units" \ + CARLA_CONFIG_DIR="$tmp/space-config" \ + PATH="$tmp/bin:$PATH" \ + SYSTEMCTL="$tmp/bin/systemctl" \ + bash "$space_repo/systemd/install-user-service.sh" >"$tmp/space.out" 2>&1; then + echo "installer accepted a repository path that breaks unit syntax" >&2 + exit 1 +fi +grep -Fq 'unsupported repository path' "$tmp/space.out" + +CARLA_SYSTEMD_USER_DIR="$tmp/units" \ + CARLA_CONFIG_DIR="$tmp/config" \ + CARLA_SYSTEMCTL_LOG="$tmp/systemctl.log" \ + PATH="$tmp/bin:$PATH" \ + SYSTEMCTL="$tmp/bin/systemctl" \ + bash "$here/install-user-service.sh" >"$tmp/install.out" + +# The printed settings are what an operator copies: a scoped token for the +# hosted relay, the locally built image, and the tokenless local relay. +grep -Fxq ' MOQ_URL=https://cdn.moq.dev/demo?jwt=' "$tmp/install.out" +grep -Fxq ' CARLA_IMAGE=localhost/moq-carla:' "$tmp/install.out" +grep -Fq 'scoped to publish carla/** and subscribe' "$tmp/install.out" +grep -Fq 'viewer/carla/**. For local development, MOQ_URL=http://localhost:4443 needs' "$tmp/install.out" +test "$(stat -c %a "$tmp/units")" = 700 +test "$(stat -c %a "$tmp/config")" = 700 +test "$(stat -c %a "$tmp/units/carla-sim.service")" = 600 +test "$(stat -c %a "$tmp/units/carla-worker.service")" = 600 +grep -Fq "Environment=CARLA_REPO=$repo" "$tmp/units/carla-sim.service" +grep -Fq "Environment=CARLA_REPO=$repo" "$tmp/units/carla-worker.service" +grep -Fxq "ExecStart=$nix_bin develop --accept-flake-config \${CARLA_REPO} --command just --justfile \${CARLA_REPO}/justfile carla server" "$tmp/units/carla-sim.service" +grep -Fxq "ExecStart=$nix_bin develop --accept-flake-config \${CARLA_REPO} --command just --justfile \${CARLA_REPO}/justfile carla worker" "$tmp/units/carla-worker.service" +# The worker must not race the simulator's RPC port on a cold boot. +grep -Fxq "ExecStartPre=$nix_bin develop --accept-flake-config \${CARLA_REPO} --command just --justfile \${CARLA_REPO}/justfile carla wait-port 127.0.0.1 2000 600" "$tmp/units/carla-worker.service" +# A simulator waiting for its worker renders frames nobody asked for, so every +# start parks it. Both steps are optional: neither is a verdict on the server. +grep -Fxq "ExecStartPost=-$nix_bin develop --accept-flake-config \${CARLA_REPO} --command just --justfile \${CARLA_REPO}/justfile carla wait-port 127.0.0.1 2000 600" "$tmp/units/carla-sim.service" +grep -Fxq "ExecStartPost=-$nix_bin develop --accept-flake-config \${CARLA_REPO} --command just --justfile \${CARLA_REPO}/justfile carla park" "$tmp/units/carla-sim.service" +# The CARLA_IMAGE pin lives in the shared env file, so reinstalling the units +# cannot silently return the simulator to the stock image. +grep -Fq 'EnvironmentFile=-%E/moq/carla.env' "$tmp/units/carla-sim.service" +if grep -E '^ExecStartPost=[^-]' "$tmp/units/carla-sim.service"; then + echo "a parking step can fail the simulator unit into a restart loop" >&2 + exit 1 +fi +if grep -Fq '@CARLA_REPO@' "$tmp/units/carla-sim.service" || + grep -Fq '@NIX_BIN@' "$tmp/units/carla-sim.service" || + grep -Fq '@CARLA_REPO@' "$tmp/units/carla-worker.service" || + grep -Fq '@NIX_BIN@' "$tmp/units/carla-worker.service"; then + echo "installed unit retained a placeholder" >&2 + exit 1 +fi +grep -Fxq -- '--user daemon-reload' "$tmp/systemctl.log" + +if command -v systemd-analyze >/dev/null; then + systemd-analyze verify \ + "$tmp/units/carla-sim.service" "$tmp/units/carla-worker.service" +fi + +printf 'CARLA user service installer test: ok\n' diff --git a/demo/carla/sim/tests/conftest.py b/demo/carla/sim/tests/conftest.py new file mode 100644 index 00000000000..34ac3561510 --- /dev/null +++ b/demo/carla/sim/tests/conftest.py @@ -0,0 +1,109 @@ +"""Lightweight stand-ins for runtime-only modules used during unit collection.""" + +import sys +import uuid +from types import ModuleType, SimpleNamespace + +import pytest + + +class MoqError(Exception): + pass + + +class AnnounceEvent: + """One announcement event; the subclass names which kind, as in moq's.""" + + def __init__(self, announce: SimpleNamespace) -> None: + self.announce = announce + + +class AnnounceEventStart(AnnounceEvent): + pass + + +class AnnounceEventUpdate(AnnounceEvent): + pass + + +class AnnounceEventEnd(AnnounceEvent): + pass + + +class AnnounceEventRestart(AnnounceEvent): + pass + + +class SnapshotProducer: + """Records every published value; demand comes from the wrapped track.""" + + def __init__(self, broadcast: object, track: SimpleNamespace, *, compression: bool = False) -> None: + self.track = track + self.compression = compression + self.values: list[object] = [] + + def demand(self) -> object: + return self.track.demand() + + def update(self, value: object) -> None: + self.values.append(value) + + +class SnapshotConsumer: + """Yields whatever the wrapped fake track yields, already parsed.""" + + def __init__(self, track: object, *, compression: bool = False) -> None: + assert compression, "control is DEFLATE-compressed" + self._track = track + + def __aiter__(self): + return aiter(self._track) + + +moq = ModuleType("moq") +moq.Error = MoqError +moq.AnnounceEventStart = AnnounceEventStart +moq.AnnounceEventUpdate = AnnounceEventUpdate +moq.AnnounceEventEnd = AnnounceEventEnd +moq.AnnounceEventRestart = AnnounceEventRestart +moq.Route = lambda **kwargs: SimpleNamespace(**kwargs) +moq.mint_epoch = lambda: str(uuid.uuid4()) + +moq_json = ModuleType("moq.json") +moq_json.SnapshotProducer = SnapshotProducer +moq_json.SnapshotConsumer = SnapshotConsumer +moq.json = moq_json + +moq_media = ModuleType("moq.media") +moq_media.VideoFormat = SimpleNamespace(AVC3="avc3") +moq_media.VideoInit = lambda **kwargs: SimpleNamespace(**kwargs) +moq_media.VideoHint = lambda **kwargs: SimpleNamespace(**kwargs) +moq_media.Dimensions = lambda **kwargs: SimpleNamespace(**kwargs) +moq_media.Named = lambda name=None: SimpleNamespace(name=name) +moq.media = moq_media + +sys.modules["moq"] = moq +sys.modules["moq.json"] = moq_json +sys.modules["moq.media"] = moq_media + +carla = ModuleType("carla") +carla.Location = lambda **kwargs: SimpleNamespace(**kwargs) +# `carla.Transform()` with no arguments is the identity transform, which is how +# a sensor is attached at its parent actor's origin. +carla.Transform = lambda location=None, rotation=None, **_kwargs: SimpleNamespace(location=location, rotation=rotation) +carla.Vector3D = lambda **kwargs: SimpleNamespace(**kwargs) +carla.VehicleControl = lambda **kwargs: SimpleNamespace(**kwargs) +carla.LaneType = SimpleNamespace(Driving="driving") +sys.modules["carla"] = carla + + +@pytest.fixture(params=[False, True], ids=["ordinary", "deep-temp-root"]) +def bridge_socket(tmp_path, request: pytest.FixtureRequest, monkeypatch: pytest.MonkeyPatch) -> str: + # AF_UNIX bounds the path passed to bind/connect, not the resolved location. + # A relative filename works even under a deep devshell/pytest temporary root. + root = tmp_path + if request.param: + root = tmp_path / ("nested" * 20) # The component alone exceeds AF_UNIX's 108-byte Linux limit. + root.mkdir() + monkeypatch.chdir(root) + return "bridge.sock" diff --git a/demo/carla/sim/tests/test_aerial.py b/demo/carla/sim/tests/test_aerial.py new file mode 100644 index 00000000000..e8964204675 --- /dev/null +++ b/demo/carla/sim/tests/test_aerial.py @@ -0,0 +1,92 @@ +from __future__ import annotations + +import math +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest + +from moq_carla_sim.aerial import calibration, capture + + +def test_footprint_contains_roads_and_preserves_world_axes() -> None: + roads = [-114.2, -68.7, 110.0, 141.1] + camera, bbox = calibration(roads) + assert camera.x == pytest.approx(-2.1) + assert camera.y == pytest.approx(36.2) + assert camera.pitch == -90 + assert camera.yaw == -90 + assert bbox == pytest.approx([-144.2, -105.9, 140.0, 178.3]) + assert bbox[0] < roads[0] < roads[2] < bbox[2] + assert bbox[1] < roads[1] < roads[3] < bbox[3] + + # Independent ray projection: a down-facing camera's right/up vectors are + # (1, 0, 0)/(0, -1, 0) at yaw -90, keeping the road map's x/y orientation. + half_width = camera.z * math.tan(math.radians(camera.fov / 2)) + for x, y, expected in [(bbox[0], bbox[1], (0, 0)), (bbox[2], bbox[3], (1, 1))]: + pixel = ((x - camera.x) / (2 * half_width) + 0.5, (y - camera.y) / (2 * half_width) + 0.5) + assert pixel == pytest.approx(expected) + + +def test_ground_height_changes_altitude_without_changing_footprint() -> None: + baseline, footprint = calibration([0, 0, 100, 200], padding=10, fov=40) + elevated, elevated_footprint = calibration([0, 0, 100, 200], padding=10, ground_z=70, fov=40) + assert elevated.z == pytest.approx(baseline.z + 70) + assert elevated_footprint == footprint + assert (elevated.z - 70) * math.tan(math.radians(20)) == pytest.approx(110) + + +@pytest.mark.parametrize("bbox", [[], [0, 0, 0, 1], [0, 1, 1, 0], [0, 0, math.nan, 2]]) +def test_rejects_invalid_bounds(bbox: list[float]) -> None: + with pytest.raises(ValueError): + calibration(bbox) + + +@pytest.mark.parametrize("options", [{"padding": -1}, {"fov": 0}, {"fov": math.nan}, {"ground_z": math.inf}]) +def test_rejects_invalid_camera_options(options: dict[str, float]) -> None: + with pytest.raises(ValueError): + calibration([0, 0, 1, 1], **options) + + +def fake_world(monkeypatch: pytest.MonkeyPatch) -> tuple[Mock, Mock]: + import carla + + monkeypatch.setattr(carla, "Rotation", lambda **values: SimpleNamespace(**values), raising=False) + sensor = Mock(id=123) + sensor.destroy.return_value = True + world = Mock() + world.get_settings.return_value = SimpleNamespace(no_rendering_mode=False) + world.spawn_actor.return_value = sensor + return world, sensor + + +def test_capture_timeout_removes_only_its_sensor(monkeypatch: pytest.MonkeyPatch) -> None: + world, sensor = fake_world(monkeypatch) + camera, _ = calibration([0, 0, 10, 10]) + with pytest.raises(TimeoutError): + capture(world, camera, 256, 0.001) + sensor.stop.assert_called_once() + sensor.destroy.assert_called_once() + world.tick.assert_not_called() + world.apply_settings.assert_not_called() + world.get_actors.assert_not_called() + + +def test_listener_failure_still_destroys_sensor(monkeypatch: pytest.MonkeyPatch) -> None: + world, sensor = fake_world(monkeypatch) + sensor.listen.side_effect = RuntimeError("stream unavailable") + sensor.stop.side_effect = RuntimeError("stop unavailable") + camera, _ = calibration([0, 0, 10, 10]) + with pytest.raises(RuntimeError, match="stop unavailable"): + capture(world, camera, 256, 1) + sensor.destroy.assert_called_once() + + +def test_parked_renderer_does_not_spawn_or_change_settings(monkeypatch: pytest.MonkeyPatch) -> None: + world, _ = fake_world(monkeypatch) + world.get_settings.return_value.no_rendering_mode = True + camera, _ = calibration([0, 0, 10, 10]) + with pytest.raises(RuntimeError, match="rendering is parked"): + capture(world, camera, 256, 1) + world.spawn_actor.assert_not_called() + world.apply_settings.assert_not_called() diff --git a/demo/carla/sim/tests/test_benchmark.py b/demo/carla/sim/tests/test_benchmark.py new file mode 100644 index 00000000000..d1489d341fb --- /dev/null +++ b/demo/carla/sim/tests/test_benchmark.py @@ -0,0 +1,85 @@ +import time +from types import SimpleNamespace + +import pytest + +from moq_carla_sim.benchmark import Frames, main, measure, require_idle, summary + + +def test_delivery_uses_frame_identity_not_callback_count(): + frames = Frames() + started = time.monotonic() + for number in (9, 10, 10, 12, 13): + frames.receive(SimpleNamespace(frame=number)) + result = frames.finish(dict.fromkeys((10, 11, 12), started), time.monotonic()) + assert (result["delivered"], result["missing"], result["duplicates"]) == (2, [11], 1) + assert result["callback_delay_max_ms"] >= 0 + + +def test_camera_without_callbacks_reports_failure(): + result = Frames().finish({10: time.monotonic()}, time.monotonic()) + assert result["delivered"] == 0 + assert result["missing"] == [10] + assert result["callback_delay_p95_ms"] is None + + +def test_tick_rate_does_not_hide_tail_stalls(): + result = summary([0.01] * 19 + [0.1]) + assert result["tick_fps"] > 30 + assert result["over_30fps_budget"] == 1 + assert result["max_ms"] == 100 + assert result["p95_ms"] == 10 + + +@pytest.mark.parametrize("synchronous,actors", [(True, []), (False, [object()])]) +def test_refuses_an_owned_world(synchronous, actors): + world = SimpleNamespace( + get_settings=lambda: SimpleNamespace(synchronous_mode=synchronous), + wait_for_tick=lambda _: None, + get_actors=lambda: SimpleNamespace(filter=lambda _: actors), + ) + with pytest.raises(RuntimeError, match="benchmark requires"): + require_idle(world, 1) + + +def test_waits_for_actor_snapshot_before_accepting_empty_world(): + actors = [] + world = SimpleNamespace( + get_settings=lambda: SimpleNamespace(synchronous_mode=False), + wait_for_tick=lambda _: actors.append(object()), + get_actors=lambda: SimpleNamespace(filter=lambda _: actors), + ) + with pytest.raises(RuntimeError, match="empty server"): + require_idle(world, 1) + + +def test_spawn_failure_restores_clock_even_when_actor_cleanup_fails(): + events = [] + blueprint = SimpleNamespace(set_attribute=lambda *_: None) + + def spawn(*args, **kwargs): + if "attach_to" in kwargs: + raise RuntimeError("sensor spawn failed") + return SimpleNamespace( + id=1, set_simulate_physics=lambda _: None, destroy=lambda: events.append("destroy vehicle") or False + ) + + world = SimpleNamespace( + get_settings=lambda: SimpleNamespace(synchronous_mode=False, no_rendering_mode=True), + wait_for_tick=lambda _: None, + get_actors=lambda: SimpleNamespace(filter=lambda _: []), + apply_settings=lambda settings: events.append((settings.synchronous_mode, settings.no_rendering_mode)), + get_blueprint_library=lambda: SimpleNamespace(find=lambda _: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [object()]), + spawn_actor=spawn, + ) + with pytest.raises(RuntimeError, match="failed to destroy"): + measure(SimpleNamespace(get_world=lambda: world), 1, 320, 180, 1, 1, 1, True) + assert events == [(True, False), "destroy vehicle", (False, True)] + + +@pytest.mark.parametrize("args", [["--ticks", "0"], ["--timeout", "nan"], ["--sizes", "1280"], ["--cameras", "-1"]]) +def test_rejects_invalid_inputs_before_connecting(args): + with pytest.raises(SystemExit) as error: + main(args) + assert error.value.code == 2 diff --git a/demo/carla/sim/tests/test_bridge.py b/demo/carla/sim/tests/test_bridge.py new file mode 100644 index 00000000000..33c1566eb96 --- /dev/null +++ b/demo/carla/sim/tests/test_bridge.py @@ -0,0 +1,385 @@ +import asyncio +import struct +from datetime import timedelta +from types import SimpleNamespace + +import moq +import pytest + +from moq_carla_sim import bridge +from moq_carla_sim.bridge import ( + Bridge, + BridgeError, + Config, + Error, + Frame, + Hello, + Rendition, + decode_event, + encode_close, + encode_demand, + encode_hello, + encode_keyframe, + encode_open, + read_event, +) + +# Shared with demo/carla/bridge/src/protocol.rs: both sides must produce and accept +# exactly these bytes. +HELLO_HEX = "03000000010100" +OPEN_HEX = "2c00000002070000000c00747275636b312f66726f6e740102007364aa010000f000000000350c00000000001e000000" +DEMAND_HEX = "06000000030700000002" +KEYFRAME_HEX = "06000000040700000001" +CLOSE_HEX = "050000000507000000" +EVENT_HELLO_HEX = "03000000810100" +CONFIG_HEX = "0f000000820700000000050000000000000167" +FRAME_HEX = "20000000830700000001d204000000000000404b4c000000000001050000000000000165" +ERROR_HEX = "0b00000084000000000400626f6f6d" + +SD = Rendition("sd", 426, 240, 800_000, 30) +HD = Rendition("hd", 1280, 720, 4_000_000, 30) + + +def test_requests_match_the_shared_vectors() -> None: + assert encode_hello(1).hex() == HELLO_HEX + assert encode_open(7, "truck1/front", [SD]).hex() == OPEN_HEX + assert encode_demand(7, 0b10).hex() == DEMAND_HEX + assert encode_keyframe(7, 1).hex() == KEYFRAME_HEX + assert encode_close(7).hex() == CLOSE_HEX + + +def _decode(hex: str): + wire = bytes.fromhex(hex) + (length,) = struct.unpack(" None: + assert _decode(EVENT_HELLO_HEX) == Hello(1) + assert _decode(CONFIG_HEX) == Config(7, 0, bytes([0, 0, 0, 1, 0x67])) + assert _decode(FRAME_HEX) == Frame(7, 1, 1234, 5_000_000, True, bytes([0, 0, 0, 1, 0x65])) + assert _decode(ERROR_HEX) == Error(0, "boom") + + +@pytest.mark.parametrize( + "hex", + [ + "0400000083070000", # truncated frame + "0400000081010000", # trailing bytes after a hello + "0100000099", # unknown kind + # keyframe flag of 2 + "20000000830700000001d204000000000000404b4c000000000002050000000000000165", + ], +) +def test_rejects_malformed_events(hex: str) -> None: + with pytest.raises(BridgeError): + _decode(hex) + + +def _reader(data: bytes) -> asyncio.StreamReader: + reader = asyncio.StreamReader() + reader.feed_data(data) + reader.feed_eof() + return reader + + +def test_read_event_distinguishes_a_clean_end_from_a_torn_message() -> None: + async def scenario() -> None: + assert await read_event(_reader(b"")) is None + with pytest.raises(BridgeError): + await read_event(_reader(bytes.fromhex(FRAME_HEX)[:10])) + with pytest.raises(BridgeError): + await read_event(_reader(bytes.fromhex("00000000"))) + assert await read_event(_reader(bytes.fromhex(ERROR_HEX))) == Error(0, "boom") + + asyncio.run(scenario()) + + +class FakeDemand: + """`moq.TrackDemand`: the first `wanted_changed` returns at once, later ones wait for a change.""" + + def __init__(self) -> None: + self._wanted: int | None = None + self._fresh = True + self._changed = asyncio.Event() + + def want(self, group: int | None) -> None: + self._wanted = group + self._fresh = True + self._changed.set() + + async def wanted_changed(self) -> int | None: + if not self._fresh: + self._changed.clear() + await self._changed.wait() + self._fresh = False + return self._wanted + + +def microseconds(timestamp: timedelta) -> int: + return timestamp // timedelta(microseconds=1) + + +class FakeTrack: + def __init__(self, init: bytes, name: str, hint) -> None: + self.init = init + self.name = name + self.hint = hint + self.frames: list[tuple[bytes, int]] = [] + self.flushed: list[int] = [] + self.finished = False + self.demand_handle = FakeDemand() + + def demand(self) -> FakeDemand: + return self.demand_handle + + def write_frame(self, payload: bytes, timestamp: timedelta) -> None: + self.frames.append((payload, microseconds(timestamp))) + + def flush(self, timestamp: timedelta) -> None: + self.flushed.append(microseconds(timestamp)) + + def finish(self) -> None: + self.finished = True + + +class FakeBroadcast: + def __init__(self) -> None: + self.tracks: dict[str, FakeTrack] = {} + + def video(self, init, target) -> FakeTrack: + assert init.format == "avc3" + self.tracks[target.name] = FakeTrack(init.data, target.name, init.hint) + return self.tracks[target.name] + + +@pytest.fixture(autouse=True) +def media_tracks(monkeypatch: pytest.MonkeyPatch) -> None: + def video(broadcast: FakeBroadcast, init, *, target) -> FakeTrack: + return broadcast.video(init, target) + + monkeypatch.setattr(moq.media, "TrackProducer", SimpleNamespace(video=video), raising=False) + + +def _message(kind: int, body: bytes) -> bytes: + return struct.pack(" bytes: + return _message(0x82, struct.pack(" bytes: + body = struct.pack(" tuple[int, bytes]: + (length,) = struct.unpack(" None: + self.connected: asyncio.Future[tuple[asyncio.StreamReader, asyncio.StreamWriter]] = ( + asyncio.get_running_loop().create_future() + ) + + async def serve(self, reader: asyncio.StreamReader, writer: asyncio.StreamWriter) -> None: + kind, body = await _request(reader) + assert (kind, body) == (0x01, struct.pack(" tuple[Bridge, asyncio.StreamReader, asyncio.StreamWriter, asyncio.Server]: + fake = FakeBridge() + path = bridge_socket + server = await asyncio.start_unix_server(fake.serve, path) + client = await Bridge.connect(path) + reader, writer = await fake.connected + return client, reader, writer, server + + +def test_stream_advertises_publishes_and_forwards_demand(bridge_socket) -> None: + async def scenario() -> None: + client, reader, writer, server = await _connected(bridge_socket) + broadcast = FakeBroadcast() + async with asyncio.TaskGroup() as group: + run = group.create_task(client.run()) + opening = group.create_task(client.open("truck1/front", [SD, HD], broadcast)) + + kind, body = await _request(reader) + assert _message(kind, body) == encode_open(1, "truck1/front", [SD, HD]) + writer.write(_config(1, 0, b"\x00\x00\x00\x01\x67sd")) + writer.write(_config(1, 1, b"\x00\x00\x00\x01\x67hd")) + stream = await opening + + # Both tracks exist, with their parameter sets, before any frame. + assert broadcast.tracks["sd"].init.endswith(b"sd") + assert broadcast.tracks["hd"].hint.coded.width == 1280 + assert broadcast.tracks["hd"].hint.bitrate == 4_000_000 + + demand = group.create_task(stream.watch_demand()) + broadcast.tracks["hd"].demand_handle.want(0) + assert await _request(reader) == (0x03, struct.pack(" None: + async def scenario() -> None: + client, _reader, writer, server = await _connected(bridge_socket) + writer.close() + with pytest.raises(BridgeError, match="closed"): + await client.run() + server.close() + + asyncio.run(scenario()) + + +def test_frames_for_unknown_streams_are_protocol_errors(bridge_socket) -> None: + async def scenario() -> None: + client, _reader, writer, server = await _connected(bridge_socket) + writer.write(_frame(9, 0, 1, 0, True, b"x")) + with pytest.raises(BridgeError, match="unknown stream"): + await client.run() + server.close() + + asyncio.run(scenario()) + + +def test_an_open_racing_a_bridge_error_fails_instead_of_hanging(bridge_socket) -> None: + async def scenario() -> None: + client, reader, writer, server = await _connected(bridge_socket) + run = asyncio.create_task(client.run()) + opening = asyncio.create_task(client.open("truck1/front", [SD, HD], FakeBroadcast())) + assert (await _request(reader))[0] == 0x02 + # The bridge fails before it sends the stream's parameter sets. + writer.write(_message(0x84, struct.pack(" None: + async def scenario() -> None: + client, reader, writer, server = await _connected(bridge_socket) + run = asyncio.create_task(client.run()) + opening = asyncio.create_task(client.open("truck1/front", [SD], FakeBroadcast())) + assert (await _request(reader))[0] == 0x02 + writer.write(_config(1, 0, b"\x00\x00\x00\x01\x67sd")) + stream = await opening + + stream.finish() + stream.finish() + assert await _request(reader) == (0x05, struct.pack(" None: + async def scenario() -> None: + client, reader, writer, server = await _connected(bridge_socket) + run = asyncio.create_task(client.run()) + broadcast = FakeBroadcast() + opening = asyncio.create_task(client.open("truck1/front", [SD, HD], broadcast)) + assert (await _request(reader))[0] == 0x02 + writer.write(_config(1, 0, b"\x00\x00\x00\x01\x67sd")) + writer.write(_config(1, 1, b"\x00\x00\x00\x01\x67hd")) + stream = await opening + + # The simulator renders nothing in no-rendering mode, so the world is + # rendering before the bridge is asked for a picture, and stays so + # until the bridge has been told nobody wants one. + order: list[str] = [] + sent = client._send + client._send = lambda message: (order.append(f"demand {message[-1]:02b}"), sent(message)) + demand = asyncio.create_task(stream.watch_demand(lambda wanted: order.append(f"render {wanted}"))) + sd, hd = broadcast.tracks["sd"].demand_handle, broadcast.tracks["hd"].demand_handle + sd.want(0) + hd.want(0) + await _request(reader) + await _request(reader) + # A subscriber moving on to the next group is still demand. + sd.want(1) + hd.want(None) + await _request(reader) + sd.want(None) + await _request(reader) + assert order == [ + "render True", + "demand 01", + "demand 11", + "demand 01", + "demand 00", + "render False", + ] + demand.cancel() + run.cancel() + await asyncio.gather(demand, run, return_exceptions=True) + server.close() + + asyncio.run(scenario()) diff --git a/demo/carla/sim/tests/test_drive.py b/demo/carla/sim/tests/test_drive.py new file mode 100644 index 00000000000..8a2b96a9142 --- /dev/null +++ b/demo/carla/sim/tests/test_drive.py @@ -0,0 +1,234 @@ +"""The control safety gate's contract.""" + +from moq_carla_sim.drive import Command, Drive, DriveState, Gear + + +def drive(driver_id: str, sequence: int, throttle: float) -> Command: + return Command( + kind="drive", + driver_id=driver_id, + sequence=sequence, + state=DriveState(throttle=throttle, brake=0.0), + ) + + +def estop(driver_id: str, sequence: int) -> Command: + return Command(kind="estop", driver_id=driver_id, sequence=sequence) + + +def reset(driver_id: str, sequence: int) -> Command: + return Command(kind="reset_estop", driver_id=driver_id, sequence=sequence) + + +def test_first_driver_keeps_control() -> None: + state = Drive() + assert state.apply(drive("first", 1, 0.5)) + assert not state.apply(drive("second", 2, 1.0)) + assert state.driver == "first" + assert state.state.throttle == 0.5 + + +def test_the_lease_belongs_to_one_run_of_a_driver() -> None: + """A driver id announced under another epoch is another driver: it cannot + steer while the first run holds the lease, nor release it.""" + state = Drive() + first = Command(kind="drive", driver_id="abc", epoch="e1", sequence=1, state=DriveState(throttle=0.5, brake=0.0)) + second = Command(kind="drive", driver_id="abc", epoch="e2", sequence=1, state=DriveState(throttle=1.0, brake=0.0)) + assert state.apply(first) + assert not state.apply(second) + assert state.state.throttle == 0.5 + assert not state.apply(Command(kind="driver_left", driver_id="abc", epoch="e2")) + assert state.driver == "abc" + + # The first run leaving frees the lease for the second. + assert state.apply(Command(kind="driver_left", driver_id="abc", epoch="e1")) + assert state.driver is None + assert state.apply(second) + assert state.holds(second) + assert state.driver == "abc" + + +def test_estop_latches_until_explicit_reset() -> None: + state = Drive() + assert state.apply(drive("driver", 1, 1.0)) + assert state.apply(estop("driver", 2)) + assert state.estopped + assert not state.apply(drive("driver", 3, 1.0)) + assert state.state == DriveState.stopped() + assert state.apply(reset("driver", 4)) + assert not state.estopped + assert state.state == DriveState.stopped() + + +def test_watchdog_stops_and_releases_driver() -> None: + state = Drive() + assert state.apply(drive("driver", 1, 1.0)) + assert state.apply(Command(kind="watchdog_expired")) + assert state.driver is None + assert state.sequence is None + assert state.state == DriveState.stopped() + + +def test_disconnect_does_not_clear_estop() -> None: + state = Drive() + assert state.apply(drive("driver", 1, 1.0)) + assert state.apply(estop("driver", 2)) + assert state.estopped + assert state.driver == "driver" + + # The driver disconnects: the lease releases but the latch must persist, so + # the truck stays stopped until an explicit reset. + assert state.apply(Command(kind="driver_left", driver_id="driver")) + assert state.estopped + assert state.driver is None + + +def test_estop_from_a_bystander_latches_without_transferring_the_lease() -> None: + state = Drive() + assert state.apply(drive("first", 1, 0.5)) + + # Anyone may hit the big red button, so the latch must engage... + assert state.apply(estop("second", 2)) + assert state.estopped + assert state.estop_by == "second" + + # ...but the lease stays with "first", and the bystander's sequence is not + # recorded as the authoritative command. + assert state.driver == "first" + assert state.sequence == 1 + + # The holder heartbeat is acknowledged without replacing the actor that + # originally latched the stop. + assert state.apply(estop("first", 3)) + assert state.estop_by == "second" + assert state.sequence == 3 + + # Anyone may clear the global latch, but doing so neither transfers the + # lease nor records the bystander's sequence as authoritative. + assert state.apply(reset("second", 4)) + assert not state.estopped + assert state.estop_by is None + assert state.driver == "first" + assert state.sequence == 3 + assert not state.apply(drive("second", 5, 1.0)) + + +def test_estop_from_holder_refreshes_the_acknowledgement_sequence() -> None: + state = Drive() + assert state.apply(drive("holder", 5, 0.3)) + assert state.sequence == 5 + + # The holder's own e-stop is acknowledged (sequence advances) and it keeps + # the lease. + assert state.apply(estop("holder", 6)) + assert state.estopped + assert state.sequence == 6 + assert state.driver == "holder" + + +def test_axes_are_clamped_into_range() -> None: + state = Drive() + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=1, + state=DriveState(steering=-4.0, throttle=9.0, brake=-1.0, gear=Gear.FORWARD), + ) + ) + accepted = state.state + assert accepted.steering == -1.0 + assert accepted.throttle == 1.0 + assert accepted.brake == 0.0 + # Clamping happens before the interlock, so invalid pedal values cannot + # smuggle a gear change through. + assert accepted.gear is Gear.NEUTRAL + + +def test_unclaimed_gate_can_synchronize_with_vehicle_feedback() -> None: + state = Drive() + measured = DriveState(steering=-0.4, throttle=0.6, brake=0.2, gear=Gear.REVERSE) + + assert state.synchronize(measured) + assert state.state == measured + assert state.apply(Command(kind="drive", driver_id="driver", sequence=1, state=measured)) + assert state.state == measured + + assert not state.synchronize(DriveState.stopped()) + assert state.state == measured + + +def test_new_lease_can_initialize_a_zero_pedal_forward_takeover() -> None: + state = Drive() + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=1, + state=DriveState(throttle=0.0, brake=0.0, gear=Gear.FORWARD), + ) + ) + assert state.state == DriveState(throttle=0.0, brake=0.0, gear=Gear.FORWARD) + + # Once the lease exists, zero pedals cannot bypass the normal shift gate. + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=2, + state=DriveState(throttle=0.0, brake=0.0, gear=Gear.REVERSE), + ) + ) + assert state.state.gear is Gear.FORWARD + + +def test_gear_change_requires_brake_and_released_throttle() -> None: + state = Drive() + + # Establish the lease without invoking CARLA's one-time forward takeover sync. + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=1, + state=DriveState(throttle=0.0, brake=0.0, gear=Gear.NEUTRAL), + ) + ) + + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=2, + state=DriveState(throttle=0.0, brake=0.0, gear=Gear.FORWARD), + ) + ) + assert state.state.gear is Gear.NEUTRAL + + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=3, + state=DriveState(throttle=0.01, brake=0.001, gear=Gear.FORWARD), + ) + ) + assert state.state.gear is Gear.FORWARD + + assert state.apply( + Command( + kind="drive", + driver_id="driver", + sequence=4, + state=DriveState(throttle=0.02, brake=1.0, gear=Gear.REVERSE), + ) + ) + assert state.state.gear is Gear.FORWARD + + +def test_resting_state_is_stopped_with_the_brake_on() -> None: + # A truck nobody is driving must hold the brake rather than coast. + resting = Drive() + assert resting.driver is None + assert resting.state == DriveState(steering=0.0, throttle=0.0, brake=1.0, gear=Gear.NEUTRAL) + assert not resting.estopped diff --git a/demo/carla/sim/tests/test_handback.py b/demo/carla/sim/tests/test_handback.py new file mode 100644 index 00000000000..4e6edd574cd --- /dev/null +++ b/demo/carla/sim/tests/test_handback.py @@ -0,0 +1,149 @@ +"""The lane-merge a released truck performs before Traffic Manager takes it. + +Geometry in CARLA's frame: x forward, y to the right, steer positive right. +""" + +from __future__ import annotations + +import math +from types import SimpleNamespace + +from moq_carla_sim import handback + + +def vector(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> SimpleNamespace: + return SimpleNamespace(x=x, y=y, z=z) + + +def lane( + x: float, + y: float, + yaw: float = 0.0, + *, + left=None, + right=None, + lane_type: str = "driving", + junction: bool = False, +) -> SimpleNamespace: + """A waypoint on a straight lane through (x, y) heading `yaw` degrees.""" + forward = vector(math.cos(math.radians(yaw)), math.sin(math.radians(yaw))) + return SimpleNamespace( + transform=SimpleNamespace(location=vector(x, y), get_forward_vector=lambda: forward), + lane_type=lane_type, + is_junction=junction, + get_left_lane=lambda: left, + get_right_lane=lambda: right, + next=lambda distance: [lane(x + forward.x * distance, y + forward.y * distance, yaw)], + ) + + +def road_map(nearest) -> SimpleNamespace: + return SimpleNamespace(get_waypoint=lambda _location, **_kwargs: nearest) + + +def transform(x: float, y: float, yaw: float = 0.0) -> SimpleNamespace: + forward = vector(math.cos(math.radians(yaw)), math.sin(math.radians(yaw))) + return SimpleNamespace(location=vector(x, y), get_forward_vector=lambda: forward) + + +def test_aligned_lane_prefers_the_lane_running_the_trucks_way_over_the_nearest() -> None: + # Two-way road: the nearest lane is oncoming, its neighbour across the + # centre line runs with the truck. + own = lane(0.0, 3.5, 0.0) + oncoming = lane(0.0, 0.0, 180.0, right=own) + + picked = handback.aligned_lane(road_map(oncoming), vector(0.0, 0.5), vector(1.0, 0.0)) + + assert picked is own + + +def test_aligned_lane_stops_walking_at_a_non_driving_lane() -> None: + shoulder = lane(0.0, -3.5, 0.0, lane_type="shoulder") + own = lane(0.0, 0.0, 90.0, left=shoulder) + + assert handback.aligned_lane(road_map(own), vector(), vector(1.0, 0.0)) is own + + +def test_centred_and_aligned_truck_is_handed_over() -> None: + control = handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, 0.3, 4.0), vector(10.0), 0.2, reversing=False) + assert control is None + + +def test_offset_truck_steers_toward_the_lane_side() -> None: + # Lane runs +x through y=0; the truck sits 2 m to its left (-y), so the + # lane is to its right: positive steer, and the mirror case negative. + right = handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, -2.0), vector(10.0), 0.0, reversing=False) + left = handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, 2.0), vector(10.0), 0.0, reversing=False) + + assert 0.0 < right.steer <= 1.0 + assert left.steer == -right.steer + assert right.throttle == 0.0 and right.brake == 0.0 + + +def test_angled_truck_on_the_centre_line_steers_back() -> None: + # Centred but pointing 30 degrees left of the lane: steer right, gently. + control = handback.plan( + road_map(lane(0.0, 0.0)), transform(0.0, 0.0, -30.0), vector(8.7, -5.0), 0.0, reversing=False + ) + + assert 0.0 < control.steer < 0.5 + + +def test_junction_is_coasted_through_on_the_drivers_steering() -> None: + control = handback.plan( + road_map(lane(0.0, 0.0, junction=True)), transform(0.0, 3.0, 45.0), vector(7.0, 7.0), -0.4, reversing=False + ) + + assert control.steer == -0.4 + assert control.throttle == 0.0 and control.brake == 0.0 + + +def test_junction_too_close_ahead_is_coasted_toward_on_the_drivers_steering() -> None: + # The lane forks 4 m ahead: any fork is an arbitrary exit, so merging + # toward it could be the very swerve this avoids. + forking = lane(0.0, 0.0) + forking.next = lambda distance: [lane(distance, 0.0), lane(distance, 0.0, 45.0)] + + control = handback.plan(road_map(forking), transform(0.0, 2.0), vector(10.0), 0.3, reversing=False) + + assert control.steer == 0.3 + + +def test_target_stops_short_of_a_junction_far_enough_ahead() -> None: + # Straight lane for 12 m, then a junction: the merge aims at 12 m, not 20. + straight = lane(0.0, 0.0) + + def stepped(distance: float): + point = lane(distance, 0.0) + point.is_junction = distance > 12.0 + point.next = lambda step: [stepped(distance + step)] + return point + + straight.next = lambda distance: [stepped(distance)] + near = handback.plan(road_map(straight), transform(0.0, 2.0), vector(10.0), 0.0, reversing=False) + far = handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, 2.0), vector(10.0), 0.0, reversing=False) + + assert near.steer < 0.0 and far.steer < 0.0 + assert near.steer < far.steer + + +def test_walking_pace_is_handed_over_wherever_it_is() -> None: + assert handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, 5.0, 90.0), vector(0.9), 0.0, reversing=False) is None + + +def test_reversing_truck_brakes_to_a_stop_first() -> None: + control = handback.plan(road_map(lane(0.0, 0.0)), transform(0.0, 0.0), vector(-4.0), 0.1, reversing=True) + + assert control.brake == 1.0 + assert control.reverse is True + + +def test_pursuit_steer_is_zero_straight_ahead_signed_by_side_and_clamped() -> None: + at = vector() + forward = vector(1.0, 0.0) + assert handback.pursuit_steer(at, forward, vector(10.0, 0.0)) == 0.0 + assert handback.pursuit_steer(at, forward, vector(10.0, 3.0)) > 0.0 + assert handback.pursuit_steer(at, forward, vector(10.0, -3.0)) < 0.0 + # A target almost beside the truck asks for more lock than it has. + assert handback.pursuit_steer(at, forward, vector(1.0, 5.0)) == 1.0 + assert handback.pursuit_steer(at, forward, vector(1.0, -5.0)) == -1.0 diff --git a/demo/carla/sim/tests/test_main.py b/demo/carla/sim/tests/test_main.py new file mode 100644 index 00000000000..8ccad470e98 --- /dev/null +++ b/demo/carla/sim/tests/test_main.py @@ -0,0 +1,232 @@ +import asyncio +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from moq_carla_sim import main as main_module +from moq_carla_sim.main import broadcast_path, default_trucks, driver_prefix, parse_args +from moq_carla_sim.watchdog import Heartbeat + + +def test_default_trucks_are_the_checked_in_list() -> None: + expected = json.loads(main_module.TRUCKS_FILE.read_text()) + assert expected + assert parse_args(["--url", "http://localhost:4443"]).trucks == expected + + +def test_default_trucks_refuses_a_malformed_list(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None: + for body in ("[]", '["truck1", ""]', '{"trucks": ["truck1"]}'): + path = tmp_path / "trucks.json" + path.write_text(body) + monkeypatch.setattr(main_module, "TRUCKS_FILE", path) + with pytest.raises(ValueError, match="non-empty list of truck names"): + default_trucks() + + +def test_relay_url_defaults_from_environment(monkeypatch: pytest.MonkeyPatch) -> None: + monkeypatch.setenv("MOQ_URL", "https://relay.example.test/demo?jwt=secret") + assert parse_args([]).url == "https://relay.example.test/demo?jwt=secret" + + +def test_relay_url_defaults_to_the_local_relay(monkeypatch: pytest.MonkeyPatch) -> None: + monkeypatch.delenv("MOQ_URL", raising=False) + assert parse_args([]).url == "http://localhost:4443" + + +def test_paths_match_the_player() -> None: + assert broadcast_path("truck1", "front") == "carla/truck1/front.hang" + assert driver_prefix("truck1") == "viewer/carla/truck1" + + +def test_log_level_configures_python_and_rust(monkeypatch: pytest.MonkeyPatch) -> None: + levels = [] + + async def fake_run(args: SimpleNamespace) -> None: + levels.append(args.log_level) + + monkeypatch.setattr(main_module.moq, "log_level", levels.append, raising=False) + monkeypatch.setattr(main_module, "run", fake_run) + + assert main_module.main(["--url", "http://localhost:4443", "--log-level", "debug"]) == 0 + assert levels == ["debug", "debug"] + + +def test_bridge_socket_defaults_to_where_the_server_puts_it(monkeypatch: pytest.MonkeyPatch) -> None: + monkeypatch.delenv("CARLA_BRIDGE_SOCKET", raising=False) + monkeypatch.setenv("XDG_RUNTIME_DIR", "/run/user/1000") + assert parse_args(["--url", "x"]).bridge_socket == "/run/user/1000/carla-bridge/bridge.sock" + monkeypatch.setenv("CARLA_BRIDGE_SOCKET", "/elsewhere.sock") + assert parse_args(["--url", "x"]).bridge_socket == "/elsewhere.sock" + + +def test_a_missing_bridge_says_how_the_simulator_must_run(bridge_socket) -> None: + with pytest.raises(RuntimeError, match="-moq-bridge-socket"): + asyncio.run(main_module.connect_bridge(bridge_socket)) + + +async def never_fails() -> None: + await asyncio.Event().wait() + + +def supervise_with(closed, clock_failed=never_fails) -> tuple[BaseException | None, asyncio.Task]: + """Run `supervise` against a truck that never ends and a session that does. + + Returns whatever ended the run plus the truck task, so a test can assert it + was cancelled and awaited rather than left to loop teardown. + """ + + async def scenario() -> tuple[BaseException | None, asyncio.Task]: + client = SimpleNamespace(session=SimpleNamespace(closed=closed)) + clock = SimpleNamespace(wait_failed=clock_failed) + watchdog = SimpleNamespace(pulse=never_fails) + truck = asyncio.create_task(asyncio.Event().wait(), name="truck:truck1") + try: + await asyncio.wait_for(main_module.supervise(client, [truck], clock, watchdog), timeout=1.0) + except BaseException as err: # noqa: BLE001 - the test asserts on it + return err, truck + return None, truck + + return asyncio.run(scenario()) + + +def test_dead_session_ends_the_run() -> None: + """A session that stops for good must fail the process, not be ignored. + + The trucks keep running against a dead session: their tracks accept frames + and no task ever finishes, so without this the relay announces nothing while + systemd sees a healthy worker. + """ + + async def closed() -> None: + return None + + err, truck = supervise_with(closed) + assert isinstance(err, RuntimeError) + assert "relay session closed" in str(err) + assert truck.cancelled() + + +def test_dead_session_surfaces_the_transport_error() -> None: + async def closed() -> None: + raise main_module.moq.Error("UnexpectedEnd") + + err, _ = supervise_with(closed) + assert isinstance(err, main_module.moq.Error) + assert "UnexpectedEnd" in str(err) + + +def test_stopped_clock_ends_the_run() -> None: + """A world nobody ticks is frozen, and nothing else would notice.""" + + async def closed() -> None: + await asyncio.Event().wait() + + async def clock_failed() -> None: + raise RuntimeError("simulation clock stopped") + + err, truck = supervise_with(closed, clock_failed) + assert isinstance(err, RuntimeError) + assert "simulation clock stopped" in str(err) + assert truck.cancelled() + + +def test_second_worker_on_the_same_simulator_is_refused(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + + first = main_module.claim_simulator("127.0.0.1", 2000) + with pytest.raises(RuntimeError, match="another worker already owns the simulator at 127.0.0.1:2000"): + main_module.claim_simulator("127.0.0.1", 2000) + # A different simulator is a different world. + main_module.claim_simulator("127.0.0.1", 2001).close() + + first.close() + main_module.claim_simulator("127.0.0.1", 2000).close() + + +def test_sigterm_unwinds_like_ctrl_c(monkeypatch: pytest.MonkeyPatch) -> None: + """Before the loop's handlers exist, SIGTERM must reach `run`'s cleanup.""" + handlers = {} + monkeypatch.setattr(main_module.signal, "signal", lambda signum, handler: handlers.__setitem__(signum, handler)) + monkeypatch.setattr(main_module.moq, "log_level", lambda _level: None, raising=False) + + async def fake_run(_args) -> None: + handlers[main_module.signal.SIGTERM](main_module.signal.SIGTERM, None) + + monkeypatch.setattr(main_module, "run", fake_run) + + assert main_module.main(["--url", "http://localhost:4443"]) == 0 + + +def test_actors_are_destroyed_before_the_relay_session_closes(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + """The actors outlive the process; a relay close that hangs must not stand between them and cleanup.""" + events: list[str] = [] + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + + class FakeWorld: + trucks: list = [] + + def __init__(self) -> None: + self.clock = SimpleNamespace(start=lambda: events.append("clock.start"), heartbeat=Heartbeat("clock")) + + def sweep(self) -> None: + events.append("sweep") + + def spawn(self, _names, _camera, _camera_name) -> None: + events.append("spawn") + + def map_name(self) -> str: + return "Town" + + def ready(self) -> None: + events.append("ready") + + def destroy(self) -> None: + events.append("destroy") + + class FakeConnect: + async def __aenter__(self): + events.append("session.open") + return SimpleNamespace() + + async def __aexit__(self, *_exc) -> None: + events.append("session.close") + + async def fake_supervise(_client, _tasks, _clock, _watchdog) -> None: + events.append("supervise") + + class FakeBridge: + async def run(self) -> None: + await asyncio.Event().wait() + + def disconnect(self) -> None: + events.append("bridge.disconnect") + + async def fake_connect_bridge(_path: str) -> FakeBridge: + events.append("bridge.connect") + return FakeBridge() + + monkeypatch.setattr(main_module.World, "connect", staticmethod(lambda *_args: FakeWorld())) + monkeypatch.setattr(main_module, "connect_bridge", fake_connect_bridge) + monkeypatch.setattr(main_module.moq, "connect", lambda _url: FakeConnect(), raising=False) + monkeypatch.setattr(main_module, "supervise", fake_supervise) + + asyncio.run(main_module.run(parse_args(["--url", "http://localhost:4443"]))) + + assert events == [ + "clock.start", + "sweep", + "spawn", + # The clock is only allowed to park once every truck is in the world: + # `sweep` and `spawn` both wait on ticks. + "ready", + # The cameras are in the world, so the simulator's bridge is listening. + "bridge.connect", + "session.open", + "supervise", + "destroy", + "session.close", + "bridge.disconnect", + "destroy", + ] diff --git a/demo/carla/sim/tests/test_park.py b/demo/carla/sim/tests/test_park.py new file mode 100644 index 00000000000..b47d5dc4903 --- /dev/null +++ b/demo/carla/sim/tests/test_park.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from moq_carla_sim import park as park_module + + +def fake_world(no_rendering: bool) -> SimpleNamespace: + settings = SimpleNamespace(no_rendering_mode=no_rendering) + world = SimpleNamespace( + settings=settings, + applied=[], + get_settings=lambda: settings, + ) + world.apply_settings = lambda value: world.applied.append(value.no_rendering_mode) + return world + + +def patch_client(monkeypatch: pytest.MonkeyPatch, world: SimpleNamespace) -> None: + monkeypatch.setattr(park_module, "connect_client", lambda *_args: SimpleNamespace(get_world=lambda: world)) + + +def test_park_disables_rendering_on_an_unowned_simulator(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + world = fake_world(no_rendering=False) + patch_client(monkeypatch, world) + + assert park_module.park("127.0.0.1", 2000, 30.0) + assert world.applied == [True] + + +def test_park_leaves_a_worker_owned_simulator_alone(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + """Parking under a live worker would black out a viewer mid-stream.""" + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + world = fake_world(no_rendering=False) + patch_client(monkeypatch, world) + + from moq_carla_sim.main import claim_simulator + + worker = claim_simulator("127.0.0.1", 2000) + try: + assert not park_module.park("127.0.0.1", 2000, 30.0) + assert world.applied == [] + finally: + worker.close() + + +def test_park_releases_the_lock_so_the_worker_can_start(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + patch_client(monkeypatch, fake_world(no_rendering=False)) + + from moq_carla_sim.main import claim_simulator + + assert park_module.park("127.0.0.1", 2000, 30.0) + claim_simulator("127.0.0.1", 2000).close() + + +def test_park_is_idempotent(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + world = fake_world(no_rendering=True) + patch_client(monkeypatch, world) + + assert park_module.park("127.0.0.1", 2000, 30.0) + assert world.applied == [] + + +def test_park_never_touches_synchronous_mode(monkeypatch: pytest.MonkeyPatch, tmp_path) -> None: + """A synchronous world nobody ticks is frozen for carla-map and the benchmark.""" + monkeypatch.setenv("XDG_RUNTIME_DIR", str(tmp_path)) + world = fake_world(no_rendering=False) + world.settings.synchronous_mode = False + world.settings.fixed_delta_seconds = 0.0 + patch_client(monkeypatch, world) + + park_module.park("127.0.0.1", 2000, 30.0) + + assert world.settings.synchronous_mode is False + assert world.settings.fixed_delta_seconds == 0.0 diff --git a/demo/carla/sim/tests/test_publish.py b/demo/carla/sim/tests/test_publish.py new file mode 100644 index 00000000000..ab36f542785 --- /dev/null +++ b/demo/carla/sim/tests/test_publish.py @@ -0,0 +1,320 @@ +import asyncio +from types import SimpleNamespace + +import moq +import pytest + +from moq_carla_sim import publish +from moq_carla_sim.drive import Command, Drive, DriveState, Gear, TimestampEntry +from moq_carla_sim.publish import ( + Collision, + Motion, + Pose, + Status, + Telemetry, + parse_command, + status_payload, +) + + +def status(**overrides) -> Status: + """A status with nothing set, so each test names only what it is about.""" + defaults = dict( + accepted=DriveState.stopped(), + estopped=False, + estop_by=None, + autonomous=False, + driver=None, + sequence=None, + media_pipeline={}, + location=None, + map_name=None, + telemetry=None, + ) + return Status(**{**defaults, **overrides}) + + +def telemetry(**overrides) -> Telemetry: + defaults = dict( + pose=Pose(x=0.0, y=0.0, yaw=0.0, speed=0.0), + measured=DriveState.stopped(), + motion=Motion(yaw_rate=0.0, accel_lon=0.0, accel_lat=0.0), + collision=None, + ) + return Telemetry(**{**defaults, **overrides}) + + +def test_malformed_timestamp_does_not_drop_driver_command() -> None: + command = parse_command( + "driver", + { + "type": "drive", + "sequence": 7, + "throttle": 0.5, + "timestamps": [ + {"label": "bad-null", "ts": None}, + {"label": "bad-string", "ts": "12"}, + {"label": "received", "ts": 12.5}, + ], + }, + ) + + assert command is not None + assert command.sequence == 7 + assert command.timestamps == (TimestampEntry(label="received", ts=12.5),) + + +def test_only_the_players_pipeline_stages_are_kept() -> None: + stages = [{"label": "received", "ts": 1.0}, {"label": "decoded", "ts": 2.0}] + junk = [{"label": "x" * 10_000, "ts": 3.0}] * 1_000 + command = parse_command( + "driver", + {"type": "drive", "sequence": 1, "timestamps": [*stages, *junk, {"label": "received", "ts": 9.0}]}, + ) + + assert command is not None + assert command.timestamps == ( + TimestampEntry(label="received", ts=1.0), + TimestampEntry(label="decoded", ts=2.0), + ) + + +def test_respawn_command_is_carla_only_control_input() -> None: + command = parse_command("driver", {"type": "respawn", "sequence": 8}) + + assert command is not None + assert command.kind == "respawn" + assert command.driver_id == "driver" + assert command.sequence == 8 + + +def test_status_names_the_driver_that_latched_estop() -> None: + payload = status_payload(status(estopped=True, estop_by="bystander", driver="holder", sequence=4)) + + assert payload["driver"] == "holder" + assert payload["estop_by"] == "bystander" + assert payload["mode"] == "simulated" + assert payload["capabilities"] == ["respawn"] + + +def test_status_carries_the_map_and_a_rounded_pose() -> None: + payload = status_payload( + status( + autonomous=True, + map_name="Town10HD_Opt", + telemetry=telemetry(pose=Pose(x=12.3456, y=-78.9012, yaw=180.06, speed=9.87654)), + ) + ) + + assert payload["map"] == "Town10HD_Opt" + assert payload["pose"] == {"x": 12.35, "y": -78.9, "yaw": 180.1, "speed": 9.88} + + +def test_status_reports_measured_feedback_beside_the_accepted_command() -> None: + """The pair is the point: what was asked for, and what the vehicle did.""" + payload = status_payload( + status( + accepted=DriveState(steering=0.5, throttle=0.4, brake=0.0, gear=Gear.FORWARD), + telemetry=telemetry( + measured=DriveState(steering=0.31234, throttle=0.38765, brake=0.0, gear=Gear.FORWARD), + motion=Motion(yaw_rate=4.567, accel_lon=1.2345, accel_lat=-2.3456), + ), + ) + ) + + assert payload["accepted"]["steering"] == 0.5 + assert payload["measured"] == { + "steering": 0.312, + "throttle": 0.388, + "brake": 0.0, + "gear": "forward", + } + assert payload["motion"] == {"yaw_rate": 4.6, "accel_lon": 1.23, "accel_lat": -2.35} + + +def test_status_carries_a_collision_until_a_later_one_replaces_it() -> None: + """An absolute instant is what makes an event visible on a snapshot track.""" + payload = status_payload( + status( + telemetry=telemetry(collision=Collision(at=1756334400123.4, other="vehicle.tesla.model3", impulse=4210.55)) + ) + ) + + assert payload["collision"] == { + "at": 1756334400123, + "other": "vehicle.tesla.model3", + "impulse": 4210.6, + } + + +def test_status_omits_everything_a_producer_has_none_of() -> None: + payload = status_payload(status()) + + for absent in ("map", "pose", "measured", "motion", "collision"): + assert absent not in payload + + +def test_status_omits_only_the_collision_a_truck_has_not_had() -> None: + payload = status_payload(status(telemetry=telemetry())) + + assert "collision" not in payload + assert "measured" in payload + assert "motion" in payload + + +def test_a_closed_status_track_fails_the_session() -> None: + """Status republishes on a timer, so a swallowed failure would spin forever.""" + + def closed(_payload: object) -> None: + raise moq.Error("track closed") + + publisher = publish.StatusPublisher.__new__(publish.StatusPublisher) + publisher._producer = SimpleNamespace(update=closed) + + with pytest.raises(moq.Error): + publisher.publish({}) + + +class Announcements: + """A scripted announcement stream. `send` returns once the watcher has + handled the event and come back for the next one.""" + + def __init__(self) -> None: + self._events: asyncio.Queue = asyncio.Queue() + self._idle = asyncio.Event() + + async def __aenter__(self): + return self + + async def __aexit__(self, *_exc) -> None: + pass + + def __aiter__(self): + return self + + async def __anext__(self): + if self._events.empty(): + self._idle.set() + event = await self._events.get() + self._idle.clear() + return event + + async def send(self, kind: type, path: str, epoch: str | None) -> None: + self._idle.clear() + self._events.put_nowait(kind(SimpleNamespace(prefix=path, route=SimpleNamespace(epoch=epoch)))) + await self._idle.wait() + + +class Drivers: + """A relay holding driver broadcasts: each request serves one drive command + numbered by how many requests came before it, then stays open.""" + + def __init__(self) -> None: + self.announcements = Announcements() + self.requested: list[str] = [] + + def announced(self, prefix: str, *, filter: str): + assert (prefix, filter) == ("viewer/carla/truck1", "*") + return self.announcements + + async def request_broadcast(self, path: str): + self.requested.append(path) + sequence = len(self.requested) + + async def control(): + yield {"type": "drive", "sequence": sequence, "throttle": 0.5} + await asyncio.Event().wait() + + async def subscribe_track(name: str): + assert name == "control" + return control() + + return SimpleNamespace(subscribe_track=subscribe_track) + + +async def watching(scenario) -> None: + """Run `scenario(relay, commands, gate)` against a live `watch_drivers`.""" + relay = Drivers() + commands: asyncio.Queue = asyncio.Queue() + watcher = asyncio.create_task(publish.watch_drivers(relay, "viewer/carla/truck1", commands)) + try: + await scenario(relay, commands, Drive()) + finally: + watcher.cancel() + with pytest.raises(asyncio.CancelledError): + await watcher + + +async def next_command(commands: asyncio.Queue, gate: Drive): + command = await asyncio.wait_for(commands.get(), timeout=1) + gate.apply(command) + return command + + +def test_a_driver_run_keeps_the_lease_across_route_updates() -> None: + """An update is the same run re-priced: no second subscription, no release.""" + + async def scenario(relay: Drivers, commands: asyncio.Queue, gate: Drive) -> None: + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla/truck1/abc", "e1") + command = await next_command(commands, gate) + assert (command.kind, command.driver_id, command.epoch) == ("drive", "abc", "e1") + assert gate.driver == "abc" + + await relay.announcements.send(moq.AnnounceEventUpdate, "viewer/carla/truck1/abc", "e1") + assert commands.empty() + assert gate.driver == "abc" + assert relay.requested == ["viewer/carla/truck1/abc"] + + asyncio.run(watching(scenario)) + + +def test_a_driver_whose_broadcast_ends_loses_the_lease() -> None: + async def scenario(relay: Drivers, commands: asyncio.Queue, gate: Drive) -> None: + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla/truck1/abc", "e1") + await next_command(commands, gate) + assert gate.driver == "abc" + + await relay.announcements.send(moq.AnnounceEventEnd, "viewer/carla/truck1/abc", "e1") + left = await next_command(commands, gate) + assert (left.kind, left.epoch) == ("driver_left", "e1") + assert gate.driver is None + + asyncio.run(watching(scenario)) + + +@pytest.mark.parametrize("kind", ["restart", "update"]) +def test_a_driver_that_restarts_under_a_new_epoch_is_a_new_driver(kind: str) -> None: + """The old run's lease ends with it, even though the path and id are the same. + + A new epoch is a restart whether the relay says so or reports it as an update. + """ + event = moq.AnnounceEventRestart if kind == "restart" else moq.AnnounceEventUpdate + + async def scenario(relay: Drivers, commands: asyncio.Queue, gate: Drive) -> None: + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla/truck1/abc", "e1") + await next_command(commands, gate) + + await relay.announcements.send(event, "viewer/carla/truck1/abc", "e2") + received = [await next_command(commands, gate) for _ in range(2)] + assert {(command.kind, command.epoch) for command in received} == {("driver_left", "e1"), ("drive", "e2")} + assert relay.requested == ["viewer/carla/truck1/abc"] * 2 + + # Whichever order those two arrived in, the new run holds the lease + # by its next command, and the old run can no longer release it. + assert not gate.holds(Command(kind="drive", driver_id="abc", epoch="e1")) + gate.apply(Command(kind="drive", driver_id="abc", epoch="e2", sequence=3, state=DriveState())) + assert gate.holds(Command(kind="drive", driver_id="abc", epoch="e2")) + assert not gate.apply(Command(kind="driver_left", driver_id="abc", epoch="e1")) + assert gate.driver == "abc" + + asyncio.run(watching(scenario)) + + +def test_only_direct_children_of_the_prefix_are_drivers() -> None: + async def scenario(relay: Drivers, commands: asyncio.Queue, gate: Drive) -> None: + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla", None) + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla/truck1/abc/deeper", None) + await relay.announcements.send(moq.AnnounceEventStart, "viewer/carla/truck10", None) + assert relay.requested == [] + + asyncio.run(watching(scenario)) diff --git a/demo/carla/sim/tests/test_roadmap.py b/demo/carla/sim/tests/test_roadmap.py new file mode 100644 index 00000000000..7425ae94440 --- /dev/null +++ b/demo/carla/sim/tests/test_roadmap.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from moq_carla_sim.roadmap import feature_collection, lane_points, road_lines, simplify + + +def waypoint(x: float, y: float, lane: tuple[int, int, int], following: list | None = None) -> SimpleNamespace: + ahead = following or [] + return SimpleNamespace( + road_id=lane[0], + section_id=lane[1], + lane_id=lane[2], + transform=SimpleNamespace(location=SimpleNamespace(x=x, y=y)), + next=lambda _step: ahead, + ) + + +def test_simplify_collapses_a_straight_run_to_its_ends() -> None: + straight = [(float(x), 0.0) for x in range(50)] + + assert simplify(straight, 0.5) == [(0.0, 0.0), (49.0, 0.0)] + + +def test_simplify_keeps_a_corner_outside_the_tolerance() -> None: + corner = [(0.0, 0.0), (5.0, 2.0), (10.0, 0.0)] + + assert simplify(corner, 0.5) == corner + # The same corner inside a wider corridor is a straight line. + assert simplify(corner, 3.0) == [(0.0, 0.0), (10.0, 0.0)] + + +def test_lane_walk_stops_where_the_lane_does() -> None: + lane = (1, 0, -1) + third = waypoint(4.0, 0.0, lane) + second = waypoint(2.0, 0.0, lane, [third]) + first = waypoint(0.0, 0.0, lane, [second]) + + assert lane_points(first, 2.0) == [(0.0, 0.0), (2.0, 0.0), (4.0, 0.0)] + + +def test_lane_walk_does_not_follow_the_next_lane() -> None: + other = waypoint(2.0, 0.0, (2, 0, -1)) + first = waypoint(0.0, 0.0, (1, 0, -1), [other]) + + assert lane_points(first, 2.0) == [(0.0, 0.0)] + + +def test_lane_walk_bounds_a_ring_road() -> None: + lane = (1, 0, -1) + looping = waypoint(0.0, 0.0, lane) + looping.next = lambda _step: [looping] + + assert len(lane_points(looping, 2.0, limit=10)) == 11 + + +def test_road_lines_drops_a_lane_too_short_to_draw() -> None: + lane = (1, 0, -1) + stub = waypoint(0.0, 0.0, lane) + end = waypoint(10.0, 0.0, lane) + drawable = waypoint(0.0, 5.0, lane, [end]) + + assert road_lines(SimpleNamespace(get_topology=lambda: [(stub, stub), (drawable, end)])) == [ + [(0.0, 5.0), (10.0, 0.0)] + ] + + +def test_feature_collection_bounds_every_lane() -> None: + collection = feature_collection("Town10HD_Opt", [[(0.0, 0.0), (10.04, 5.0)], [(-3.0, 2.0), (1.0, -8.0)]]) + + assert collection["name"] == "Town10HD_Opt" + assert collection["bbox"] == [-3.0, -8.0, 10.0, 5.0] + assert collection["features"][0]["geometry"]["coordinates"][0] == [[0.0, 0.0], [10.0, 5.0]] + + +def test_a_map_with_no_drivable_lane_is_an_error() -> None: + with pytest.raises(RuntimeError, match="no drivable lanes"): + feature_collection("Empty", []) diff --git a/demo/carla/sim/tests/test_runtime.py b/demo/carla/sim/tests/test_runtime.py new file mode 100644 index 00000000000..298329becc3 --- /dev/null +++ b/demo/carla/sim/tests/test_runtime.py @@ -0,0 +1,540 @@ +import asyncio +import contextlib +from types import SimpleNamespace + +import pytest + +from moq_carla_sim import runtime +from moq_carla_sim.drive import Command, Drive, DriveState, Gear, TimestampEntry +from moq_carla_sim.publish import Pose +from moq_carla_sim.runtime import MAX_STAGE_MS, StuckDetector, latency_breakdown + + +class FakeClock: + """Records holds so a test can assert what keeps the world stepping.""" + + def __init__(self, running: bool = True) -> None: + self.held: set[str] = set() + self.running = running + + def hold(self, reason: str) -> None: + self.held.add(reason) + + def release(self, reason: str) -> None: + self.held.discard(reason) + + +class DemandPublisher: + def __init__(self) -> None: + self.use = asyncio.Event() + self.unuse = asyncio.Event() + self.cuts = 0 + + async def used(self) -> None: + await self.use.wait() + self.use.clear() + + async def unused(self) -> None: + await self.unuse.wait() + self.unuse.clear() + + def cut(self) -> None: + self.cuts += 1 + + +def test_latency_breakdown_survives_any_finite_timestamps() -> None: + stages = ( + TimestampEntry(label="received", ts=-1e308), + TimestampEntry(label="decoded", ts=1e308), + TimestampEntry(label="rendered", ts=-1e308), + ) + + assert [stage["ms"] for stage in latency_breakdown(stages)] == [MAX_STAGE_MS, 0] + + +def test_takeover_synchronizes_gate_with_current_vehicle_controls() -> None: + async def scenario() -> None: + measured = DriveState(steering=-0.3, throttle=0.7, brake=0.0, gear=Gear.FORWARD) + commands = asyncio.Queue() + commands.put_nowait(Command(kind="drive", driver_id="driver", sequence=1, state=measured)) + + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace( + name="truck1", + autonomous=True, + telemetry=lambda: SimpleNamespace(measured=measured), + ) + session._drive = Drive() + session._commands = commands + session._deadline = None + session._media_pipeline = {} + session._actuate = lambda: None + session._publish = lambda: None + + task = asyncio.create_task(session._loop()) + try: + for _ in range(10): + if session._drive.driver is not None: + break + await asyncio.sleep(0) + assert session._drive.driver == "driver" + assert session._drive.state == measured + finally: + task.cancel() + with contextlib.suppress(asyncio.CancelledError): + await task + + asyncio.run(scenario()) + + +def test_only_unestopped_lease_holder_can_respawn_truck() -> None: + async def scenario() -> None: + commands = asyncio.Queue() + commands.put_nowait(Command(kind="respawn", driver_id="bystander", sequence=2)) + commands.put_nowait(Command(kind="respawn", driver_id="holder", sequence=3)) + commands.put_nowait(Command(kind="reset_estop", driver_id="holder", sequence=4)) + commands.put_nowait(Command(kind="respawn", driver_id="holder", sequence=5)) + respawns: list[str] = [] + published: list[bool] = [] + + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1", respawn=lambda: respawns.append("truck1")) + session._drive = Drive() + session._stuck = StuckDetector() + assert session._drive.apply(Command(kind="drive", driver_id="holder", sequence=1, state=DriveState())) + assert session._drive.apply(Command(kind="estop", driver_id="bystander", sequence=2)) + session._commands = commands + session._deadline = None + session._media_pipeline = {"holder": []} + session._actuate = lambda: None + session._publish = lambda: published.append(True) + + task = asyncio.create_task(session._loop()) + try: + for _ in range(10): + if respawns: + break + await asyncio.sleep(0) + assert respawns == ["truck1"] + assert session._drive.driver is None + assert session._media_pipeline == {} + assert published == [True, True] + finally: + task.cancel() + with contextlib.suppress(asyncio.CancelledError): + await task + + asyncio.run(scenario()) + + +def test_video_demand_renders_the_world_for_the_truck(monkeypatch: pytest.MonkeyPatch) -> None: + """The stream's demand is what turns the simulator's renderer on for a truck.""" + + async def wait_forever(*_args: object) -> None: + await asyncio.Event().wait() + + async def scenario() -> None: + demanded: list[bool] = [] + watching = asyncio.Event() + + async def watch_demand(on_demand) -> None: + on_demand(True) + on_demand(False) + watching.set() + await asyncio.Event().wait() + + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1", set_video_demand=demanded.append) + session._commands = asyncio.Queue() + session._video = SimpleNamespace(watch_demand=watch_demand, published=[0, 0], rendition_names=["sd", "hd"]) + session._status = DemandPublisher() + session._publish = lambda: None + session._loop = wait_forever + monkeypatch.setattr(runtime, "watch_drivers", wait_forever) + + run = asyncio.create_task(session.run(SimpleNamespace(), "viewer/carla/truck1")) + await watching.wait() + run.cancel() + await asyncio.gather(run, return_exceptions=True) + assert demanded == [True, False] + + asyncio.run(scenario()) + + +def test_session_exits_when_its_video_demand_fails(monkeypatch: pytest.MonkeyPatch) -> None: + """A world that cannot start rendering for a viewer fails the session.""" + + async def wait_forever(*_args: object) -> None: + await asyncio.Event().wait() + + async def fail_demand(_on_demand) -> None: + raise RuntimeError("CARLA rejected settings") + + async def scenario() -> None: + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1", set_video_demand=lambda _demanded: None) + session._commands = asyncio.Queue() + session._video = SimpleNamespace(watch_demand=fail_demand, published=[0], rendition_names=["hd"]) + session._status = DemandPublisher() + session._publish = lambda: None + session._loop = wait_forever + monkeypatch.setattr(runtime, "watch_drivers", wait_forever) + + with pytest.raises(ExceptionGroup) as failure: + await session.run(SimpleNamespace(), "viewer/carla/truck1") + + assert len(failure.value.exceptions) == 1 + assert str(failure.value.exceptions[0]) == "CARLA rejected settings" + + asyncio.run(scenario()) + + +def test_status_republishes_on_a_timer_so_telemetry_keeps_arriving( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """An autonomous truck publishes no accepted-state change, but still drives.""" + + async def scenario() -> None: + published: list[int] = [] + slept: list[float] = [] + + async def sleep(delay: float) -> None: + slept.append(delay) + if len(slept) == 3: + raise asyncio.CancelledError + + monkeypatch.setattr(runtime.asyncio, "sleep", sleep) + session = telemetry_session(pose=lambda: Pose(x=0.0, y=0.0, yaw=0.0, speed=0.0)) + session._publish = lambda _telemetry: published.append(len(published)) + + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + + assert slept == [runtime.TELEMETRY_INTERVAL] * 3 + assert len(published) == 2 + + asyncio.run(scenario()) + + +def telemetry_session(pose, *, autonomous: bool = True) -> runtime.TruckSession: + """A session with just what the telemetry timer touches, at the resting state.""" + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace( + name="truck1", + autonomous=autonomous, + telemetry=lambda: SimpleNamespace(pose=pose()), + respawn=lambda: None, + release=lambda: None, + ) + session._drive = Drive() + session._stuck = StuckDetector() + return session + + +def test_telemetry_timer_keeps_releasing_a_driverless_truck(monkeypatch: pytest.MonkeyPatch) -> None: + """The handback merges over several ticks, so the timer must revisit it.""" + + async def scenario() -> None: + ticks = 0 + releases = 0 + + async def sleep(_delay: float) -> None: + nonlocal ticks + ticks += 1 + if ticks > 5: + raise asyncio.CancelledError + + def release() -> None: + nonlocal releases + releases += 1 + + monkeypatch.setattr(runtime.asyncio, "sleep", sleep) + session = telemetry_session(pose=lambda: Pose(x=0.0, y=0.0, yaw=0.0, speed=8.0), autonomous=False) + session._truck.release = release + session._publish = lambda _telemetry: None + + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + assert releases == 5 + + # A held or e-stopped truck is not driverless, and stays off autonomy. + for hold in (holding, estopped_by_bystander): + ticks = releases = 0 + session = telemetry_session(pose=lambda: Pose(x=0.0, y=0.0, yaw=0.0, speed=0.0), autonomous=False) + session._truck.release = release + session._publish = lambda _telemetry: None + hold(session._drive) + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + assert releases == 0 + + asyncio.run(scenario()) + + +def test_stuck_detector_fires_only_after_the_timeout_without_displacement() -> None: + detector = StuckDetector(timeout=300.0, distance=2.0) + parked = Pose(x=10.0, y=10.0, yaw=0.0, speed=0.0) + + assert not detector.update(parked, now=0.0) + assert not detector.update(parked, now=299.9) + # Grinding sideways against a barrier stays inside the window. + assert not detector.update(Pose(x=11.0, y=10.0, yaw=0.0, speed=0.3), now=299.9) + assert detector.update(parked, now=300.0) + + # Getting clear re-anchors and restarts the clock. + assert not detector.update(Pose(x=12.0, y=10.0, yaw=0.0, speed=0.0), now=301.0) + assert not detector.update(Pose(x=12.0, y=10.0, yaw=0.0, speed=0.0), now=600.0) + assert detector.update(Pose(x=12.0, y=10.0, yaw=0.0, speed=0.0), now=601.0) + + +def test_autonomous_truck_wedged_for_the_timeout_is_respawned(monkeypatch: pytest.MonkeyPatch) -> None: + async def scenario() -> None: + clock = 0.0 + respawns: list[float] = [] + published: list[Pose] = [] + + async def sleep(delay: float) -> None: + nonlocal clock + clock += delay + if clock > runtime.STUCK_TIMEOUT + 1.0: + raise asyncio.CancelledError + + monkeypatch.setattr(runtime.asyncio, "sleep", sleep) + loop = asyncio.get_running_loop() + monkeypatch.setattr(loop, "time", lambda: clock) + + wedged = Pose(x=5.0, y=5.0, yaw=0.0, speed=0.0) + session = telemetry_session(pose=lambda: wedged) + session._truck.respawn = lambda: respawns.append(clock) + session._publish = lambda telemetry: published.append(telemetry.pose) + + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + + assert len(respawns) == 1 + assert runtime.STUCK_TIMEOUT <= respawns[0] < runtime.STUCK_TIMEOUT + 1.0 + # Status keeps flowing through the whole wait, and after the respawn. + assert len(published) > respawns[0] / runtime.TELEMETRY_INTERVAL + + asyncio.run(scenario()) + + +def holding(drive: Drive) -> None: + """A driver parked with the brake on.""" + assert drive.apply(Command(kind="drive", driver_id="holder", sequence=1, state=DriveState(brake=1.0))) + + +def estopped_by_bystander(drive: Drive) -> None: + """A bystander's e-stop outlives the lease it never held.""" + assert drive.apply(Command(kind="estop", driver_id="bystander", sequence=1)) + assert drive.driver is None and drive.estopped + + +@pytest.mark.parametrize("hold", [holding, estopped_by_bystander]) +def test_stuck_watchdog_leaves_a_held_or_estopped_truck_alone(monkeypatch: pytest.MonkeyPatch, hold) -> None: + async def scenario() -> None: + clock = 0.0 + respawns: list[float] = [] + + async def sleep(delay: float) -> None: + nonlocal clock + clock += delay + if clock > 2 * runtime.STUCK_TIMEOUT: + raise asyncio.CancelledError + + monkeypatch.setattr(runtime.asyncio, "sleep", sleep) + loop = asyncio.get_running_loop() + monkeypatch.setattr(loop, "time", lambda: clock) + + wedged = Pose(x=5.0, y=5.0, yaw=0.0, speed=0.0) + session = telemetry_session(pose=lambda: wedged, autonomous=False) + session._truck.respawn = lambda: respawns.append(clock) + session._publish = lambda _telemetry: None + hold(session._drive) + + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + + assert respawns == [] + + asyncio.run(scenario()) + + +def test_session_announces_with_the_run_epoch_after_populating_tracks() -> None: + """The player lists trucks from announcements, and a broadcast starts + unadvertised, so a session that never announces is invisible. The route + carries the run's epoch so a restarted worker reads as a restart.""" + calls: list[object] = [] + broadcast = SimpleNamespace( + publish_track=lambda name: calls.append(name) or SimpleNamespace(demand=SimpleNamespace), + announce=lambda route: calls.append(route), + ) + route = SimpleNamespace(epoch="0190b2a4-0000-7000-8000-000000000000") + + # The video tracks were advertised when the bridge stream opened. + runtime.TruckSession(SimpleNamespace(name="truck1"), broadcast, SimpleNamespace(), None, None, FakeClock(), route) + + assert calls == ["status", route] + + +def test_a_driver_keeps_the_world_stepping_with_nobody_watching() -> None: + """Video demand and the lease are independent; the gate must hold for both.""" + + async def scenario() -> None: + commands = asyncio.Queue() + commands.put_nowait(Command(kind="drive", driver_id="holder", sequence=1, state=DriveState(throttle=0.5))) + + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1", autonomous=False) + session._drive = Drive() + session._commands = commands + session._deadline = None + session._media_pipeline = {} + session._stuck = StuckDetector() + session._actuate = lambda: None + session._publish = lambda: None + + with contextlib.suppress(asyncio.TimeoutError): + await asyncio.wait_for(session._loop(), timeout=0.1) + + assert session._clock.held == {"drive:truck1"} + + # The watchdog hands the truck back, and the hold goes with the lease. + session._commands.put_nowait(Command(kind="watchdog_expired")) + with contextlib.suppress(asyncio.TimeoutError): + await asyncio.wait_for(session._loop(), timeout=0.1) + + assert session._clock.held == set() + + asyncio.run(scenario()) + + +def test_a_parked_world_is_not_a_stuck_truck(monkeypatch: pytest.MonkeyPatch) -> None: + """Nothing is meant to be moving, so respawning on it would fire forever.""" + + async def scenario() -> None: + clock = 0.0 + respawns: list[float] = [] + + async def sleep(delay: float) -> None: + nonlocal clock + clock += delay + if clock > runtime.STUCK_TIMEOUT * 2: + raise asyncio.CancelledError + + monkeypatch.setattr(runtime.asyncio, "sleep", sleep) + loop = asyncio.get_running_loop() + monkeypatch.setattr(loop, "time", lambda: clock) + + session = telemetry_session(pose=lambda: Pose(x=5.0, y=5.0, yaw=0.0, speed=0.0)) + session._clock = FakeClock(running=False) + session._truck.respawn = lambda: respawns.append(clock) + session._publish = lambda _telemetry: None + + with pytest.raises(asyncio.CancelledError): + await session._publish_telemetry() + + assert respawns == [] + + asyncio.run(scenario()) + + +def test_a_status_subscriber_keeps_the_world_stepping() -> None: + """The map draws positions from `status`, so an open map must not freeze.""" + + async def scenario() -> None: + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1") + session._status = DemandPublisher() + watcher = asyncio.create_task(session._watch_status_demand()) + + async def expect_held(held: set[str]) -> None: + for _ in range(10): + if session._clock.held == held: + return + await asyncio.sleep(0) + pytest.fail(f"holds never became {held}, still {session._clock.held}") + + try: + session._status.use.set() + await expect_held({"status:truck1"}) + session._status.unuse.set() + await expect_held(set()) + session._status.use.set() + await expect_held({"status:truck1"}) + finally: + watcher.cancel() + with contextlib.suppress(asyncio.CancelledError): + await watcher + # Cancelled mid-subscription, as a session shutting down would be. + assert session._clock.held == set() + + asyncio.run(scenario()) + + +def test_an_estopped_holder_neither_steps_the_world_nor_keeps_its_lease( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """A latched truck stands still, so its refused frames must let the lease lapse.""" + + async def scenario() -> None: + now = 0.0 + loop = asyncio.get_running_loop() + monkeypatch.setattr(loop, "time", lambda: now) + published: list[bool] = [] + + session = runtime.TruckSession.__new__(runtime.TruckSession) + session._clock = FakeClock() + session._truck = SimpleNamespace(name="truck1", autonomous=False) + session._drive = Drive() + session._commands = asyncio.Queue() + session._deadline = None + session._media_pipeline = {} + session._actuate = lambda: None + session._publish = lambda: published.append(True) + + async def settle(count: int) -> None: + for _ in range(10): + if len(published) == count: + return + await asyncio.sleep(0) + pytest.fail(f"expected {count} publishes, saw {len(published)}") + + task = asyncio.create_task(session._loop()) + try: + session._commands.put_nowait( + Command(kind="drive", driver_id="holder", sequence=1, state=DriveState(brake=1.0)) + ) + await settle(1) + assert session._clock.held == {"drive:truck1"} + + session._commands.put_nowait(Command(kind="estop", driver_id="holder", sequence=2)) + await settle(2) + assert session._drive.driver == "holder" + assert session._clock.held == set() + assert session._deadline == runtime.COMMAND_TIMEOUT + + # The holder keeps sending and the gate refuses every frame. A + # bystander's e-stop publishes without refreshing the lease, which + # marks the refused frame as processed. + now = runtime.COMMAND_TIMEOUT / 2 + session._commands.put_nowait( + Command(kind="drive", driver_id="holder", sequence=3, state=DriveState(throttle=0.5)) + ) + session._commands.put_nowait(Command(kind="estop", driver_id="bystander", sequence=1)) + await settle(3) + assert session._deadline == runtime.COMMAND_TIMEOUT + assert session._clock.held == set() + finally: + task.cancel() + with contextlib.suppress(asyncio.CancelledError): + await task + + asyncio.run(scenario()) diff --git a/demo/carla/sim/tests/test_watchdog.py b/demo/carla/sim/tests/test_watchdog.py new file mode 100644 index 00000000000..8a3daeef64a --- /dev/null +++ b/demo/carla/sim/tests/test_watchdog.py @@ -0,0 +1,87 @@ +import asyncio +import time + +from moq_carla_sim.watchdog import Heartbeat, Watchdog + + +def test_a_stale_heartbeat_ends_the_process() -> None: + """A clock thread that never returns from `world.tick()` beats nothing.""" + + async def scenario() -> list[str]: + failures: list[str] = [] + clock = Heartbeat("simulation clock") + watchdog = Watchdog(0.1, clock, fail=failures.append) + watchdog.start() + pulse = asyncio.create_task(watchdog.pulse()) + try: + deadline = time.monotonic() + 2.0 + while not failures and time.monotonic() < deadline: + await asyncio.sleep(0.01) + finally: + pulse.cancel() + watchdog.stop() + return failures + + failures = asyncio.run(scenario()) + assert failures and failures[0].startswith("simulation clock has not advanced") + + +def test_a_blocked_event_loop_is_a_stall_even_while_the_clock_ticks() -> None: + """The loop's own heartbeat is watched, since every asyncio watcher is stuck behind a blocked loop.""" + + async def scenario() -> list[str]: + failures: list[str] = [] + clock = Heartbeat("simulation clock") + watchdog = Watchdog(0.1, clock, fail=failures.append) + watchdog.start() + pulse = asyncio.create_task(watchdog.pulse()) + try: + await asyncio.sleep(0.05) + # A blocking call on the loop thread: nothing awaits, so the pulse + # cannot run, while the clock keeps beating from its own thread. + deadline = time.monotonic() + 0.5 + while time.monotonic() < deadline: + clock.beat() + time.sleep(0.01) + finally: + pulse.cancel() + watchdog.stop() + return failures + + failures = asyncio.run(scenario()) + assert failures and failures[0].startswith("event loop has not advanced") + + +def test_a_worker_making_progress_is_left_alone() -> None: + async def scenario() -> list[str]: + failures: list[str] = [] + clock = Heartbeat("simulation clock") + watchdog = Watchdog(0.1, clock, fail=failures.append) + watchdog.start() + pulse = asyncio.create_task(watchdog.pulse()) + try: + for _ in range(20): + clock.beat() + await asyncio.sleep(0.025) + finally: + pulse.cancel() + watchdog.stop() + return failures + + assert asyncio.run(scenario()) == [] + + +def test_the_loop_is_watched_only_once_its_pulse_starts() -> None: + """Startup runs blocking CARLA calls before `supervise` schedules the pulse; that is not a stall.""" + failures: list[str] = [] + clock = Heartbeat("simulation clock") + watchdog = Watchdog(0.05, clock, fail=failures.append) + watchdog.start() + try: + deadline = time.monotonic() + 0.3 + while time.monotonic() < deadline: + clock.beat() + time.sleep(0.01) + finally: + watchdog.stop() + assert failures == [] diff --git a/demo/carla/sim/tests/test_world.py b/demo/carla/sim/tests/test_world.py new file mode 100644 index 00000000000..0013fdda3a1 --- /dev/null +++ b/demo/carla/sim/tests/test_world.py @@ -0,0 +1,955 @@ +from __future__ import annotations + +import asyncio +from types import SimpleNamespace + +import pytest + +from moq_carla_sim import world as world_module +from moq_carla_sim.drive import Accepted, DriveState, Gear +from moq_carla_sim.publish import Collision, Motion, Pose +from moq_carla_sim.world import ( + CAMERA_STANDOFF, + COLLISION_WINDOW, + CONVOY_SPACING, + SPAWN_TICK_TIMEOUT, + CameraConfig, + Clock, + Truck, + World, + camera_transform, + convoy_anchor_points, + convoy_transform, + free_spawn_point, + nearby_spawn_points, + next_deadline, +) + + +def vector(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> SimpleNamespace: + return SimpleNamespace(x=x, y=y, z=z) + + +def fake_sensor(events: list[str] | None = None, name: str = "sensor") -> SimpleNamespace: + """A sensor whose lifecycle calls are recorded, or ignored.""" + record = (lambda call: events.append(f"{name}.{call}")) if events is not None else (lambda _call: None) + return SimpleNamespace( + listen=lambda _callback: record("listen"), + stop=lambda: record("stop"), + destroy=lambda: record("destroy"), + ) + + +def fake_traffic_manager(modes: list[bool] | None = None) -> SimpleNamespace: + return SimpleNamespace(set_synchronous_mode=lambda sync: modes is not None and modes.append(sync)) + + +def fake_vehicle(**overrides) -> SimpleNamespace: + """A stationary vehicle pointing along +x, applying nothing.""" + defaults = dict( + id=1, + set_autopilot=lambda *_args: None, + get_transform=lambda: SimpleNamespace( + location=vector(), + rotation=SimpleNamespace(yaw=0.0), + get_forward_vector=lambda: vector(x=1.0), + get_right_vector=lambda: vector(y=1.0), + ), + get_velocity=lambda: vector(), + get_acceleration=lambda: vector(), + get_angular_velocity=lambda: vector(), + get_control=lambda: SimpleNamespace(steer=0.0, throttle=0.0, brake=0.0, reverse=False), + ) + vehicle = SimpleNamespace(**{**defaults, **overrides}) + if "get_world" not in overrides: + vehicle.get_world = lambda: SimpleNamespace( + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [vehicle.get_transform()]), + get_actors=lambda: SimpleNamespace(filter=lambda _pattern: [vehicle]), + ) + return vehicle + + +def test_camera_config_builds_240p_sd_and_720p_hd_by_default() -> None: + assert CameraConfig().renditions() == ( + world_module.RenditionConfig(name="sd", width=426, height=240, fps=30, bitrate=800_000), + world_module.RenditionConfig(name="hd", width=1280, height=720, fps=30, bitrate=4_000_000), + ) + + +def test_pose_reports_world_metres_and_speed() -> None: + vehicle = fake_vehicle( + get_transform=lambda: SimpleNamespace( + location=vector(x=12.5, y=-3.25), + rotation=SimpleNamespace(yaw=90.0), + get_forward_vector=lambda: vector(y=1.0), + get_right_vector=lambda: vector(x=-1.0), + ), + get_velocity=lambda: vector(x=3.0, y=4.0), + ) + + telemetry = Truck("truck1", vehicle, [], fake_sensor(), 8000, road_map=SimpleNamespace()).telemetry() + + assert telemetry.pose == Pose(x=12.5, y=-3.25, yaw=90.0, speed=5.0) + + +def test_measured_control_is_what_the_physics_applied_not_the_command() -> None: + """`get_control` is populated whoever is driving, Traffic Manager included.""" + vehicle = fake_vehicle(get_control=lambda: SimpleNamespace(steer=-0.4, throttle=0.6, brake=0.0, reverse=True)) + + measured = Truck("truck1", vehicle, [], fake_sensor(), 8000, road_map=SimpleNamespace()).telemetry().measured + + assert (measured.steering, measured.throttle, measured.brake) == (-0.4, 0.6, 0.0) + assert measured.gear is Gear.REVERSE + + +def test_respawn_returns_to_spawn_and_reengages_autonomy() -> None: + events: list[object] = [] + spawn = SimpleNamespace( + name="spawn", + location=vector(), + rotation=SimpleNamespace(yaw=0.0), + get_forward_vector=lambda: vector(x=1.0), + get_right_vector=lambda: vector(y=1.0), + ) + vehicle = fake_vehicle( + get_transform=lambda: spawn, + set_autopilot=lambda enabled, port: events.append(("autopilot", enabled, port)), + apply_control=lambda _control: events.append("control.reset"), + set_transform=lambda transform: events.append(("transform", transform)), + set_target_velocity=lambda _velocity: events.append("velocity.reset"), + set_target_angular_velocity=lambda _velocity: events.append("angular.reset"), + get_world=lambda: SimpleNamespace( + get_map=lambda: pytest.fail("respawn downloads the map again"), + get_actors=lambda: SimpleNamespace(filter=lambda _pattern: [vehicle]), + ), + ) + road_map = SimpleNamespace(get_spawn_points=lambda: [spawn]) + truck = Truck("truck1", vehicle, [], fake_sensor(), 8000, road_map=road_map) + truck._collision = Collision(at=1.0, other="barrier", impulse=2.0) + events.clear() + + truck.respawn() + + assert events == [ + ("autopilot", False, 8000), + "control.reset", + ("transform", spawn), + "velocity.reset", + "angular.reset", + ("autopilot", True, 8000), + ] + assert truck.autonomous + assert truck.telemetry().collision is None + + +def test_acceleration_is_projected_onto_the_vehicles_own_axes() -> None: + """A truck facing south feels a westward push as acceleration to its right.""" + vehicle = fake_vehicle( + get_transform=lambda: SimpleNamespace( + location=vector(), + rotation=SimpleNamespace(yaw=90.0), + get_forward_vector=lambda: vector(y=1.0), + get_right_vector=lambda: vector(x=-1.0), + ), + get_acceleration=lambda: vector(x=-3.0, y=2.0), + get_angular_velocity=lambda: vector(z=7.5), + ) + + assert Truck("truck1", vehicle, [], fake_sensor(), 8000, road_map=SimpleNamespace()).telemetry().motion == Motion( + yaw_rate=7.5, accel_lon=2.0, accel_lat=3.0 + ) + + +def test_acceleration_on_a_grade_keeps_its_vertical_component() -> None: + """A road that is graded and banked is where the z terms live.""" + # Nose up a 3-4-5 grade: forward tilts out of the horizontal plane, and the + # truck accelerating along that slope has acceleration pointing up it too. + vehicle = fake_vehicle( + get_transform=lambda: SimpleNamespace( + location=vector(), + rotation=SimpleNamespace(yaw=0.0), + get_forward_vector=lambda: vector(x=0.8, z=0.6), + get_right_vector=lambda: vector(y=1.0), + ), + get_acceleration=lambda: vector(x=4.0, z=3.0), + ) + + motion = Truck("truck1", vehicle, [], fake_sensor(), 8000, road_map=SimpleNamespace()).telemetry().motion + + # 4.0*0.8 + 3.0*0.6; dropping the vertical term would report 3.2. + assert motion.accel_lon == pytest.approx(5.0) + + +class Contacts: + """Drive `_on_collision` with both clocks under the test's control.""" + + def __init__(self, truck: Truck, patch: pytest.MonkeyPatch) -> None: + self.truck = truck + self.monotonic = 100.0 + self.wall = 1756334400000.0 + patch.setattr(world_module.time, "monotonic", lambda: self.monotonic) + patch.setattr(world_module.time, "time", lambda: self.wall / 1000.0) + + def advance(self, seconds: float) -> None: + self.monotonic += seconds + self.wall += seconds * 1000.0 + + def hit(self, magnitude: float, other: str, actor: int = 1) -> None: + self.truck._on_collision( + SimpleNamespace( + normal_impulse=vector(x=magnitude), + other_actor=SimpleNamespace(id=actor, type_id=other), + ) + ) + + +def test_continuous_contact_is_one_collision_with_its_peak_impulse() -> None: + """CARLA emits an event per frame of contact; a barrier is not 30 crashes.""" + truck = Truck("truck1", fake_vehicle(), [], fake_sensor(), 8000, road_map=SimpleNamespace()) + + with pytest.MonkeyPatch.context() as patch: + contacts = Contacts(truck, patch) + started = contacts.wall + contacts.hit(400.0, "static.sidewalk") + contacts.advance(COLLISION_WINDOW / 2) + contacts.hit(900.0, "static.sidewalk") + contacts.advance(COLLISION_WINDOW / 2) + contacts.hit(100.0, "static.sidewalk") + # The timestamp stays at the start of contact: moving it forward as the + # scrape continues would republish one collision as a stream of them. + assert truck.telemetry().collision == Collision(at=started, other="static.sidewalk", impulse=900.0) + + # Contact keeps extending the window, so only a real gap starts a second. + contacts.advance(COLLISION_WINDOW * 2) + assert truck.telemetry().collision.at == started + contacts.hit(50.0, "vehicle.tesla.model3", actor=2) + assert truck.telemetry().collision == Collision(at=contacts.wall, other="vehicle.tesla.model3", impulse=50.0) + + +def test_striking_something_else_is_a_new_collision_however_soon() -> None: + """Scrape a barrier, then hit a truck: two events, not one.""" + truck = Truck("truck1", fake_vehicle(), [], fake_sensor(), 8000, road_map=SimpleNamespace()) + + with pytest.MonkeyPatch.context() as patch: + contacts = Contacts(truck, patch) + contacts.hit(400.0, "static.guardrail", actor=7) + contacts.advance(COLLISION_WINDOW / 4) + contacts.hit(50.0, "vehicle.carlamotors.european_hgv", actor=9) + + # The second is quieter and well inside the window, so a time-only + # window would have hidden it behind the guardrail's own event. + assert truck.telemetry().collision == Collision( + at=contacts.wall, other="vehicle.carlamotors.european_hgv", impulse=50.0 + ) + + +def test_two_of_the_same_model_are_two_collisions() -> None: + """The window keys on the actor's id; `type_id` is a blueprint name.""" + truck = Truck("truck1", fake_vehicle(), [], fake_sensor(), 8000, road_map=SimpleNamespace()) + + with pytest.MonkeyPatch.context() as patch: + contacts = Contacts(truck, patch) + contacts.hit(100.0, "vehicle.tesla.model3", actor=3) + first = truck.telemetry().collision + contacts.advance(COLLISION_WINDOW / 4) + contacts.hit(100.0, "vehicle.tesla.model3", actor=4) + + assert truck.telemetry().collision.at != first.at + + +def test_a_truck_that_has_hit_nothing_reports_no_collision() -> None: + truck = Truck("truck1", fake_vehicle(), [], fake_sensor(), 8000, road_map=SimpleNamespace()) + assert truck.telemetry().collision is None + + +def test_camera_sits_beyond_vehicle_front() -> None: + vehicle = SimpleNamespace( + bounding_box=SimpleNamespace( + location=SimpleNamespace(x=0.01), + extent=SimpleNamespace(x=3.97), + ) + ) + + transform = camera_transform(vehicle) + + assert transform.location.x == 0.01 + 3.97 + CAMERA_STANDOFF + assert transform.location.z == 2.8 + + +def test_nearby_spawn_points_cluster_around_first_truck() -> None: + def point(x: float, y: float) -> SimpleNamespace: + return SimpleNamespace(location=SimpleNamespace(x=x, y=y, z=0.0)) + + far = point(100.0, 100.0) + closest = point(12.0, 10.0) + anchor = point(10.0, 10.0) + middle = point(20.0, 10.0) + + ordered = nearby_spawn_points([far, closest, anchor, middle], SimpleNamespace(x=10.0, y=10.0, z=0.0)) + + assert ordered == [anchor, closest, middle, far] + + +def test_free_spawn_point_skips_an_original_occupied_by_another_truck() -> None: + original = SimpleNamespace(location=vector()) + fallback = SimpleNamespace(location=vector(x=20.0)) + occupying = SimpleNamespace(id=2, get_transform=lambda: original) + + assert free_spawn_point([original, fallback], [occupying], own_id=1) is fallback + + +def test_convoy_transform_places_trucks_behind_lead_in_same_lane() -> None: + lead = SimpleNamespace(location="lead") + follower = SimpleNamespace( + road_id=1, + section_id=0, + lane_id=-1, + s=88.0, + transform=SimpleNamespace(location="follower", rotation="same lane"), + ) + lead_waypoint = SimpleNamespace( + road_id=1, lane_id=-1, previous=lambda distance: [follower] if distance == 2 * CONVOY_SPACING else [] + ) + road_map = SimpleNamespace(get_waypoint=lambda location: lead_waypoint if location == "lead" else None) + + transform = convoy_transform(road_map, lead, 2) + + assert transform is follower.transform + + +def test_convoy_anchor_points_exclude_junction_crossings() -> None: + safe = SimpleNamespace(location="safe") + junction = SimpleNamespace(location="junction") + safe_follower = SimpleNamespace( + road_id=1, + section_id=0, + lane_id=-1, + s=0.0, + transform=SimpleNamespace(), + ) + waypoints = { + "safe": SimpleNamespace(road_id=1, lane_id=-1, previous=lambda _distance: [safe_follower]), + "junction": SimpleNamespace(road_id=2, lane_id=-1, previous=lambda _distance: [safe_follower]), + } + road_map = SimpleNamespace(get_waypoint=lambda location: waypoints[location]) + + assert convoy_anchor_points(road_map, [junction, safe], followers=1) == [safe] + + +def test_resting_claim_preserves_autonomy_until_driver_input() -> None: + autopilot = [] + controls = [] + vehicle = fake_vehicle( + set_autopilot=lambda enabled, port: autopilot.append((enabled, port)), + apply_control=controls.append, + ) + camera = fake_sensor() + truck = Truck("truck1", vehicle, [camera, camera], fake_sensor(), 8000, road_map=SimpleNamespace()) + + truck.actuate(Accepted(state=DriveState.stopped()), has_driver=True) + + assert truck.autonomous + assert autopilot == [(True, 8000)] + assert controls == [] + + truck.actuate( + Accepted(state=DriveState(throttle=0.2, brake=0.0, gear=Gear.FORWARD)), + has_driver=True, + ) + + assert not truck.autonomous + assert autopilot[-1] == (False, 8000) + assert controls[-1].throttle == 0.2 + + +def test_released_truck_merges_onto_its_lane_before_autonomy_resumes() -> None: + autopilot = [] + controls = [] + # A lane along +x through y=0; the truck starts 2 m to its left at speed. + forward = vector(x=1.0) + + def ahead(distance: float) -> SimpleNamespace: + return SimpleNamespace( + transform=SimpleNamespace(location=vector(x=distance), get_forward_vector=lambda: forward), + is_junction=False, + next=lambda step: [ahead(distance + step)], + ) + + lane = SimpleNamespace( + transform=SimpleNamespace(location=vector(), get_forward_vector=lambda: forward), + lane_type="driving", + is_junction=False, + get_left_lane=lambda: None, + get_right_lane=lambda: None, + next=lambda distance: [ahead(distance)], + ) + location = vector(y=-2.0) + vehicle = fake_vehicle( + set_autopilot=lambda enabled, port: autopilot.append((enabled, port)), + apply_control=controls.append, + get_transform=lambda: SimpleNamespace( + location=location, + rotation=SimpleNamespace(yaw=0.0), + get_forward_vector=lambda: forward, + get_right_vector=lambda: vector(y=1.0), + ), + get_velocity=lambda: vector(x=10.0), + get_control=lambda: SimpleNamespace(steer=0.4, throttle=0.6, brake=0.0, reverse=False), + ) + camera = fake_sensor() + road_map = SimpleNamespace(get_waypoint=lambda _location, **_kwargs: lane) + truck = Truck("truck1", vehicle, [camera, camera], fake_sensor(), 8000, road_map=road_map) + truck.actuate(Accepted(state=DriveState(throttle=0.6, steering=0.4, gear=Gear.FORWARD)), has_driver=True) + assert not truck.autonomous + + # Letting go off the lane coasts toward it rather than handing Traffic + # Manager a truck it would swerve. + truck.actuate(Accepted(state=DriveState.stopped()), has_driver=False) + + assert not truck.autonomous + assert autopilot == [(True, 8000), (False, 8000)] + assert controls[-1].steer > 0.0 + assert controls[-1].throttle == 0.0 and controls[-1].brake == 0.0 + + # On the lane: autonomy resumes, once. + location.y = 0.2 + truck.release() + assert truck.autonomous + assert autopilot[-1] == (True, 8000) + + truck.release() + assert autopilot.count((True, 8000)) == 2 + + +def test_truck_video_demand_renders_without_listening_to_a_camera() -> None: + events: list[str] = [] + demand: list[tuple[str, bool]] = [] + cameras = [fake_sensor(events, name) for name in ("sd", "hd")] + # The collision sensor listens for the truck's life. The cameras never do: + # their pictures go to the bridge, not to this process. + truck = Truck( + "truck1", + fake_vehicle(), + cameras, + fake_sensor(events, "collision"), + 8000, + lambda name, demanded: demand.append((name, demanded)), + road_map=SimpleNamespace(), + ) + + truck.set_video_demand(True) + truck.set_video_demand(True) + truck.set_video_demand(False) + + assert events == ["collision.listen"] + assert demand == [("truck1", True), ("truck1", False)] + + +def test_world_renders_only_while_any_truck_camera_is_demanded() -> None: + applied: list[bool] = [] + settings = SimpleNamespace(no_rendering_mode=False) + carla_world = SimpleNamespace( + get_settings=lambda: settings, + apply_settings=lambda value: applied.append(value.no_rendering_mode), + ) + client = SimpleNamespace( + get_world=lambda: carla_world, + get_trafficmanager=lambda _port: fake_traffic_manager(), + ) + world = World(client, 8000, 30) + trucks = [ + Truck( + name, + fake_vehicle(), + [fake_sensor(), fake_sensor()], + fake_sensor(), + 8000, + world._set_render_demand, + road_map=SimpleNamespace(), + ) + for name in ("truck1", "truck2") + ] + + trucks[0].set_video_demand(True) + trucks[1].set_video_demand(True) + trucks[0].set_video_demand(False) + trucks[1].set_video_demand(False) + + # Initial idle, first viewer, and last viewer only. Demand changing while + # another truck is visible does not flap the world. + assert applied == [True, False, True] + + +def test_failed_render_transition_does_not_commit_demand() -> None: + no_rendering = True + attempts = 0 + + def get_settings() -> SimpleNamespace: + return SimpleNamespace(no_rendering_mode=no_rendering) + + def apply_settings(settings: SimpleNamespace) -> None: + nonlocal attempts, no_rendering + attempts += 1 + # The first apply is the constructor's fixed-step setup; fail the demand one. + if attempts == 2: + raise RuntimeError("CARLA rejected settings") + no_rendering = settings.no_rendering_mode + + client = SimpleNamespace( + get_world=lambda: SimpleNamespace( + get_settings=get_settings, + apply_settings=apply_settings, + ), + get_trafficmanager=lambda _port: fake_traffic_manager(), + ) + world = World(client, 8000, 30) + + with pytest.raises(RuntimeError, match="CARLA rejected settings"): + world._set_render_demand("truck1", True) + + assert world._rendering_trucks == set() + assert no_rendering + + world._set_render_demand("truck1", True) + + assert world._rendering_trucks == {"truck1"} + assert not no_rendering + + +def test_truck_constructor_failure_cleans_spawned_actors( + monkeypatch: pytest.MonkeyPatch, +) -> None: + events = [] + blueprint = SimpleNamespace(set_attribute=lambda *_args: None, has_attribute=lambda _name: True) + transform = SimpleNamespace(location=SimpleNamespace(x=0.0, y=0.0, z=0.0)) + vehicle = SimpleNamespace( + id=1, + bounding_box=SimpleNamespace(location=SimpleNamespace(x=0.0), extent=SimpleNamespace(x=4.0)), + get_transform=lambda: transform, + destroy=lambda: events.append("vehicle.destroy"), + ) + sensors = iter(fake_sensor(events, name) for name in ("sd", "hd", "collision")) + carla_world = SimpleNamespace( + get_blueprint_library=lambda: SimpleNamespace(find=lambda _name: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [transform]), + try_spawn_actor=lambda _blueprint, _transform: vehicle, + wait_for_tick=lambda _timeout: None, + spawn_actor=lambda *_args, **_kwargs: next(sensors), + ) + world = World.__new__(World) + world._world = carla_world + world._tm_port = 8000 + world._map = None + world.trucks = [] + monkeypatch.setattr( + world_module, + "Truck", + lambda *_args, **_kwargs: (_ for _ in ()).throw(RuntimeError("autopilot failed")), + ) + + with pytest.raises(RuntimeError, match="autopilot failed"): + world.spawn(["truck1"], world_module.CameraConfig(), "front") + + assert events == [ + "sd.stop", + "sd.destroy", + "hd.stop", + "hd.destroy", + "collision.stop", + "collision.destroy", + "vehicle.destroy", + ] + + +@pytest.mark.parametrize("interrupt", [RuntimeError("collision spawn failed"), KeyboardInterrupt()]) +def test_sensor_spawn_failure_attempts_every_cleanup(interrupt: BaseException) -> None: + """A refused spawn and a SIGTERM mid-RPC both leave the vehicle in the world unless cleaned here.""" + events = [] + blueprint = SimpleNamespace(set_attribute=lambda *_args: None, has_attribute=lambda _name: True) + transform = SimpleNamespace(location=SimpleNamespace(x=0.0, y=0.0, z=0.0)) + vehicle = SimpleNamespace( + id=1, + bounding_box=SimpleNamespace(location=SimpleNamespace(x=0.0), extent=SimpleNamespace(x=4.0)), + get_transform=lambda: transform, + destroy=lambda: events.append("vehicle.destroy"), + ) + + def fail_sensor_destroy() -> None: + events.append("sd.destroy") + raise RuntimeError("sensor already gone") + + # The collision sensor is spawned last, so its failure is the case where + # cameras are already in the world and one of them refuses to be destroyed. + spawned = iter( + ( + SimpleNamespace(destroy=fail_sensor_destroy), + SimpleNamespace(destroy=lambda: events.append("hd.destroy")), + interrupt, + ) + ) + + def spawn_actor(*_args, **_kwargs): + result = next(spawned) + if isinstance(result, BaseException): + raise result + return result + + carla_world = SimpleNamespace( + get_blueprint_library=lambda: SimpleNamespace(find=lambda _name: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [transform]), + try_spawn_actor=lambda _blueprint, _transform: vehicle, + wait_for_tick=lambda _timeout: None, + spawn_actor=spawn_actor, + ) + world = World.__new__(World) + world._world = carla_world + world._tm_port = 8000 + world._map = None + world.trucks = [] + + with pytest.raises(type(interrupt)): + world.spawn(["truck1"], world_module.CameraConfig(), "front") + + assert events == ["sd.destroy", "hd.destroy", "vehicle.destroy"] + + +def test_spawn_tick_failure_destroys_the_vehicle() -> None: + """The vehicle is in the world before its landing tick, and `destroy` cannot see it yet.""" + events = [] + blueprint = SimpleNamespace(set_attribute=lambda *_args: None, has_attribute=lambda _name: True) + transform = SimpleNamespace(location=SimpleNamespace(x=0.0, y=0.0, z=0.0)) + vehicle = SimpleNamespace(id=1, destroy=lambda: events.append("vehicle.destroy")) + + def wait_for_tick(_timeout: float) -> None: + raise RuntimeError("time-out while waiting for the simulator") + + world = World.__new__(World) + world._world = SimpleNamespace( + get_blueprint_library=lambda: SimpleNamespace(find=lambda _name: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [transform]), + try_spawn_actor=lambda _blueprint, _transform: vehicle, + wait_for_tick=wait_for_tick, + spawn_actor=lambda *_args, **_kwargs: pytest.fail("no sensor spawns before the tick"), + ) + world._tm_port = 8000 + world._map = None + world.trucks = [] + + with pytest.raises(RuntimeError, match="time-out"): + world.spawn(["truck1"], world_module.CameraConfig(), "front") + + assert events == ["vehicle.destroy"] + + +def test_every_truck_is_protected_from_traffic_manager_idle_deletion( + monkeypatch: pytest.MonkeyPatch, +) -> None: + # CARLA 0.9.16's ALSM excludes role_name=hero from stuck-vehicle deletion. + # Capture the role at spawn, before the shared blueprint is reused. + attributes: dict[str, str] = {} + roles: list[str] = [] + blueprint = SimpleNamespace(set_attribute=attributes.__setitem__, has_attribute=lambda _name: True) + transform = SimpleNamespace(location=vector()) + + def spawn_vehicle(_blueprint, _transform): + roles.append(attributes["role_name"]) + return fake_vehicle( + id=len(roles), + bounding_box=SimpleNamespace(location=vector(), extent=vector(x=4)), + ) + + world = World.__new__(World) + world._world = SimpleNamespace( + get_blueprint_library=lambda: SimpleNamespace(find=lambda _name: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [transform] * 3), + try_spawn_actor=spawn_vehicle, + wait_for_tick=lambda _timeout: None, + spawn_actor=lambda *_args, **_kwargs: fake_sensor(), + ) + world._tm_port = 8000 + world._map = None + world.trucks = [] + monkeypatch.setattr(world_module, "convoy_anchor_points", lambda _map, points, _count: points) + monkeypatch.setattr(world_module, "convoy_transform", lambda *_args: transform) + + world.spawn(["truck1", "truck2", "truck3"], CameraConfig(), "front") + + assert roles == ["hero", "hero", "hero"] + assert all(truck._autonomous for truck in world.trucks) + + +def spawning_world(blueprint: SimpleNamespace, spawn_actor) -> World: + transform = SimpleNamespace(location=vector()) + world = World.__new__(World) + world._world = SimpleNamespace( + get_blueprint_library=lambda: SimpleNamespace(find=lambda _name: blueprint), + get_map=lambda: SimpleNamespace(get_spawn_points=lambda: [transform]), + try_spawn_actor=lambda _blueprint, _transform: fake_vehicle( + bounding_box=SimpleNamespace(location=vector(), extent=vector(x=4)), + destroy=lambda: None, + ), + wait_for_tick=lambda _timeout: None, + spawn_actor=spawn_actor, + ) + world._tm_port = 8000 + world._map = None + world.trucks = [] + return world + + +def test_each_camera_renders_for_its_trucks_bridge_stream() -> None: + attributes: dict[str, str] = {} + streams: list[str] = [] + blueprint = SimpleNamespace(set_attribute=attributes.__setitem__, has_attribute=lambda _name: True) + + def spawn_actor(*_args, **_kwargs): + streams.append(attributes.get(world_module.BRIDGE_ATTRIBUTE, "")) + return fake_sensor() + + spawning_world(blueprint, spawn_actor).spawn(["truck1"], CameraConfig(), "front") + + # One camera per rendition, then the collision sensor. + assert streams[:2] == ["truck1/front", "truck1/front"] + + +def test_a_simulator_without_the_bridge_is_refused_before_spawning() -> None: + spawned: list[object] = [] + blueprint = SimpleNamespace(set_attribute=lambda *_args: None, has_attribute=lambda _name: False) + world = spawning_world(blueprint, lambda *args, **_kwargs: spawned.append(args)) + + with pytest.raises(RuntimeError, match="lacks the GPU video bridge"): + world.spawn(["truck1"], CameraConfig(), "front") + + assert spawned == [] + assert world.trucks == [] + + +def test_sweep_removes_stale_vehicles_and_their_sensors_only() -> None: + """A previous run's trucks and cameras go; an unattached sensor is not ours.""" + events: list[str] = [] + ticks: list[float] = [] + + def actor(name: str, parent=None) -> SimpleNamespace: + return SimpleNamespace( + id=hash(name) & 0xFFFF, + parent=parent, + stop=lambda: events.append(f"{name}.stop"), + destroy=lambda: events.append(f"{name}.destroy") is None, + ) + + stale = [actor("truck1"), actor("truck2")] + sensors = [actor("truck1.camera", stale[0]), actor("truck2.collision", stale[1]), actor("aerial")] + + def fail_stop() -> None: + events.append("truck1.camera.stop") + raise RuntimeError("already culled") + + sensors[0].stop = fail_stop + actors = {"vehicle.*": stale, "sensor.*": sensors} + world = World.__new__(World) + world._world = SimpleNamespace( + get_actors=lambda: SimpleNamespace(filter=lambda pattern: actors[pattern]), + wait_for_tick=lambda timeout: ticks.append(timeout), + ) + + world.sweep() + + assert events == [ + "truck1.camera.stop", + "truck1.camera.destroy", + "truck2.collision.stop", + "truck2.collision.destroy", + "truck1.destroy", + "truck2.destroy", + ] + # Destroys land on the next tick in synchronous mode, before spawn walks the map. + assert ticks == [SPAWN_TICK_TIMEOUT] + + +def test_sweep_of_an_empty_world_touches_nothing() -> None: + world = World.__new__(World) + world._world = SimpleNamespace(get_actors=lambda: SimpleNamespace(filter=lambda _pattern: [])) + + world.sweep() + + +def test_sweep_fails_startup_when_a_stale_vehicle_survives() -> None: + """A truck the server refuses to remove is the jam itself; try them all, then fail.""" + destroyed: list[int] = [] + stale = [ + SimpleNamespace(id=1, destroy=lambda: False), + SimpleNamespace(id=2, destroy=lambda: (_ for _ in ()).throw(RuntimeError("timed out"))), + SimpleNamespace(id=3, destroy=lambda: destroyed.append(3) is None), + ] + actors = {"vehicle.*": stale, "sensor.*": []} + world = World.__new__(World) + world._world = SimpleNamespace(get_actors=lambda: SimpleNamespace(filter=lambda pattern: actors[pattern])) + + with pytest.raises(RuntimeError, match="1: server refused; 2: timed out"): + world.sweep() + + assert destroyed == [3] + + +def test_world_opens_synchronous_at_the_frame_period_and_hands_it_back() -> None: + applied: list[tuple[bool, float, bool]] = [] + settings = SimpleNamespace(no_rendering_mode=False, synchronous_mode=False, fixed_delta_seconds=0.0) + tm_modes: list[bool] = [] + client = SimpleNamespace( + get_world=lambda: SimpleNamespace( + get_settings=lambda: settings, + apply_settings=lambda value: applied.append( + (value.synchronous_mode, value.fixed_delta_seconds, value.no_rendering_mode) + ), + ), + get_trafficmanager=lambda _port: fake_traffic_manager(tm_modes), + ) + + world = World(client, 8000, 30) + assert applied == [(True, 1 / 30, True)] + assert tm_modes == [True] + + world.destroy() + assert applied[-1] == (False, 0.0, True) + assert tm_modes == [True, False] + + +def test_traffic_manager_setup_failure_restores_free_running() -> None: + """A TM that refuses sync must not leave the persistent world frozen.""" + applied: list[tuple[bool, float, bool]] = [] + settings = SimpleNamespace(no_rendering_mode=False, synchronous_mode=False, fixed_delta_seconds=0.0) + + def apply_settings(value: SimpleNamespace) -> None: + applied.append((value.synchronous_mode, value.fixed_delta_seconds, value.no_rendering_mode)) + settings.synchronous_mode = value.synchronous_mode + settings.fixed_delta_seconds = value.fixed_delta_seconds + settings.no_rendering_mode = value.no_rendering_mode + + def set_synchronous_mode(sync: bool) -> None: + if sync: + raise RuntimeError("Traffic Manager unavailable") + + client = SimpleNamespace( + get_world=lambda: SimpleNamespace(get_settings=lambda: settings, apply_settings=apply_settings), + get_trafficmanager=lambda _port: SimpleNamespace(set_synchronous_mode=set_synchronous_mode), + ) + + with pytest.raises(RuntimeError, match="Traffic Manager unavailable"): + World(client, 8000, 30) + + assert applied == [(True, 1 / 30, True), (False, 0.0, True)] + assert (settings.synchronous_mode, settings.fixed_delta_seconds) == (False, 0.0) + + +def test_next_deadline_keeps_cadence_from_the_previous_deadline() -> None: + # A tick that finished early or late leaves the next deadline untouched. + assert next_deadline(10.0, 10.01, 0.1) == pytest.approx(10.1) + assert next_deadline(10.0, 10.09, 0.1) == pytest.approx(10.1) + + +def test_next_deadline_reanchors_after_a_hitch_instead_of_bursting() -> None: + # A tick that overran by more than a period does not owe the missed ticks. + assert next_deadline(10.0, 10.25, 0.1) == pytest.approx(10.25) + + +def test_clock_failure_reaches_the_session() -> None: + def tick() -> None: + raise RuntimeError("server gone") + + async def scenario() -> None: + clock = Clock(SimpleNamespace(tick=tick), 30) + clock.start() + try: + with pytest.raises(RuntimeError, match="simulation clock stopped") as err: + await asyncio.wait_for(clock.wait_failed(), timeout=1.0) + assert "server gone" in str(err.value.__cause__) + finally: + clock.stop() + + asyncio.run(scenario()) + + +def test_clock_is_held_until_every_truck_is_spawned() -> None: + """`sweep` and `spawn` wait on ticks, so the world must step through them.""" + clock = Clock(SimpleNamespace(tick=lambda: None), 30) + assert clock.running + + clock.release(world_module.STARTUP_HOLD) + + assert not clock.running + + +def test_clock_follows_camera_demand() -> None: + settings = SimpleNamespace(no_rendering_mode=False) + carla_world = SimpleNamespace( + get_settings=lambda: settings, + apply_settings=lambda value: setattr(settings, "no_rendering_mode", value.no_rendering_mode), + ) + client = SimpleNamespace( + get_world=lambda: carla_world, + get_trafficmanager=lambda _port: fake_traffic_manager(), + ) + world = World(client, 8000, 30) + world.ready() + + assert not world.clock.running + + world._set_render_demand("truck1", True) + assert world.clock.running + + # A second viewer must not let the first one's departure park the world. + world._set_render_demand("truck2", True) + world._set_render_demand("truck1", False) + assert world.clock.running + + world._set_render_demand("truck2", False) + assert not world.clock.running + + +def test_holds_do_not_release_on_each_others_behalf() -> None: + clock = Clock(SimpleNamespace(tick=lambda: None), 30) + clock.release(world_module.STARTUP_HOLD) + + clock.hold(world_module.RENDER_HOLD) + clock.hold(world_module.drive_hold("truck1")) + clock.release(world_module.RENDER_HOLD) + + # Nobody is watching, but a driver still holds the lease: freezing here + # would accept the driver's commands and move nothing. + assert clock.running + + clock.release(world_module.drive_hold("truck1")) + assert not clock.running + + +def test_parked_clock_keeps_beating_so_the_watchdog_lets_it_rest() -> None: + """A parked clock is idle by design; the watchdog only exists for a wedged one.""" + ticks = 0 + + def tick() -> None: + nonlocal ticks + ticks += 1 + + async def scenario() -> None: + clock = Clock(SimpleNamespace(tick=tick), 30) + clock.release(world_module.STARTUP_HOLD) + clock.start() + try: + for _ in range(100): + if clock.heartbeat.age() > 0.0: + break + await asyncio.sleep(0.01) + # Parked, but still proving the thread is alive. + assert clock.heartbeat.age() < 0.5 + assert ticks == 0 + + clock.hold(world_module.RENDER_HOLD) + for _ in range(100): + if ticks: + break + await asyncio.sleep(0.01) + assert ticks > 0 + finally: + clock.stop() + + asyncio.run(scenario()) diff --git a/demo/carla/sim/trucks.json b/demo/carla/sim/trucks.json new file mode 100644 index 00000000000..f071f4059b3 --- /dev/null +++ b/demo/carla/sim/trucks.json @@ -0,0 +1 @@ +["truck1", "truck2"] diff --git a/demo/carla/sim/typings/carla.pyi b/demo/carla/sim/typings/carla.pyi new file mode 100644 index 00000000000..e27843e5338 --- /dev/null +++ b/demo/carla/sim/typings/carla.pyi @@ -0,0 +1,3 @@ +from typing import Any + +def __getattr__(name: str) -> Any: ... diff --git a/demo/carla/sim/venv.sh b/demo/carla/sim/venv.sh new file mode 100755 index 00000000000..5e7c7fed765 --- /dev/null +++ b/demo/carla/sim/venv.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env bash +# +# Create (or refresh) the CARLA worker's virtualenv. +# +# Python 3.12 exactly: CARLA 0.9.16 publishes wheels for cp310-cp312 only. +# +# moq comes from this checkout's py/ rather than PyPI, so the worker always +# runs the bindings of the tree it ships in: moq-ffi built by maturin from +# rs/moq-ffi, and the pure-Python moq-rs wrapper installed editable over it. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo="$(cd "$here/../../.." && pwd)" +venv="$here/.venv" + +# The devshell exports a PYTHONPATH into its own Python's site-packages. A venv +# does not override it, so a 3.12 interpreter would import packages built for +# another version and fail somewhere unrelated. python.sh does the same. +export PYTHONPATH= + +uv venv --allow-existing --python 3.12 "$venv" + +# The worker and its CARLA client wheel. +uv pip install --python "$venv/bin/python" --quiet -e "$here" + +# maturin comes from the root workspace's locked dev tools, the same one +# `just py` builds with, but installs into this venv: VIRTUAL_ENV picks the +# target. Release, since this is the runtime the worker publishes through. +uv sync --project "$repo" --locked --no-install-workspace --quiet +( + cd "$repo/py/moq-ffi" + VIRTUAL_ENV="$venv" "$repo/.venv/bin/maturin" develop --locked --release --uv +) + +# --no-deps: moq-rs depends on moq-ffi, which maturin just installed from this +# tree; resolving it again would fetch a release from PyPI over it. +uv pip install --python "$venv/bin/python" --quiet --no-deps -e "$repo/py/moq-rs" + +echo "venv: ready at $venv" >&2 diff --git a/demo/carla/web/bunfig.toml b/demo/carla/web/bunfig.toml new file mode 100644 index 00000000000..b0b3f89b138 --- /dev/null +++ b/demo/carla/web/bunfig.toml @@ -0,0 +1,2 @@ +[test] +preload = ["./test/worklet.ts"] diff --git a/demo/carla/web/index.html b/demo/carla/web/index.html new file mode 100644 index 00000000000..0489e974d1b --- /dev/null +++ b/demo/carla/web/index.html @@ -0,0 +1,54 @@ + + + + + + CARLA · MoQ demo + + + +
+

MoQ CARLA

+ + Connecting… +
+ + +
+
+ +
+
+ +
+ + + + +
+
+
+
Controls

Select a vehicle, then choose Take control or use a drive key. W throttle · S brake · A / D steer · C center · Space / G gear while braking.

E emergency stop · R reset stop · Esc release control, then return to the map. Keyboard inputs hold when released.

Plug in a steering wheel and turn it to connect. Select its name in the header to calibrate. Move it with the throttle released to take over.

A generic MoQ relay carries the video, telemetry, and controls.
+ + + diff --git a/demo/carla/web/package.json b/demo/carla/web/package.json new file mode 100644 index 00000000000..656a91b164a --- /dev/null +++ b/demo/carla/web/package.json @@ -0,0 +1,28 @@ +{ + "name": "@moq/demo-carla", + "private": true, + "type": "module", + "version": "0.1.0", + "description": "MoQ CARLA Demo - Drive simulated vehicles over MoQ", + "license": "(MIT OR Apache-2.0)", + "scripts": { + "dev": "vite --open", + "build": "vite build", + "check": "tsc --noEmit", + "test": "bun test" + }, + "dependencies": { + "@moq/clock": "workspace:^", + "@moq/json": "workspace:^", + "@moq/net": "workspace:^", + "@moq/watch": "workspace:^", + "zod": "^4.6.5" + }, + "devDependencies": { + "@types/bun": "^1.4.2", + "rolldown": "^1.2.12", + "typescript": "7.0.2", + "vite": "^8.3.2", + "wrangler": "^4.147.0" + } +} diff --git a/demo/carla/web/src/calibration.css b/demo/carla/web/src/calibration.css new file mode 100644 index 00000000000..35a506d9df4 --- /dev/null +++ b/demo/carla/web/src/calibration.css @@ -0,0 +1,105 @@ +#wheel-cal { + position: fixed; + inset: 0; + z-index: 10; + background: rgba(0, 0, 0, 0.75); + display: flex; + align-items: center; + justify-content: center; + padding: 1.25rem; +} +#wheel-cal[hidden] { + display: none; +} +.cal-box { + background: var(--panel); + border: 1px solid var(--accent); + border-radius: 8px; + padding: 1.1rem; + width: min(30rem, 100%); + display: flex; + flex-direction: column; + gap: 0.7rem; +} +.cal-box h2 { + font-size: 0.9rem; + color: var(--accent); + letter-spacing: 0.04em; +} +.cal-device { + font-size: 0.72rem; +} +.cal-progress { + font-size: 0.66rem; + color: var(--muted); + text-transform: uppercase; + letter-spacing: 0.06em; +} +.cal-prompt { + font-size: 0.8rem; + line-height: 1.5; +} +.cal-live { + font-size: 0.66rem; + color: var(--muted); + background: #000; + border-radius: 4px; + padding: 0.4rem 0.5rem; + overflow-x: auto; + white-space: nowrap; +} +.cal-warn { + font-size: 0.7rem; + color: var(--danger); +} +.cal-actions { + display: flex; + gap: 0.5rem; +} +.cal-actions button { + font: + 0.7rem ui-monospace, + monospace; + color: var(--text); + background: #000; + border: 1px solid var(--line); + border-radius: 4px; + padding: 0.3rem 0.7rem; + cursor: pointer; +} +.cal-actions button:hover { + border-color: var(--muted); +} +.cal-actions .cal-capture { + border-color: var(--accent); + color: var(--accent); +} +.cal-actions .cal-cancel { + margin-left: auto; +} +.cal-safety { + display: flex; + align-items: center; + gap: 0.6rem; + border-top: 1px solid var(--line); + padding-top: 0.7rem; + font-size: 0.66rem; + color: var(--muted); + line-height: 1.4; +} +.cal-safety .estop-btn { + font: + 600 0.68rem ui-monospace, + monospace; + letter-spacing: 0.06em; + color: #fff; + background: var(--danger); + border: 1px solid var(--danger); + border-radius: 4px; + padding: 0.25rem 0.6rem; + cursor: pointer; + flex: none; +} +.cal-safety .estop-btn:hover { + background: #c53437; +} diff --git a/demo/carla/web/src/feedback.test.ts b/demo/carla/web/src/feedback.test.ts new file mode 100644 index 00000000000..e3c4b1a762d --- /dev/null +++ b/demo/carla/web/src/feedback.test.ts @@ -0,0 +1,71 @@ +import { describe, expect, test } from "bun:test"; +import { COLLISION_ALERT_MS, collisionAge, collisionLabel, DIVERGENCE, diverged, divergence } from "./feedback.ts"; +import type { DriveState } from "./schemas.ts"; + +const REST: DriveState = { steering: 0, throttle: 0, brake: 1, gear: "neutral" }; + +describe("divergence", () => { + test("is measured minus requested, per axis", () => { + const gap = divergence( + { steering: 0.5, throttle: 0.4, brake: 0, gear: "forward" }, + { steering: 0.3, throttle: 0.45, brake: 0, gear: "forward" }, + ); + expect(gap.steering).toBeCloseTo(-0.2); + expect(gap.throttle).toBeCloseTo(0.05); + expect(gap.worst).toBeCloseTo(0.2); + expect(gap.gear).toBe(false); + }); + + test("ignores lag smaller than an actuator always has", () => { + const gap = divergence(REST, { ...REST, brake: 1 - DIVERGENCE / 2 }); + expect(diverged(gap)).toBe(false); + }); + + test("names only the axes that are actually apart", () => { + const gap = divergence( + { steering: 0.6, throttle: 0.5, brake: 0, gear: "forward" }, + { steering: 0.1, throttle: 0.49, brake: 0, gear: "forward" }, + ); + expect(diverged(gap)).toBe(true); + }); + + test("calls a truck driving the wrong way a divergence on its own", () => { + const gap = divergence( + { steering: 0, throttle: 0, brake: 0, gear: "forward" }, + { steering: 0, throttle: 0, brake: 0, gear: "reverse" }, + ); + expect(gap.gear).toBe(true); + expect(diverged(gap)).toBe(true); + }); + + test("does not fault a neutral request the simulator cannot express", () => { + // CARLA has no neutral: it holds the direction and applies zero throttle. + const gap = divergence(REST, { ...REST, gear: "forward" }); + expect(gap.gear).toBe(false); + expect(diverged(gap)).toBe(false); + }); +}); + +describe("labels", () => { + test("scales a collision impulse into kN·s", () => { + const hit = { at: 1_000_000, other: "vehicle.tesla.model3", impulse: 4210.5 }; + expect(collisionLabel(hit, 0)).toBe("vehicle.tesla.model3 · 4.2 kN·s · just now"); + expect(collisionLabel(hit, COLLISION_ALERT_MS + 4000)).toBe("vehicle.tesla.model3 · 4.2 kN·s · 12s ago"); + expect(collisionLabel({ ...hit, impulse: 640.4 }, 0)).toBe("vehicle.tesla.model3 · 640 N·s · just now"); + }); +}); + +describe("collisionAge", () => { + const hit = { at: 1_000_000, other: "static.guardrail", impulse: 100 }; + + test("ages against the truck's own timestamp, not when this player saw it", () => { + // A viewer that opens the page three minutes late still sees three + // minutes, rather than treating the retained snapshot as a fresh crash. + expect(collisionAge(hit, 1_180_000)).toBe(180_000); + }); + + test("clamps a truck whose clock runs ahead of the viewer's", () => { + expect(collisionAge(hit, 999_000)).toBe(0); + expect(collisionLabel(hit, collisionAge(hit, 999_000))).toContain("just now"); + }); +}); diff --git a/demo/carla/web/src/feedback.ts b/demo/carla/web/src/feedback.ts new file mode 100644 index 00000000000..34a1cfcbaa9 --- /dev/null +++ b/demo/carla/web/src/feedback.ts @@ -0,0 +1,84 @@ +/** + * Measured feedback: what the vehicle did, against what the driver asked for. + * + * The `accepted` state on the status track is command acknowledgement - the + * gate repeating what it let through. `measured`, `motion`, and `collision` are + * the vehicle's own response, so everything here is about the gap between the + * two, which is the only thing a teleoperator can act on. + * + * Pure formatting and arithmetic, deliberately: the rendering that uses it + * (canvas overlay, telemetry rows) is browser-tested, and this is the part with + * decisions in it. + * + * @module + */ +import type { Collision, DriveState } from "./schemas.ts"; + +/** + * How far apart the requested and measured axes have to be before the player + * calls it a divergence. + * + * Not a tolerance hiding a bug: an actuator tracking a command always lags it, + * and a heavy vehicle lags it a lot. Below this the two rails are the same line + * on screen anyway, so highlighting it would mean the mismatch warning is lit + * whenever the truck is moving, which is the same as no warning at all. + */ +export const DIVERGENCE = 0.08; + +/** One axis of the request/response gap. */ +export interface Divergence { + steering: number; + throttle: number; + brake: number; + /** True when the vehicle is driving in a direction nobody asked for. */ + gear: boolean; + /** The largest axis error, for deciding whether to say anything at all. */ + worst: number; +} + +/** Signed measured-minus-requested error per axis. */ +export function divergence(requested: DriveState, measured: DriveState): Divergence { + const steering = measured.steering - requested.steering; + const throttle = measured.throttle - requested.throttle; + const brake = measured.brake - requested.brake; + // CARLA has no neutral - it drives a neutral request as zero throttle in the + // direction it was already facing - so a neutral request can never disagree + // with the direction the physics reports. + const gear = requested.gear !== "neutral" && measured.gear !== requested.gear; + return { + steering, + throttle, + brake, + gear, + worst: Math.max(Math.abs(steering), Math.abs(throttle), Math.abs(brake)), + }; +} + +/** Whether the gap is worth drawing attention to. */ +export function diverged(gap: Divergence): boolean { + return gap.gear || gap.worst >= DIVERGENCE; +} + +/** How long a collision stays called out on the tile. */ +export const COLLISION_ALERT_MS = 8000; + +/** + * How long ago the collision happened, by the truck's clock against ours. + * + * Clamped at zero because the two are clocks on different machines: a truck + * running slightly ahead would otherwise produce a negative age, and a + * collision dated in the future reads worse than one that just happened. + */ +export function collisionAge(collision: Collision, now: number): number { + return Math.max(0, now - collision.at); +} + +/** The last collision, aged against the viewer's clock. */ +export function collisionLabel(collision: Collision, ageMs: number): string { + const impulse = + collision.impulse >= 1000 + ? `${(collision.impulse / 1000).toFixed(1)} kN·s` + : `${Math.round(collision.impulse)} N·s`; + const age = ageMs < COLLISION_ALERT_MS ? "just now" : `${Math.round(ageMs / 1000)}s ago`; + return `${collision.other} · ${impulse} · ${age}`; +} diff --git a/demo/carla/web/src/guides.test.ts b/demo/carla/web/src/guides.test.ts new file mode 100644 index 00000000000..b2ad5df9627 --- /dev/null +++ b/demo/carla/web/src/guides.test.ts @@ -0,0 +1,35 @@ +import { describe, expect, test } from "bun:test"; +import { longitudinalIndicator } from "./guides.ts"; +import type { DriveState } from "./schemas.ts"; + +function drive(patch: Partial): DriveState { + return { steering: 0, throttle: 0, brake: 0, gear: "forward", ...patch }; +} + +describe("longitudinal indicator", () => { + test("uses the truck's measured pedal instead of the request", () => { + const indicator = longitudinalIndicator(drive({ throttle: 0.8 }), drive({ throttle: 0.35 })); + expect(indicator).toMatchObject({ pedal: "throttle", amount: 0.35, source: "actual" }); + }); + + test("points forward throttle away and reverse throttle back", () => { + const forward = longitudinalIndicator(undefined, drive({ throttle: 0.5 })); + const reverse = longitudinalIndicator(undefined, drive({ throttle: 0.5, gear: "reverse" })); + expect(forward?.to).toBeGreaterThan(forward?.from ?? 0); + expect(reverse?.to).toBeLessThan(reverse?.from ?? 0); + }); + + test("braking points against the selected gear", () => { + const forward = longitudinalIndicator(undefined, drive({ brake: 0.5 })); + const reverse = longitudinalIndicator(undefined, drive({ brake: 0.5, gear: "reverse" })); + expect(forward?.to).toBeLessThan(forward?.from ?? 0); + expect(reverse?.to).toBeGreaterThan(reverse?.from ?? 0); + }); + + test("shows braking during a brief pedal overlap", () => { + expect(longitudinalIndicator(undefined, drive({ throttle: 0.7, brake: 0.2 }))).toMatchObject({ + pedal: "brake", + amount: 0.2, + }); + }); +}); diff --git a/demo/carla/web/src/guides.ts b/demo/carla/web/src/guides.ts new file mode 100644 index 00000000000..a78fb80d6f6 --- /dev/null +++ b/demo/carla/web/src/guides.ts @@ -0,0 +1,237 @@ +/** + * Backup-camera-style steering guides projected onto the video. + * + * Two curved rails that bend with a steering angle. The same projection draws + * the driver's **requested input** (a local preview of what is being sent) and + * the truck's **measured** steering off the status track, so the gap between + * them is a shape on the road rather than two numbers to subtract. + * + * @module + */ +import type { DriveState } from "./schemas.ts"; + +export interface GuideStyle { + /** Stroke color (CSS). */ + color: string; + /** Line width in CSS pixels. */ + width: number; + /** Dash pattern, or empty for solid. */ + dash?: number[]; + /** Overall opacity. */ + alpha?: number; +} + +// How far the far end of the rails swings sideways at full lock, as a fraction +// of width. Tuned to read like a reversing camera, not a literal kinematic model. +const CURVE_GAIN = 0.34; + +// Vertical extent of the projected ground plane (fractions of canvas height). +const Y_NEAR = 0.98; +const Y_FAR = 0.46; +// Half-width of the wheel track at the near and far ends (fractions of width). +const HALF_NEAR = 0.36; +const HALF_FAR = 0.06; + +const SEGMENTS = 48; + +/** One rail point. */ +interface Pt { + x: number; + y: number; +} + +/** Sample the left and right rails for a given steering angle. */ +function rails(w: number, h: number, steering: number): { left: Pt[]; right: Pt[] } { + const cx = w / 2; + const left: Pt[] = []; + const right: Pt[] = []; + for (let i = 0; i < SEGMENTS; i++) { + const t = i / (SEGMENTS - 1); + // Perspective: rails converge and rise toward the horizon. + const y = (Y_NEAR + (Y_FAR - Y_NEAR) * t) * h; + const half = (HALF_NEAR + (HALF_FAR - HALF_NEAR) * t) * w; + // Lateral swing grows with distance (t²) so the rails curve, not shear. + const shift = steering * CURVE_GAIN * t * t * w; + left.push({ x: cx - half + shift, y }); + right.push({ x: cx + half + shift, y }); + } + return { left, right }; +} + +function stroke(ctx: CanvasRenderingContext2D, pts: Pt[]) { + ctx.beginPath(); + pts.forEach((p, i) => { + if (i) ctx.lineTo(p.x, p.y); + else ctx.moveTo(p.x, p.y); + }); + ctx.stroke(); +} + +/** Draw one trajectory (two rails). */ +export function drawGuide(ctx: CanvasRenderingContext2D, w: number, h: number, drive: DriveState, style: GuideStyle) { + const { left, right } = rails(w, h, drive.steering); + + ctx.save(); + ctx.globalAlpha = style.alpha ?? 1; + ctx.lineJoin = "round"; + ctx.lineCap = "round"; + ctx.lineWidth = style.width; + ctx.strokeStyle = style.color; + ctx.setLineDash(style.dash ?? []); + stroke(ctx, left); + stroke(ctx, right); + ctx.restore(); +} + +/** + * Shade the ground between the requested trajectory and the measured one. + * + * The wedge is the difference itself: it closes to nothing while the truck is + * tracking, and opens toward the side the truck is actually going. Drawing it + * is what stops two nearly-identical curves from reading as one. + */ +export function drawDivergence( + ctx: CanvasRenderingContext2D, + w: number, + h: number, + requested: DriveState, + measured: DriveState, + style: { color: string; alpha?: number }, +) { + const want = rails(w, h, requested.steering); + const got = rails(w, h, measured.steering); + + ctx.save(); + ctx.globalAlpha = style.alpha ?? 1; + ctx.fillStyle = style.color; + for (const [a, b] of [ + [want.left, got.left], + [want.right, got.right], + ]) { + ctx.beginPath(); + a.forEach((p, i) => { + if (i) ctx.lineTo(p.x, p.y); + else ctx.moveTo(p.x, p.y); + }); + for (let i = b.length - 1; i >= 0; i--) ctx.lineTo(b[i].x, b[i].y); + ctx.closePath(); + ctx.fill(); + } + ctx.restore(); +} + +function pointAt(pts: Pt[], progress: number): Pt { + const position = Math.max(0, Math.min(1, progress)) * (pts.length - 1); + const before = Math.floor(position); + const after = Math.min(pts.length - 1, before + 1); + const mix = position - before; + return { + x: pts[before].x + (pts[after].x - pts[before].x) * mix, + y: pts[before].y + (pts[after].y - pts[before].y) * mix, + }; +} + +/** Sample a rail in either direction, including an exact endpoint. */ +function portion(pts: Pt[], from: number, to: number): Pt[] { + const count = Math.max(2, Math.ceil(Math.abs(to - from) * (pts.length - 1)) + 1); + return Array.from({ length: count }, (_, i) => pointAt(pts, from + ((to - from) * i) / (count - 1))); +} + +function centerline(left: Pt[], right: Pt[]): Pt[] { + return left.map((point, index) => ({ + x: (point.x + right[index].x) / 2, + y: (point.y + right[index].y) / 2, + })); +} + +function arrow(ctx: CanvasRenderingContext2D, pts: Pt[], color: string, width: number) { + if (pts.length < 2) return; + const tip = pts[pts.length - 1]; + const previous = pts[pts.length - 2]; + const angle = Math.atan2(tip.y - previous.y, tip.x - previous.x); + const size = width * 1.55; + + ctx.save(); + ctx.lineCap = "round"; + ctx.lineJoin = "round"; + ctx.strokeStyle = "rgba(0, 0, 0, 0.72)"; + ctx.lineWidth = width + 4; + stroke(ctx, pts); + ctx.strokeStyle = color; + ctx.lineWidth = width; + stroke(ctx, pts); + ctx.translate(tip.x, tip.y); + ctx.rotate(angle); + ctx.fillStyle = "rgba(0, 0, 0, 0.72)"; + ctx.beginPath(); + ctx.moveTo(size + 3, 0); + ctx.lineTo(-size, size * 0.72 + 2); + ctx.lineTo(-size, -size * 0.72 - 2); + ctx.closePath(); + ctx.fill(); + ctx.fillStyle = color; + ctx.beginPath(); + ctx.moveTo(size, 0); + ctx.lineTo(-size, size * 0.72); + ctx.lineTo(-size, -size * 0.72); + ctx.closePath(); + ctx.fill(); + ctx.restore(); +} + +export interface LongitudinalIndicator { + pedal: "throttle" | "brake"; + amount: number; + from: number; + to: number; + source: "actual" | "requested"; +} + +function pedal(drive: DriveState): Pick { + // Braking wins if a physical controller briefly reports both pedals. + return drive.brake > 0 + ? { pedal: "brake", amount: Math.max(0, Math.min(1, drive.brake)) } + : { pedal: "throttle", amount: Math.max(0, Math.min(1, drive.throttle)) }; +} + +/** Resolve the one arrow shown by the HUD, preferring the truck's measured state. */ +export function longitudinalIndicator( + requested: DriveState | undefined, + measured: DriveState | undefined, +): LongitudinalIndicator | undefined { + const drive = measured ?? requested; + if (!drive) return undefined; + const value = pedal(drive); + const reversing = drive.gear === "reverse"; + const pointsAway = value.pedal === "throttle" ? !reversing : reversing; + const start = pointsAway ? 0.1 : 0.9; + const end = start + (pointsAway ? 0.8 : -0.8) * value.amount; + return { ...value, from: start, to: end, source: measured ? "actual" : "requested" }; +} + +/** Draw one centered pedal arrow, using measured truck feedback when available. */ +export function drawDriveHud( + ctx: CanvasRenderingContext2D, + w: number, + h: number, + requested: DriveState | undefined, + measured: DriveState | undefined, +) { + const indicator = longitudinalIndicator(requested, measured); + if (!indicator) return; + const drive = measured ?? requested; + if (!drive) return; + const scale = Math.max(0.78, Math.min(1.15, w / 900)); + const { left, right } = rails(w, h, drive.steering); + const middle = centerline(left, right); + const throttleColor = "#39d353"; + const brakeColor = "#ff4d55"; + const arrowColor = + indicator.source === "requested" ? "#50c8ff" : indicator.pedal === "throttle" ? throttleColor : brakeColor; + const barWidth = 7 * scale; + + ctx.save(); + if (indicator.amount > 0) arrow(ctx, portion(middle, indicator.from, indicator.to), arrowColor, barWidth); + + ctx.restore(); +} diff --git a/demo/carla/web/src/hint.test.ts b/demo/carla/web/src/hint.test.ts new file mode 100644 index 00000000000..454e8c5e391 --- /dev/null +++ b/demo/carla/web/src/hint.test.ts @@ -0,0 +1,50 @@ +import { describe, expect, test } from "bun:test"; +import { type ControlHintState, controlHint } from "./hint.ts"; + +const active: ControlHintState = { + active: true, + focused: true, + estopped: false, + status: "pending", + driverId: "mine", + wheel: false, +}; + +describe("controlHint", () => { + test("distinguishes an unavailable status track from a pending lease", () => { + expect(controlHint(active)).toBe("Requesting control…"); + expect(controlHint({ ...active, status: "unavailable", statusDriver: "mine" })).toBe( + "Vehicle status lost · Control unconfirmed", + ); + }); + + test("reports another lease holder once status is available", () => { + expect(controlHint({ ...active, status: "available", statusDriver: "theirs" })).toBe("Driven by theirs"); + }); + + test("reminds both input sources about the shift interlock", () => { + const mine = { ...active, status: "available" as const, statusDriver: "mine" }; + expect(controlHint(mine)).toContain("Brake + G shifts"); + expect(controlHint({ ...mine, wheel: true })).toContain("Brake before shifting"); + }); + + test("names the e-stop actor independently of the lease holder", () => { + expect(controlHint({ ...active, estopped: true, estopBy: "bystander", statusDriver: "holder" })).toBe( + "Stopped by bystander - R to reset", + ); + expect(controlHint({ ...active, active: false, focused: false, estopped: true, estopBy: "bystander" })).toBe( + "Stopped by bystander - Reset to resume", + ); + }); + + test("preserves preview and focus without requesting control", () => { + expect(controlHint({ ...active, active: false, focused: false })).toBe("Open camera"); + expect(controlHint({ ...active, active: false })).toBe("Waiting for vehicle status"); + expect(controlHint({ ...active, active: false, status: "available", statusDriver: "theirs" })).toBe( + "Driven by theirs", + ); + expect(controlHint({ ...active, active: false, status: "unavailable" })).toBe( + "Control unavailable · Esc to leave", + ); + }); +}); diff --git a/demo/carla/web/src/hint.ts b/demo/carla/web/src/hint.ts new file mode 100644 index 00000000000..5ef977b9430 --- /dev/null +++ b/demo/carla/web/src/hint.ts @@ -0,0 +1,33 @@ +export interface ControlHintState { + active: boolean; + focused: boolean; + estopped: boolean; + estopBy?: string; + status: "pending" | "available" | "unavailable"; + driverId?: string; + statusDriver?: string; + wheel: boolean; +} + +/** Describe the control state without confusing a dead status track for a pending lease. */ +export function controlHint(state: ControlHintState): string { + if (state.estopped) { + const actor = state.estopBy ? ` by ${state.estopBy}` : ""; + const action = state.focused ? "R to reset" : "Reset to resume"; + return `Stopped${actor} - ${action}`; + } + if (!state.active && !state.focused) return "Open camera"; + if (!state.active && state.status === "unavailable") { + return "Control unavailable · Esc to leave"; + } + if (!state.active && state.statusDriver) return `Driven by ${state.statusDriver}`; + if (!state.active && state.status === "pending") return "Waiting for vehicle status"; + if (!state.active) return "Take control to drive"; + if (state.status === "unavailable") return "Vehicle status lost · Control unconfirmed"; + if (!state.driverId || !state.statusDriver) return "Requesting control…"; + if (state.statusDriver !== state.driverId) return `Driven by ${state.statusDriver}`; + if (state.wheel) { + return "Wheel · Brake before shifting · Esc to release"; + } + return "W/S pedals · A/D steer · Brake + G shifts · Esc to release"; +} diff --git a/demo/carla/web/src/input.test.ts b/demo/carla/web/src/input.test.ts new file mode 100644 index 00000000000..df33d609522 --- /dev/null +++ b/demo/carla/web/src/input.test.ts @@ -0,0 +1,241 @@ +import { describe, expect, test } from "bun:test"; +import { + applyKeys, + canShift, + changed, + type DriveTarget, + longitudinalAxis, + moveToward, + REST, + stepDrive, + takeoverState, +} from "./input.ts"; +import type { DriveState } from "./schemas.ts"; + +const DT = 1 / 60; + +describe("takeoverState", () => { + test("adopts the vehicle's measured controls", () => { + const measured: DriveState = { steering: -0.4, throttle: 0.6, brake: 0.2, gear: "reverse" }; + expect(takeoverState(measured)).toEqual(measured); + expect(takeoverState(measured)).not.toBe(measured); + }); + + test("coasts in forward before the first measured feedback", () => { + expect(takeoverState(undefined)).toEqual({ + steering: 0, + throttle: 0, + brake: 0, + gear: "forward", + }); + }); +}); + +function run(state: DriveState, keys: string[] = [], seconds = 1): DriveState { + const held = new Set(keys); + let s = { ...state }; + for (let t = 0; t < seconds; t += DT) { + s = { ...s, ...applyKeys(s, held, DT) }; + } + return s; +} + +describe("applyKeys", () => { + test("holds everything with no input", () => { + const s = run({ steering: -0.4, throttle: 0.6, brake: 0.2, gear: "forward" }, [], 5); + expect(s.steering).toBeCloseTo(-0.4); + expect(s.throttle).toBeCloseTo(0.6); + expect(s.brake).toBeCloseTo(0.2); + }); + + test("resting brake never drops without input", () => { + expect(run({ ...REST }, [], 5).brake).toBe(1); + }); + + test("steering holds its angle when the key is released", () => { + const turned = run({ ...REST }, ["d"], 1); + expect(turned.steering).toBeGreaterThan(0.2); + expect(run(turned, [], 5).steering).toBe(turned.steering); + }); + + test("steering is slew-limited, not instant", () => { + // A tap responds visibly, while full lock still requires a deliberate hold. + expect(run({ ...REST }, ["d"], 0.25).steering).toBeGreaterThan(0.1); + expect(run({ ...REST }, ["d"], 1).steering).toBeLessThan(0.75); + expect(run({ ...REST }, ["d"], 2).steering).toBeLessThan(1); + expect(run({ ...REST }, ["d"], 2.5).steering).toBe(1); + }); + + test("C recenters the steering gradually", () => { + const turned = run({ ...REST }, ["a"], 5); + expect(turned.steering).toBe(-1); + const centering = run(turned, ["c"], 1); + expect(centering.steering).toBeGreaterThan(-1); + expect(centering.steering).toBeLessThan(0); + expect(run(turned, ["c"], 5).steering).toBe(0); + }); + + test("opposite steering keys cancel", () => { + expect(run({ ...REST }, ["a", "d"], 2).steering).toBe(0); + }); + + test("W releases the resting brake promptly, then ramps throttle", () => { + const firstFrame = applyKeys({ ...REST }, new Set(["w"]), DT); + expect(firstFrame.brake).toBeGreaterThan(0); + expect(firstFrame.brake).toBeLessThan(1); + expect(firstFrame.throttle).toBe(0); + + const early = run({ ...REST }, ["w"], 1); + expect(early.brake).toBe(0); + expect(early.throttle).toBeGreaterThan(0.05); + expect(early.throttle).toBeLessThan(0.2); + const late = run({ ...REST }, ["w"], 4); + expect(late.brake).toBe(0); + expect(late.throttle).toBe(1); + }); + + test("W slews an applied brake off before rebuilding throttle", () => { + const braking: DriveState = { steering: 0, throttle: 0, brake: 0.6, gear: "forward" }; + const firstFrame = applyKeys(braking, new Set(["w"]), DT); + expect(firstFrame.brake).toBeGreaterThan(0); + expect(firstFrame.brake).toBeLessThan(braking.brake); + expect(firstFrame.throttle).toBe(0); + + let released = braking; + while (released.brake > 0) { + released = { ...released, ...applyKeys(released, new Set(["w"]), DT) }; + expect(released.throttle).toBe(0); + } + expect(applyKeys(released, new Set(["w"]), DT).throttle).toBeGreaterThan(0); + }); + + test("W drains wheel pedal overlap before releasing the brake", () => { + const wheel: DriveState = { steering: 0, throttle: 0.6, brake: 0.4, gear: "forward" }; + const firstFrame = applyKeys(wheel, new Set(["w"]), DT); + expect(firstFrame.throttle).toBeLessThan(wheel.throttle); + expect(firstFrame.brake).toBe(wheel.brake); + + let normalized = wheel; + while (normalized.throttle > 0) { + normalized = { ...normalized, ...applyKeys(normalized, new Set(["w"]), DT) }; + expect(normalized.brake).toBe(wheel.brake); + } + const releasing = applyKeys(normalized, new Set(["w"]), DT); + expect(releasing.throttle).toBe(0); + expect(releasing.brake).toBeLessThan(wheel.brake); + }); + + test("S cuts throttle before braking, and wins over W", () => { + const rolling: DriveState = { steering: 0, throttle: 1, brake: 0, gear: "forward" }; + const early = run({ ...rolling }, ["w", "s"], 0.5); + expect(early.throttle).toBeLessThan(1); + expect(early.brake).toBe(0); + const late = run({ ...rolling }, ["w", "s"], 10); + expect(late.throttle).toBe(0); + expect(late.brake).toBe(1); + }); +}); + +describe("longitudinalAxis", () => { + test("follows brake release before throttle application", () => { + expect(longitudinalAxis({ throttle: 0.5, brake: 1 }, new Set(["w"]))).toBe("throttle"); + expect(longitudinalAxis({ throttle: 0, brake: 1 }, new Set(["w"]))).toBe("brake"); + expect(longitudinalAxis({ throttle: 0, brake: 0 }, new Set(["w"]))).toBe("throttle"); + }); + + test("follows throttle release before brake application", () => { + expect(longitudinalAxis({ throttle: 0.5, brake: 0 }, new Set(["s"]))).toBe("throttle"); + expect(longitudinalAxis({ throttle: 0, brake: 0 }, new Set(["s"]))).toBe("brake"); + }); + + test("matches S winning over W", () => { + expect(longitudinalAxis({ throttle: 0.5, brake: 0 }, new Set(["w", "s"]))).toBe("throttle"); + }); +}); + +describe("moveToward", () => { + test("does not overshoot", () => { + expect(moveToward(0.9, 1, 0.5)).toBe(1); + expect(moveToward(0.1, 0, 0.5)).toBe(0); + }); +}); + +describe("changed", () => { + test("ignores sub-epsilon wiggle", () => { + expect(changed({ ...REST }, { steering: 0.005, throttle: 0, brake: 1 })).toBe(false); + expect(changed({ ...REST }, { steering: 0.05, throttle: 0, brake: 1 })).toBe(true); + }); +}); + +describe("canShift", () => { + test("requires released throttle and any amount of brake", () => { + expect(canShift({ throttle: 0, brake: 0.001 })).toBe(true); + expect(canShift({ throttle: 0.01, brake: 1 })).toBe(true); + expect(canShift({ throttle: 0.02, brake: 1 })).toBe(false); + expect(canShift({ throttle: 0, brake: 0 })).toBe(false); + }); +}); + +describe("stepDrive", () => { + function target(state: DriveState = { ...REST }) { + const calls: Array> = []; + let cur = state; + let stopped = false; + const t: DriveTarget = { + drive: { peek: () => cur }, + estopped: { peek: () => stopped }, + setDrive(patch) { + calls.push(patch); + cur = { ...cur, ...patch }; + }, + }; + return { + t, + calls, + get: () => cur, + stop: () => { + stopped = true; + }, + }; + } + + // One frame of steering at 120 Hz is 0.45/120 = 0.00375, under the publish + // epsilon, so a loop that skipped sub-epsilon frames would discard every one + // and held keys would move nothing. Integration must reach the target + // regardless of size. + test("commits a frame whose delta is under the publish epsilon", () => { + const { t, calls, get } = target(); + const dt = 1 / 120; + const next = applyKeys(get(), new Set(["d"]), dt); + expect(changed(get(), next)).toBe(false); // below the epsilon, by construction + stepDrive(t, next); + expect(calls).toHaveLength(1); + expect(get().steering).toBeGreaterThan(0); + }); + + test("accumulates a full second of 60 Hz frames", () => { + const { t, get } = target(); + const dt = 1 / 60; + for (let i = 0; i < 60; i++) stepDrive(t, applyKeys(get(), new Set(["d"]), dt)); + expect(get().steering).toBeCloseTo(0.45, 2); + }); + + test("auto-selects forward while the truck begins releasing its brake", () => { + const { t, get } = target(); + stepDrive(t, { steering: 0, throttle: 0, brake: 0.975 }); + expect(get()).toEqual({ steering: 0, throttle: 0, brake: 0.975, gear: "forward" }); + }); + + test("does not auto-shift once the truck has fully released the brake", () => { + const { t, get } = target({ ...REST, brake: 0.1 }); + stepDrive(t, { steering: 0, throttle: 0, brake: 0 }); + expect(get().gear).toBe("neutral"); + }); + + test("a latched e-stop swallows the frame", () => { + const { t, calls, get, stop } = target(); + stop(); + stepDrive(t, applyKeys(get(), new Set(["d"]), 1 / 60)); + expect(calls).toHaveLength(0); + }); +}); diff --git a/demo/carla/web/src/input.ts b/demo/carla/web/src/input.ts new file mode 100644 index 00000000000..c93f293f7b6 --- /dev/null +++ b/demo/carla/web/src/input.ts @@ -0,0 +1,154 @@ +/** + * Pure keyboard-to-drive-state helpers. + * + * The simulated vehicles are heavy trucks: inputs are deliberate, slew-limited + * nudges, and axes **hold** their position when the keys are released - steering + * does not snap back to center and the throttle does not drop to zero. The only + * instant transition is e-stop (handled elsewhere), which latches into REST. + * + * @module + */ + +import type { DriveState } from "./schemas.ts"; + +/** The truck's safe resting state. */ +export const REST: DriveState = { steering: 0, throttle: 0, brake: 1, gear: "neutral" }; + +/** Initial manual handoff: preserve live feedback, or coast when none exists. */ +export function takeoverState(measured: DriveState | undefined): DriveState { + return measured ? { ...measured } : { steering: 0, throttle: 0, brake: 0, gear: "forward" }; +} + +/** Slew rates in normalized units per second. Heavy responses, with fast safety actions. */ +export const RATES = { + /** Steering: full lock takes ~2.2 s of held key. */ + steer: 0.45, + /** Recentering (C) is a touch quicker than turning further. */ + steerCenter: 0.65, + /** Throttle takes 2.5 s to build from idle to full. */ + throttleUp: 0.4, + /** ...and backs off faster than it builds. */ + throttleDown: 1.2, + /** Brake release is deliberate: full to clear takes ~0.67 s. */ + brakeDown: 1.5, + /** Brakes apply quickly (safety), */ + brakeUp: 1.5, +}; + +/** A gear change requires some brake and a released throttle. */ +export const SHIFT_MAX_THROTTLE = 0.01; + +/** Mirror the truck-side gear interlock so rejected shifts get immediate UI feedback. */ +export function canShift(state: Pick): boolean { + return state.brake > 0 && state.throttle <= SHIFT_MAX_THROTTLE; +} + +/** Move `current` toward `target` by at most `step`, without overshooting. */ +export function moveToward(current: number, target: number, step: number): number { + if (Math.abs(target - current) <= step) return target; + return current + Math.sign(target - current) * step; +} + +/** + * Integrate the held keys into the next analog state over `dt` seconds. + * + * - `A`/`D` (or arrows) slew the steering toward full lock; releasing holds + * the current angle, and `C` slews it back to center. + * - `W` first drains any throttle/brake overlap from a wheel, then releases the + * brake promptly and builds throttle. + * - `S` first eases the throttle off, then applies brake (so it always wins + * over `W`). + * - No keys: everything holds where it is. + */ +export function applyKeys( + current: DriveState, + held: ReadonlySet, + dt: number, +): Pick { + let { steering, throttle, brake } = current; + + const left = held.has("a") || held.has("arrowleft"); + const right = held.has("d") || held.has("arrowright"); + const accelerating = held.has("w") || held.has("arrowup"); + const braking = held.has("s") || held.has("arrowdown"); + + if (left !== right) { + const dir = right ? 1 : -1; + steering = Math.max(-1, Math.min(1, steering + dir * RATES.steer * dt)); + } else if (held.has("c")) { + steering = moveToward(steering, 0, RATES.steerCenter * dt); + } + + if (braking) { + // Ease off the throttle before touching the brakes. + if (throttle > 0) { + throttle = moveToward(throttle, 0, RATES.throttleDown * dt); + } else { + brake = moveToward(brake, 1, RATES.brakeUp * dt); + } + } else if (accelerating) { + // A wheel can apply both pedals independently. Drain that overlap before + // releasing the brake, then finish braking before building throttle. + if (throttle > 0 && brake > 0) { + throttle = moveToward(throttle, 0, RATES.throttleDown * dt); + } else if (brake > 0) { + brake = moveToward(brake, 0, RATES.brakeDown * dt); + } else { + throttle = moveToward(throttle, 1, RATES.throttleUp * dt); + } + } + + return { steering, throttle, brake }; +} + +/** The longitudinal axis the held keys will move next, matching {@link applyKeys}' phase ordering. */ +export function longitudinalAxis( + current: Pick, + held: ReadonlySet, +): "throttle" | "brake" | undefined { + const accelerating = held.has("w") || held.has("arrowup"); + const braking = held.has("s") || held.has("arrowdown"); + if (braking) return current.throttle > 0 ? "throttle" : "brake"; + if (accelerating) { + if (current.throttle > 0 && current.brake > 0) return "throttle"; + return current.brake > 0 ? "brake" : "throttle"; + } + return undefined; +} + +/** Return whether an analog state differs enough to publish. */ +export function changed(current: DriveState, next: Pick): boolean { + const epsilon = 0.01; + return ( + Math.abs(current.steering - next.steering) > epsilon || + Math.abs(current.throttle - next.throttle) > epsilon || + Math.abs(current.brake - next.brake) > epsilon + ); +} + +/** The subset of `Truck` the control loop drives, so the loop is testable without one. */ +export interface DriveTarget { + readonly drive: { peek(): DriveState }; + readonly estopped: { peek(): boolean }; + setDrive(patch: Partial): void; +} + +/** + * One frame of the control loop. + * + * Hands the integrated state to the target UNCONDITIONALLY. Gating this on a + * per-frame delta would freeze steering: at 120 Hz, one frame moves 0.45/120 = + * 0.00375, under the publish epsilon, so every frame would be discarded. + * Throttling belongs on the wire (see `Truck.setDrive`), which measures against + * the last state actually sent. The truck shifts itself into forward on the + * first brake-release frame, while the brake is still applied, so W/pedal + * acceleration works without bypassing the gear interlock. + */ +export function stepDrive(target: DriveTarget, next: Pick): void { + if (target.estopped.peek()) return; + const current = target.drive.peek(); + const requested = { ...current, ...next }; + const gear = + current.gear === "neutral" && next.brake < current.brake && canShift(requested) ? "forward" : undefined; + target.setDrive(gear ? { ...next, gear } : next); +} diff --git a/demo/carla/web/src/interaction.test.ts b/demo/carla/web/src/interaction.test.ts new file mode 100644 index 00000000000..ab72a00a4ee --- /dev/null +++ b/demo/carla/web/src/interaction.test.ts @@ -0,0 +1,107 @@ +import { describe, expect, test } from "bun:test"; +import { focusFromHash, hashWithFocus, Interaction } from "./interaction.ts"; + +describe("Interaction", () => { + test("focuses a truck without taking control", () => { + const interaction = new Interaction(); + interaction.focus("truck1/front"); + expect(interaction.focused.peek()).toBe("truck1/front"); + expect(interaction.driving.peek()).toBeUndefined(); + }); + + test("only drive intent for the focused truck takes control", () => { + const interaction = new Interaction(); + expect(interaction.takeDrive("truck1/front")).toBe(false); + expect(interaction.driving.peek()).toBeUndefined(); + + interaction.focus("truck1/front"); + expect(interaction.takeDrive("truck1/front")).toBe(true); + expect(interaction.driving.peek()).toBe("truck1/front"); + }); + + test("the control button toggles the focused truck", () => { + const interaction = new Interaction("truck1/front"); + interaction.toggleDrive("truck1/front"); + expect(interaction.driving.peek()).toBe("truck1/front"); + interaction.toggleDrive("truck1/front"); + expect(interaction.driving.peek()).toBeUndefined(); + }); + + test("release keeps focus so a second escape can leave the view", () => { + const interaction = new Interaction("truck1/front"); + interaction.takeDrive("truck1/front"); + expect(interaction.releaseDrive()).toBe(true); + expect(interaction.driving.peek()).toBeUndefined(); + expect(interaction.focused.peek()).toBe("truck1/front"); + expect(interaction.releaseDrive()).toBe(false); + }); + + test("releases control when focus is left or changed", () => { + const interaction = new Interaction("truck1/front"); + interaction.toggleDrive("truck1/front"); + interaction.focus("truck2/front"); + expect(interaction.driving.peek()).toBeUndefined(); + + interaction.toggleDrive("truck2/front"); + interaction.focus(undefined); + expect(interaction.driving.peek()).toBeUndefined(); + }); + + test("clears control when an unfocused truck disappears", () => { + const interaction = new Interaction(); + // Exercise cleanup independently of focus, as if navigation already moved. + interaction.driving.set("truck1/front"); + expect(interaction.remove("truck1/front")).toBe(false); + expect(interaction.driving.peek()).toBeUndefined(); + }); + + test("targets a focused viewer's truck for modal e-stop", () => { + const interaction = new Interaction("truck1/front"); + expect(interaction.driving.peek()).toBeUndefined(); + expect(interaction.safetyTarget()).toBe("truck1/front"); + }); +}); + +describe("focus URL state", () => { + test("round trips a broadcast id", () => { + const hash = hashWithFocus("", "truck1/front"); + expect(hash).toBe("#truck=truck1%2Ffront"); + expect(focusFromHash(hash)).toBe("truck1/front"); + }); + + test("preserves unrelated fragment parameters", () => { + expect(hashWithFocus("#other=1", "truck2/front")).toBe("#other=1&truck=truck2%2Ffront"); + expect(hashWithFocus("#other=1&truck=truck2%2Ffront", undefined)).toBe("#other=1"); + }); + + test("treats an empty focus as preview", () => { + expect(focusFromHash("#truck=")).toBeUndefined(); + expect(hashWithFocus("#truck=truck1%2Ffront", undefined)).toBe(""); + }); +}); + +test("hover cannot claim control and duplicate URL focus does not erase a keyboard preview", () => { + const state = new Interaction(); + state.hovered.set("truck1/front"); + state.focus(undefined); + expect(state.hovered.peek()).toBe("truck1/front"); + expect(state.driving.peek()).toBeUndefined(); + state.focus("truck2/front"); + expect(state.hovered.peek()).toBeUndefined(); + expect(state.driving.peek()).toBeUndefined(); + state.hovered.set("truck1/front"); + state.remove("truck1/front"); + expect(state.hovered.peek()).toBeUndefined(); +}); + +test("a retracted truck loses preview, focus, and control before re-announcement", () => { + const state = new Interaction("truck1/front"); + state.takeDrive("truck1/front"); + state.hovered.set("truck1/front"); + expect(state.remove("truck1/front")).toBe(true); + expect(state.focused.peek()).toBeUndefined(); + expect(state.driving.peek()).toBeUndefined(); + expect(state.hovered.peek()).toBeUndefined(); + state.focus("truck1/front"); + expect(state.driving.peek()).toBeUndefined(); +}); diff --git a/demo/carla/web/src/interaction.ts b/demo/carla/web/src/interaction.ts new file mode 100644 index 00000000000..d39bdfffd56 --- /dev/null +++ b/demo/carla/web/src/interaction.ts @@ -0,0 +1,77 @@ +import * as Moq from "@moq/net"; + +const FOCUS_PARAM = "truck"; + +/** Preview, focus, and drive state shared by fleet navigation and truck sessions. */ +export class Interaction { + readonly focused: Moq.Signals.Signal; + readonly hovered = new Moq.Signals.Signal(undefined); + readonly driving = new Moq.Signals.Signal(undefined); + + constructor(focused?: string) { + this.focused = new Moq.Signals.Signal(focused); + } + + /** Focus a truck for viewing. Leaving or changing focus releases control. */ + focus(id: string | undefined) { + const active = this.driving.peek(); + if (active && active !== id) this.driving.set(undefined); + if (this.focused.peek() !== id) this.hovered.set(undefined); + this.focused.set(id); + } + + /** Take control of the focused truck, including from a drive-intent key. */ + takeDrive(id: string): boolean { + if (this.focused.peek() !== id) return false; + this.driving.set(id); + return true; + } + + /** Release control without leaving the focused view. */ + releaseDrive(): boolean { + if (!this.driving.peek()) return false; + this.driving.set(undefined); + return true; + } + + /** Take or release control of the focused truck from the explicit button. */ + toggleDrive(id: string) { + if (this.driving.peek() === id) { + this.releaseDrive(); + } else { + this.takeDrive(id); + } + } + + /** Drop every local state tied to a truck that disappeared. */ + remove(id: string): boolean { + if (this.hovered.peek() === id) this.hovered.set(undefined); + if (this.driving.peek() === id) this.driving.set(undefined); + if (this.focused.peek() !== id) return false; + this.focused.set(undefined); + return true; + } + + /** Truck whose e-stop must remain reachable through a covering modal. */ + safetyTarget(): string | undefined { + return this.focused.peek() ?? this.driving.peek(); + } +} + +/** Read the focused truck from the URL fragment. */ +export function focusFromHash(hash: string): string | undefined { + const value = new URLSearchParams(hash.replace(/^#/, "")).get(FOCUS_PARAM); + return value || undefined; +} + +/** Return a fragment with the focused truck changed and other state preserved. */ +export function hashWithFocus(hash: string, truck: string | undefined): string { + const params = new URLSearchParams(hash.replace(/^#/, "")); + if (truck) { + params.set(FOCUS_PARAM, truck); + } else { + params.delete(FOCUS_PARAM); + } + const value = params.toString(); + return value ? `#${value}` : ""; +} diff --git a/demo/carla/web/src/main.ts b/demo/carla/web/src/main.ts new file mode 100644 index 00000000000..3aced677727 --- /dev/null +++ b/demo/carla/web/src/main.ts @@ -0,0 +1,1198 @@ +/** + * A steering player for vehicles simulated in CARLA. + * + * Connects to the relay, discovers camera broadcasts by announcement (like + * moq-boy), renders each on a map with a video preview, and lets you take + * control of one vehicle with the keyboard or a steering wheel. The + * {@link Truck} backend publishes a `control` track and subscribes to the + * vehicle's `status` track for round-trip confirmation. + * + * @module + */ + +import { Milli, Nano } from "@moq/clock"; +import * as Moq from "@moq/net"; +import { COLLISION_ALERT_MS, collisionAge, collisionLabel, diverged, divergence } from "./feedback.ts"; +import { drawDivergence, drawDriveHud, drawGuide } from "./guides.ts"; +import { controlHint } from "./hint.ts"; +import { applyKeys, canShift, REST, stepDrive } from "./input.ts"; +import { focusFromHash, hashWithFocus, Interaction } from "./interaction.ts"; +import { + expandToAspect, + fitPoints, + fleetTown, + mapView, + needsRefit, + pathData, + project, + type RoadMap, + scaleBar, + type View, + viewBox, +} from "./map.ts"; +import { loadRoadMap } from "./maps/index.ts"; +import { CAMERAS, parseCamera } from "./paths.ts"; +import { presence } from "./presence.ts"; +import type { Collision, Gear, Pose, TruckStatus } from "./schemas.ts"; +import { nextGear, shiftGear, Truck } from "./truck.ts"; +import { + applyWheel, + arbitrate, + Calibration, + failSafe, + type InputSource, + shortName, + Wheel, + type WheelSample, +} from "./wheel.ts"; + +const RELAY_URL = import.meta.env.VITE_RELAY_URL ?? "http://localhost:4443"; + +const preview = import.meta.env.DEV && new URLSearchParams(location.search).has("preview"); +const connection = new Moq.Connection({ enabled: !preview, url: new URL(RELAY_URL) }); +const interaction = new Interaction(focusFromHash(window.location.hash)); +const { driving, focused, hovered } = interaction; +const signals = new Moq.Signals.Effect(); + +/** Required template nodes fail at startup instead of silently losing a control. */ +function required(selector: string, parent: ParentNode = document): T { + const element = parent.querySelector(selector); + if (!element) throw new Error(`Missing UI element: ${selector}`); + return element; +} + +const els = { + conn: document.getElementById("conn") as HTMLSpanElement, + map: document.getElementById("map") as HTMLDivElement, + roads: document.querySelector("#roads") as SVGSVGElement, + markers: document.getElementById("markers") as HTMLDivElement, + scale: document.getElementById("scale") as HTMLDivElement, + note: document.getElementById("map-note") as HTMLDivElement, + offline: document.getElementById("offline") as HTMLDivElement, + backdrop: document.getElementById("backdrop") as HTMLDivElement, + wheel: document.getElementById("wheel") as HTMLButtonElement, + cal: document.getElementById("wheel-cal") as HTMLDivElement, + inspector: document.getElementById("inspector") as HTMLDivElement, +}; + +els.backdrop.addEventListener("click", () => navigateFocus(undefined)); + +// --- Connection status indicator. --- +signals.run((effect) => { + const state = effect.get(connection.status); + els.conn.textContent = preview + ? "Local preview" + : state === "connected" + ? "Connected" + : state === "connecting" + ? "Connecting…" + : "Disconnected"; + els.conn.dataset.state = preview ? "preview" : state; +}); + +// --- Discovery: maintain a Truck per announced camera broadcast. --- +const tiles = new Map(); +const vehicles = new Moq.Signals.Signal(0); + +signals.run((effect) => { + // Connection retracts old announcements on disconnect, so stale trucks are removed + // and a returning truck cannot inherit an abandoned control session. + const announced = connection.announced(CAMERAS); + effect.cleanup(() => announced.close()); + effect.spawn(async () => { + for (;;) { + const entry = await effect.race(announced.next()); + if (!entry) break; + + // A broad route can cover the scope without naming one camera. + if (!parseCamera(entry.prefix)) continue; + + // A restart is a new simulator instance on the same path; the watch + // broadcast follows it, so only an end removes the tile. + if (entry.kind !== "end" && !tiles.has(entry.prefix)) { + const truck = new Truck({ broadcastId: entry.prefix, connection, driving, focused, hovered }); + tiles.set(entry.prefix, new TruckTile(truck)); + vehicles.set(tiles.size); + refreshFocusLayout(); + } else if (entry.kind === "end") { + const tile = tiles.get(entry.prefix); + tile?.dispose(); + tiles.delete(entry.prefix); + vehicles.set(tiles.size); + if (tile && interaction.remove(tile.truck.id)) navigateFocus(undefined); + refreshFocusLayout(); + } + } + }); +}); + +// --- Offline state: say why the map is empty instead of showing nothing. --- +signals.run((effect) => { + const state = presence(effect.get(connection.status), effect.get(vehicles)); + els.offline.hidden = state === "online"; + const [title, text] = els.offline.children; + if (state === "offline") { + title.textContent = "Simulator offline"; + text.replaceChildren( + "The simulated vehicles run in CARLA on a workstation and appear here as soon as it is publishing. ", + "Check back later, or run your own with the ", + link("CARLA demo", "https://github.com/moq-dev/moq/tree/main/demo/carla"), + ".", + ); + } else { + title.textContent = "Connecting to the relay…"; + text.replaceChildren(); + } +}); + +function link(text: string, href: string): HTMLAnchorElement { + const a = document.createElement("a"); + a.href = href; + a.textContent = text; + return a; +} + +function hasTruck(id: string | undefined): boolean { + for (const tile of tiles.values()) if (tile.truck.id === id) return true; + return false; +} + +// --- The map. --- +// +// Roads and truck markers share one coordinate system: CARLA world metres, as +// published on `status.pose` and baked by the simulator's roadmap module. `fitted` is +// the rectangle we want to show; `view` is that rectangle widened to the map +// box's own aspect ratio, which is what lets an SVG viewBox in metres and +// markers positioned in percent agree on where a truck is. +const poses = new Map(); +const towns = new Map(); +let shownTown: string | undefined; +let roadMap: RoadMap | undefined; +let wantedMap: string | undefined; +let unreadableMap: string | undefined; +let fitted: View | undefined; +let fittedFleet = ""; +let view: View | undefined; + +/** + * Adopt the fleet's town, drawing its roads if they were baked. + * + * `undefined` is a town in its own right - the fleet reporting none, because the + * last one left or its status lapsed. It has to drop the roads too: a pose-only + * truck would otherwise be projected onto the departed town's coordinates, and + * an empty map would keep drawing streets nobody is on. + */ +async function useRoadMap(name: string | undefined) { + if (wantedMap === name) return; + wantedMap = name; + // Drop the old town before the load rather than after it: an await that + // fails, or loses the race to another town, would otherwise leave the + // previous map drawn under trucks that are somewhere else entirely. + useRoads(undefined); + if (name === undefined) return; + + let loaded: RoadMap | undefined; + try { + loaded = await loadRoadMap(name); + } catch (err) { + // Deliberately no retry. Status arrives at 10 Hz, so releasing the name + // here would turn telemetry into a tight retry timer, and would re-clear + // `fitted` on every tick. A reload is the supervisor that can actually + // re-establish this, so say the chunk failed rather than reporting a town + // that IS baked as if it were not. + console.warn(`failed to load the road map for ${name}:`, err); + if (wantedMap === name) unreadableMap = name; + // The refit above already painted the note, back when this town still + // looked merely unbaked. Repaint it: status snapshots dedupe, so a parked + // fleet may never publish again to trigger one. + scheduleRefit(); + return; + } + // Another town won the race while this one loaded: it owns the map now. + if (wantedMap !== name) return; + useRoads(loaded); +} + +/** + * Adopt a town's roads, dropping the view the previous town fixed. + * + * `fitted` has to go with them. It came from the old town's bounds, or from + * trucks driving around inside them, and neither describes where these trucks + * are: without this, a fleet that lands inside the old extent leaves + * `needsRefit` false and gets projected against the wrong town. + */ +function useRoads(map: RoadMap | undefined) { + roadMap = map; + unreadableMap = undefined; + fitted = undefined; + els.roads.querySelector("#lane-geometry")?.setAttribute("d", map ? pathData(map) : ""); + const image = required("#aerial-image"); + image.removeAttribute("href"); + els.roads.dataset.aerial = String(Boolean(map?.aerial)); + if (map?.aerial) { + const { bbox, image: url } = map.aerial; + image.setAttribute("x", String(bbox[0])); + image.setAttribute("y", String(bbox[1])); + image.setAttribute("width", String(bbox[2] - bbox[0])); + image.setAttribute("height", String(bbox[3] - bbox[1])); + image.setAttribute("href", url); + } + scheduleRefit(); +} + +let refitting = false; + +/** + * Coalesce refits into the next frame. + * + * Every truck republishes its pose ten times a second, and a refit measures the + * map before positioning markers. Doing that per pose would interleave + * layout reads and writes all frame; once a frame reads everything, then moves + * everything. + */ +function scheduleRefit() { + if (refitting) return; + refitting = true; + requestAnimationFrame(() => { + refitting = false; + refitMap(); + }); +} + +/** + * Recompute the view and put every truck on it. + * + * A baked town fixes the view, so the map holds still while the trucks drive + * across it. Without one the view fits the trucks themselves, on the two + * triggers below. + */ +function refitMap() { + // One map, one town. A relay can carry trucks from more than one CARLA world, + // and their coordinates are unrelated, so projecting them together would + // place someone confidently in the wrong place. Show the majority's town and + // hide everyone else, like a truck with no position yet. + shownTown = fleetTown(towns.values()); + void useRoadMap(shownTown); + + const points = [...poses.entries()].filter(([id]) => onShownMap(id)).map(([, pose]) => pose); + // Two triggers, deliberately different. A truck joining or leaving is a + // discrete event that always refits: one that left should not keep the view + // zoomed out around where it was, and the last one leaving has to clear the + // view rather than leave an empty extent behind. Trucks MOVING refit only + // once one nears an edge, or the map slides under the driver at 10 Hz. + const fleet = [...poses.keys()].filter(onShownMap).sort().join(" "); + if (roadMap) { + fitted = mapView(roadMap, 0.06); + } else if (fleet !== fittedFleet || needsRefit(fitted, points)) { + fitted = fitPoints(points); + } + fittedFleet = fleet; + + const box = els.map.getBoundingClientRect(); + view = fitted && box.height > 0 ? expandToAspect(fitted, box.width / box.height) : undefined; + + if (view) els.roads.setAttribute("viewBox", viewBox(view)); + els.roads.style.visibility = view && roadMap ? "visible" : "hidden"; + refreshScale(); + refreshNote(); + + for (const tile of tiles.values()) tile.place(); +} + +/** Whether this truck's coordinates belong to the town the map is showing. */ +function onShownMap(id: string): boolean { + const town = towns.get(id); + return town === undefined || town === shownTown; +} + +function refreshScale() { + els.scale.hidden = !view; + if (!view) return; + const { metres, fraction } = scaleBar(view); + els.scale.style.width = `${fraction * 100}%`; + (els.scale.querySelector(".label") as HTMLElement).textContent = `${metres} m`; +} + +let note = ""; + +function code(text: string): HTMLElement { + const el = document.createElement("code"); + el.textContent = text; + return el; +} + +/** Say why the map is bare, rather than leaving an empty box. */ +function refreshNote() { + // A refit runs every frame a truck moves, so rebuild the note's nodes only + // when what it has to say changes. + const key = !view + ? "position" + : roadMap + ? "" + : unreadableMap + ? `unreadable:${unreadableMap}` + : wantedMap + ? `unbaked:${wantedMap}` + : ""; + if (key !== note) { + note = key; + if (key === "position") { + els.note.textContent = "Waiting for a truck to report where it is…"; + } else if (key.startsWith("unreadable:")) { + els.note.replaceChildren( + "The road map for ", + code(unreadableMap ?? ""), + " did not load - reload the page to try again.", + ); + } else if (key.startsWith("unbaked:")) { + els.note.replaceChildren( + "Road imagery unavailable for ", + code(wantedMap ?? ""), + ". Showing truck positions only.", + ); + } + } + // Before any truck is online, the offline state already says why. + els.note.hidden = key === "" || (key === "position" && tiles.size === 0); +} + +// The map's aspect ratio decides how wide the view has to be, so a resize is a +// refit rather than just a repaint. +new ResizeObserver(scheduleRefit).observe(els.map); + +// --- Navigation: preview -> focus -> drive. --- + +let cameraChooser: HTMLDialogElement | undefined; + +function closeCameraChooser() { + cameraChooser?.remove(); + cameraChooser = undefined; +} + +/** A shared map position must still expose every camera to mouse and touch. */ +function selectMarker(tile: TruckTile) { + closeCameraChooser(); + const pin = required(".pin", tile.root); + const bounds = pin.getBoundingClientRect(); + const neighbours = [...tiles.values()].filter((candidate) => { + if (candidate.root.hidden) return false; + const other = required(".pin", candidate.root).getBoundingClientRect(); + return ( + bounds.left < other.right && + bounds.right > other.left && + bounds.top < other.bottom && + bounds.bottom > other.top + ); + }); + if (neighbours.length < 2) { + navigateFocus(tile.truck.id); + return; + } + const dialog = document.createElement("dialog"); + dialog.className = "camera-chooser"; + dialog.setAttribute("aria-label", "Select camera"); + for (const neighbour of neighbours) { + const button = document.createElement("button"); + button.type = "button"; + button.textContent = neighbour.truck.id.replace("/", " · "); + button.addEventListener("click", () => navigateFocus(neighbour.truck.id)); + dialog.appendChild(button); + } + const cancel = document.createElement("button"); + cancel.type = "button"; + cancel.textContent = "Cancel"; + const dismiss = () => { + closeCameraChooser(); + if (pin.isConnected) pin.focus(); + }; + cancel.addEventListener("click", dismiss); + dialog.addEventListener("cancel", (event) => { + event.preventDefault(); + dismiss(); + }); + dialog.appendChild(cancel); + document.body.appendChild(dialog); + cameraChooser = dialog; + dialog.showModal(); +} + +function navigateFocus(id: string | undefined) { + const hash = hashWithFocus(window.location.hash, id); + if (hash !== window.location.hash) window.location.hash = hash; + applyFocus(id); +} + +function applyFocus(id: string | undefined) { + closeCameraChooser(); + if (focused.peek() !== id) clearKeys(); + interaction.focus(id); + refreshFocusLayout(); +} + +let previousFocus: string | undefined; +function refreshFocusLayout() { + const id = focused.peek(); + const open = hasTruck(id); + els.backdrop.hidden = !open; + for (const el of document.querySelectorAll("header, main, footer")) el.inert = open; + if (id !== previousFocus) { + const last = previousFocus; + previousFocus = id; + requestAnimationFrame(() => { + if (open) els.inspector.querySelector(".close-btn")?.focus(); + else if (last) [...tiles.values()].find((tile) => tile.truck.id === last)?.focusMarker(); + }); + } + // Keep markers aligned when the layout changes. + scheduleRefit(); +} + +window.addEventListener("hashchange", () => applyFocus(focusFromHash(window.location.hash))); + +// --- Keyboard steering for the active truck. --- +const held = new Moq.Signals.Signal>(new Set()); +const shiftBlocked = new Moq.Signals.Signal(undefined); +let shiftBlockedTimer: number | undefined; +const KEY = (e: KeyboardEvent) => e.key.toLowerCase(); +const DRIVE_KEYS = ["w", "a", "s", "d", "c", "arrowup", "arrowdown", "arrowleft", "arrowright"]; + +function holdKey(key: string) { + if (held.peek().has(key)) return; + held.set(new Set([...held.peek(), key])); +} + +function releaseKey(key: string) { + if (!held.peek().has(key)) return; + const next = new Set(held.peek()); + next.delete(key); + held.set(next); +} + +function clearKeys() { + if (held.peek().size > 0) held.set(new Set()); +} + +window.addEventListener("keydown", (e) => { + // The native chooser owns navigation and button activation while it is open. + if (cameraChooser) return; + // The calibration dialog owns the driving keys while it's up, so that + // working the pedals to bind them can't also steer. E-stop is the exception: + // an unavailable e-stop is a hazard, while a spurious one is an annoyance, + // so E keeps working here and the dialog carries its own button. + if (calibration) { + if (e.key === "Escape") closeCalibration(); + if (KEY(e) === "e") safetyTruck()?.emergencyStop(); + e.preventDefault(); + return; + } + const k = KEY(e); + if (k === "e") { + safetyTruck()?.emergencyStop(); + e.preventDefault(); + return; + } + if (e.metaKey || e.ctrlKey || e.altKey) return; + if (k === "tab" && hasTruck(focused.peek())) { + const controls = [...els.inspector.querySelectorAll("button:not(:disabled), summary")].filter( + (el) => el.getClientRects().length > 0, + ); + const first = controls[0]; + const last = controls[controls.length - 1]; + if (e.shiftKey && document.activeElement === first) { + last?.focus(); + e.preventDefault(); + } else if (!e.shiftKey && document.activeElement === last) { + first?.focus(); + e.preventDefault(); + } + return; + } + if (k === "escape") { + clearKeys(); + if (!interaction.releaseDrive() && focused.peek()) navigateFocus(undefined); + e.preventDefault(); + return; + } + if (k === "g" || k === " ") { + // Keep native Space activation for STOP and map navigation. In the + // driving view it shifts even if Take control or Close still has focus. + if ( + k === " " && + e.target instanceof HTMLElement && + (e.target.closest(".estop-btn, input, select, textarea") || + (!focused.peek() && e.target.closest("button, summary"))) + ) + return; + e.preventDefault(); + if (e.repeat) return; + const t = intentTruck(); + if (t) requestGear(t, nextGear(t.drive.peek().gear as Gear)); + return; + } + if (k === "r" && !e.repeat) { + truckById(focused.peek())?.resetEstop(); + e.preventDefault(); + return; + } + if (DRIVE_KEYS.includes(k)) { + intentTruck(); + holdKey(k); + // Reaching for the keyboard takes the axes back from the wheel, which + // otherwise holds them at wherever the rim and pedals happen to sit. + source.set("keyboard"); + e.preventDefault(); + } +}); +window.addEventListener("keyup", (e) => releaseKey(KEY(e))); +window.addEventListener("blur", clearKeys); + +function activeTruck(): Truck | undefined { + return truckById(driving.peek()); +} + +/** Claim the focused truck for an input that expresses driving intent. */ +function intentTruck(): Truck | undefined { + const id = focused.peek(); + const truck = truckById(id); + if (!id || !truck) return undefined; + if (driving.peek() !== id && !truck.beginTakeover()) return undefined; + if (!interaction.takeDrive(id)) return undefined; + return truck; +} + +function safetyTruck(): Truck | undefined { + return truckById(interaction.safetyTarget()); +} + +function truckById(id: string | undefined): Truck | undefined { + for (const tile of tiles.values()) if (tile.truck.id === id) return tile.truck; + return undefined; +} + +/** Apply the client-side half of the truck's authoritative gear interlock. */ +function requestGear(truck: Truck, gear: Gear) { + if (!canShift(truck.drive.peek())) { + shiftBlocked.set(truck.id); + if (shiftBlockedTimer !== undefined) window.clearTimeout(shiftBlockedTimer); + shiftBlockedTimer = window.setTimeout(() => shiftBlocked.set(undefined), 2000); + return; + } + if (shiftBlocked.peek() === truck.id) shiftBlocked.set(undefined); + truck.setDrive({ gear }); +} + +// --- Steering wheel (Gamepad API). --- +// +// Only one source drives the axes at a time. The keyboard holds them by +// default; a connected wheel takes over once it actually moves, and only with +// the throttle released, so a pedal resting under a foot can never be what +// hands it control. Any drive key takes the axes straight back. +const wheel = new Wheel(); +const source = new Moq.Signals.Signal("keyboard"); + +/** + * Advance the arbitration, failing the truck safe if an armed wheel vanished. + * + * Unplugging mid-drive must not silently become a held keyboard command: the + * driver has lost the device that was setting throttle, so cut it here rather + * than letting {@link applyKeys} hold it. Fires once on the transition, so the + * keyboard is free to drive normally afterwards. + */ +function updateSource(sample: WheelSample | undefined) { + const { source: next, lost } = arbitrate(source.peek(), sample); + source.set(next); + if (!lost) return; + // A key held from before the wheel armed is still in `held`, and applyKeys + // runs later this same frame - without this it would start easing the brake + // back off the instant the fail-safe applied it. + clearKeys(); + const truck = activeTruck(); + if (truck && !truck.estopped.peek()) truck.setDrive(failSafe(truck.drive.peek())); +} + +function wheelButtons(sample: WheelSample) { + const truck = activeTruck(); + // E-stop is the documented exception to arbitration: any source may latch it + // at any time, on the truck in view whether or not this browser drives it. + // Gear is a drive command, so it follows the active source - a bumped paddle + // must not shift a truck being driven from the keyboard. + if (sample.pressed.has("estop")) safetyTruck()?.emergencyStop(); + if (!truck || truck.estopped.peek() || source.peek() !== "wheel") return; + const gear = truck.drive.peek().gear as Gear; + if (sample.pressed.has("gearUp")) requestGear(truck, shiftGear(gear, 1)); + if (sample.pressed.has("gearDown")) requestGear(truck, shiftGear(gear, -1)); +} + +// Integrate the active input source into the drive state ~60 Hz. The interval +// fallback keeps input (and therefore the control heartbeat's payload) +// advancing when rAF is paused, e.g. in a hidden or embedded tab. +let lastFrame = performance.now(); + +function step() { + const now = performance.now(); + const dt = Math.min((now - lastFrame) / 1000, 0.1); + lastFrame = now; + + const sample = wheel.poll(); + refreshWheel(); + + // Calibration reads the raw device, so nothing it sees should reach a truck. + if (calibration) { + const pad = wheel.pad(); + if (pad) calibration.update(pad); + refreshCalibration(); + return; + } + + // Arbitrate before reading the buttons: the gear gate below consults the + // active source, which must already reflect this frame's sample. + updateSource(sample); + // Moving an armed wheel is drive intent just like pressing a drive key. The + // arbitration throttle gate has already run, so a depressed accelerator can + // never be the movement that claims the truck. + if (!driving.peek() && sample?.moved && source.peek() === "wheel") intentTruck(); + if (sample) wheelButtons(sample); + + const truck = activeTruck(); + if (truck) { + const cur = truck.drive.peek(); + const next = + source.peek() === "wheel" && sample ? applyWheel(cur, sample, dt) : applyKeys(cur, held.peek(), dt); + stepDrive(truck, next); + } +} + +function tick() { + step(); + requestAnimationFrame(tick); +} +requestAnimationFrame(tick); +setInterval(step, 250); + +// --- Wheel status chip + calibration dialog. --- +let calibration: Calibration | undefined; +let lastWheelLabel = ""; + +const calEls = { + device: els.cal.querySelector(".cal-device") as HTMLDivElement, + progress: els.cal.querySelector(".cal-progress") as HTMLDivElement, + prompt: els.cal.querySelector(".cal-prompt") as HTMLParagraphElement, + live: els.cal.querySelector(".cal-live") as HTMLDivElement, + warn: els.cal.querySelector(".cal-warn") as HTMLDivElement, + capture: els.cal.querySelector(".cal-capture") as HTMLButtonElement, + skip: els.cal.querySelector(".cal-skip") as HTMLButtonElement, + cancel: els.cal.querySelector(".cal-cancel") as HTMLButtonElement, + estop: els.cal.querySelector(".cal-estop") as HTMLButtonElement, +}; + +els.wheel.addEventListener("click", () => openCalibration()); +// The modal covers the tile's e-stop, so it has to carry its own. +calEls.estop.addEventListener("click", () => safetyTruck()?.emergencyStop()); +calEls.cancel.addEventListener("click", () => closeCalibration()); +calEls.skip.addEventListener("click", () => { + calibration?.skip(); + finishCalibration(); +}); +calEls.capture.addEventListener("click", () => { + // A failed capture means the control didn't move far enough to tell apart + // from the rest snapshot; leave the step up and say so. + if (calibration && !calibration.capture()) { + calEls.warn.hidden = false; + return; + } + calEls.warn.hidden = true; + finishCalibration(); +}); + +/** Reflect the connected wheel in the header chip. */ +function refreshWheel() { + const id = wheel.id; + if (!id) { + els.wheel.hidden = true; + lastWheelLabel = ""; + return; + } + const label = `${shortName(id)}${wheel.calibrated ? "" : " · uncalibrated"}`; + if (label !== lastWheelLabel) { + els.wheel.textContent = label; + els.wheel.title = `${id} - click to calibrate`; + lastWheelLabel = label; + } + els.wheel.hidden = false; + els.wheel.dataset.driving = String(source.peek() === "wheel"); +} + +function openCalibration() { + if (!wheel.pad()) return; + calibration = new Calibration(); + // The wheel has to re-arm afterwards, since the axes it reports now mean + // something different than they did a moment ago. + source.set("keyboard"); + clearKeys(); + // Calibration means working the pedals for reasons that have nothing to do + // with driving, and the dialog suppresses the normal control loop, so park + // the truck before it goes up rather than leaving whatever was commanded to + // keep heartbeating out for the duration. + activeTruck()?.setDrive({ ...REST }); + calEls.warn.hidden = true; + els.cal.hidden = false; + refreshCalibration(); +} + +function closeCalibration() { + calibration = undefined; + els.cal.hidden = true; +} + +/** Save and close once the last step is bound, otherwise redraw the dialog. */ +function finishCalibration() { + if (!calibration) return; + if (!calibration.done) { + refreshCalibration(); + return; + } + const profile = calibration.result(); + if (profile) wheel.save(profile); + closeCalibration(); +} + +function refreshCalibration() { + const cal = calibration; + if (!cal) return; + if (cal.done) { + finishCalibration(); + return; + } + + const step = cal.step; + if (!step) return; + + calEls.device.textContent = wheel.id ? shortName(wheel.id) : "wheel disconnected"; + calEls.progress.textContent = `step ${cal.index + 1} of ${cal.total}`; + calEls.prompt.textContent = step.prompt; + // Button steps bind on the press itself - there is no free hand for Capture. + calEls.capture.hidden = step.kind === "button"; + calEls.skip.hidden = !step.optional; + + const pad = wheel.pad(); + calEls.live.textContent = pad + ? pad.axes.map((value, index) => `a${index} ${value.toFixed(2)}`).join(" ") + : "no signal from the wheel"; +} + +// --- A rendered tile bound to one Truck. --- +class TruckTile { + readonly truck: Truck; + readonly root: HTMLDivElement; + readonly #signals = new Moq.Signals.Effect(); + #overlay!: HTMLCanvasElement; + #arrow!: HTMLElement; + readonly #card: HTMLDivElement; + readonly #panel = document.createElement("div"); + // The last collision the truck reported, held across a status dropout. + #collision?: Collision; + + constructor(truck: Truck) { + this.truck = truck; + this.root = document.createElement("div"); + this.root.className = "truck"; + this.root.innerHTML = ` + +
+
+ +
Connecting
+ + Waiting for camera +
+
Throttle
+
Brake
+
+
km/h
+
MeasuredInput
+
+
+
+ +
+ + + +
+
+
+ +
Diagnostics
Control round trip
Camera pipeline
+
+
+
+
`; + this.root.hidden = true; + els.markers.appendChild(this.root); + this.#card = this.root.querySelector(".card") as HTMLDivElement; + this.#panel.className = "truck"; + els.inspector.appendChild(this.#panel); + + const canvas = this.root.querySelector("canvas:not(.overlay)") as HTMLCanvasElement; + truck.setCanvas(canvas); + this.#overlay = this.root.querySelector("canvas.overlay") as HTMLCanvasElement; + this.#arrow = this.root.querySelector(".pin .arrow") as HTMLElement; + + this.root.addEventListener("click", () => selectMarker(this)); + for (const target of [this.root]) { + target.addEventListener("pointerenter", (event) => { + if (event.pointerType !== "touch") hovered.set(truck.id); + }); + target.addEventListener("pointerleave", () => { + if (hovered.peek() === truck.id) hovered.set(undefined); + }); + target.addEventListener("focusin", () => hovered.set(truck.id)); + target.addEventListener("focusout", (event) => { + if ( + !(event.relatedTarget instanceof Node && target.contains(event.relatedTarget)) && + hovered.peek() === truck.id + ) + hovered.set(undefined); + }); + } + required(".close-btn", this.root).addEventListener("click", (event) => { + event.stopPropagation(); + navigateFocus(undefined); + }); + + const name = this.root.querySelector(".name"); + const control = this.root.querySelector(".control-btn") as HTMLButtonElement | null; + const respawn = this.root.querySelector(".respawn-btn") as HTMLButtonElement | null; + const estop = this.root.querySelector(".estop-btn"); + if (!name || !control || !respawn || !estop) + throw new Error("truck tile template is missing required controls"); + name.textContent = truck.id.replace("/", " · "); + required(".pin", this.root).setAttribute("aria-label", `Open ${truck.id} on map`); + required(".pin-label", this.root).textContent = truck.id.split("/")[0]; + + control.addEventListener("click", (e) => { + e.stopPropagation(); + if (this.truck.active.peek()) { + interaction.releaseDrive(); + } else if (this.truck.beginTakeover()) { + interaction.takeDrive(truck.id); + } + }); + + respawn.addEventListener("click", (e) => { + e.stopPropagation(); + this.truck.respawn(); + }); + + // E-stop is a safety control anyone may trigger, so it lives on every tile + // and works whether or not you hold the lease. stopPropagation keeps the + // click from also navigating into this tile's focused view. + estop.addEventListener("click", (e) => { + e.stopPropagation(); + this.truck.toggleEmergencyStop(); + }); + + this.#bind(); + } + + /** + * Move this marker to the reported position, hiding it until there is one. + * + * Markers and their hover previews follow the reported position. A focused + * truck's card lives in the inspector, so focus works either way. + */ + place() { + const pose = onShownMap(this.truck.id) ? poses.get(this.truck.id) : undefined; + this.root.hidden = !pose || !view; + if (!pose || !view) return; + + const { left, top } = project(view, pose); + this.root.style.left = `${left}%`; + this.root.style.top = `${top}%`; + // CARLA yaw is clockwise from +x, and so is a CSS rotation drawn on a map + // whose y grows downward, so this needs no conversion. + this.#arrow.style.transform = `rotate(${pose.yaw}deg)`; + } + + #bind() { + const q = (s: string) => (s === ".card" ? this.#card : (this.#card.querySelector(s) as HTMLElement)); + + // The map's inputs: where this truck is, and which town it is in. + this.#signals.run((effect) => { + const status = effect.get(this.truck.status); + if (status?.map) { + towns.set(this.truck.id, status.map); + } else { + towns.delete(this.truck.id); + } + if (status?.pose) { + poses.set(this.truck.id, status.pose); + } else { + poses.delete(this.truck.id); + } + q(".speed-value").textContent = status?.pose ? String(Math.round(status.pose.speed * 3.6)) : "–"; + scheduleRefit(); + }); + + // Preview, focus, and drive each have a distinct visual and action. + this.#signals.run((effect) => { + const isFocused = effect.get(focused) === this.truck.id; + const active = effect.get(this.truck.active); + this.root.dataset.focused = String(isFocused); + this.root.dataset.active = String(active); + this.#panel.dataset.focused = String(isFocused); + this.#panel.dataset.active = String(active); + const cardParent = isFocused ? this.#panel : this.root; + if (this.#card.parentElement !== cardParent) cardParent.appendChild(this.#card); + const isHovered = effect.get(hovered) === this.truck.id && !effect.get(focused); + this.root.dataset.preview = String(isHovered); + q(".card").hidden = !isFocused && !isHovered; + q(".card").setAttribute("role", isFocused ? "dialog" : "region"); + q(".card").setAttribute("aria-label", this.truck.id); + q(".card").setAttribute("aria-modal", String(isFocused)); + q(".guide-key").hidden = !active; + const status = effect.get(this.truck.status); + const controlButton = q(".control-btn") as HTMLButtonElement; + controlButton.textContent = active ? "Release control" : "Take control"; + // Status carries the measured takeover state. Never guess while it is unavailable. + controlButton.disabled = !active && !status; + const respawnButton = q(".respawn-btn") as HTMLButtonElement; + const driverId = effect.get(this.truck.driverId); + const estopped = effect.get(this.truck.estopped) || Boolean(status?.estopped); + respawnButton.hidden = !isFocused; + respawnButton.disabled = + effect.get(this.truck.respawning) || !active || !driverId || status?.driver !== driverId || estopped; + // Latched if either side thinks so: the local signal reacts + // instantly, the status echo is authoritative (see resetEstop). + const stopButton = q(".estop-btn") as HTMLButtonElement; + const stopParent = q(isFocused ? ".actions" : ".video-wrap"); + if (stopButton.parentElement !== stopParent) stopParent.appendChild(stopButton); + stopButton.textContent = estopped ? "RESET E-STOP" : isFocused ? "E-STOP" : "STOP"; + stopButton.dataset.reset = String(estopped); + // Reset waits for the authoritative echo so a fast second click cannot + // replace the stop command before the truck receives and latches it. + stopButton.disabled = estopped && !status?.estopped; + stopButton.title = estopped + ? "Clear the confirmed e-stop latch and return an unclaimed truck to autonomy" + : "Emergency stop · E"; + const statusState = effect.get(this.truck.statusState); + q(".hint").hidden = + !active && + !estopped && + statusState === "available" && + !status?.driver && + effect.get(shiftBlocked) !== this.truck.id; + q(".hint").textContent = + effect.get(shiftBlocked) === this.truck.id + ? "shift blocked - release throttle and apply the brake" + : controlHint({ + active, + focused: isFocused, + estopped, + estopBy: status?.estop_by, + status: statusState, + driverId, + statusDriver: status?.driver, + wheel: effect.get(source) === "wheel", + }); + }); + + // Camera readiness is separate from telemetry and control ownership. + this.#signals.run((effect) => { + const visible = + effect.get(focused) === this.truck.id || + (!effect.get(focused) && effect.get(hovered) === this.truck.id); + const frame = effect.get(this.truck.videoDecoder.out.frame); + const stalled = effect.get(this.truck.videoDecoder.out.stalled); + const live = visible && Boolean(frame) && !stalled; + q(".badge").textContent = live ? "Live" : frame ? "Interrupted" : "Connecting"; + q(".video-state").textContent = frame ? "Camera interrupted" : "Waiting for camera"; + q(".badge").dataset.live = String(live); + q(".video-state").hidden = live; + }); + + this.#signals.run((effect) => { + const s = effect.get(this.truck.status); + const measured = s?.measured; + const input = effect.get(this.truck.drive); + const active = effect.get(this.truck.active); + for (const pedal of ["throttle", "brake"] as const) { + q(`.${pedal}`).textContent = measured ? `${Math.round(measured[pedal] * 100)}%` : "–"; + const meter = q(`.${pedal}-meter`) as HTMLMeterElement; + meter.hidden = !measured; + meter.value = measured?.[pedal] ?? 0; + q(`.${pedal}-request`).hidden = !active || !measured; + q(`.${pedal}-request`).style.left = `${Math.max(0, Math.min(1, input[pedal])) * 100}%`; + } + q(".gear").textContent = measured + ? measured.gear === "forward" + ? "D" + : measured.gear === "reverse" + ? "R" + : "N" + : "–"; + q(".gear").setAttribute("aria-label", measured ? `Gear: ${measured.gear}` : "Gear unavailable"); + q(".mode").textContent = !s + ? "No status" + : s.estopped + ? "Stopped" + : s.autonomy === "autonomous" + ? "Autonomous" + : s.autonomy === "manual" + ? "Manual" + : "Unknown"; + q(".mode").className = `mode${s?.estopped ? " estopped" : ""}`; + const me = effect.get(this.truck.driverId); + const mine = me ? s?.media_pipeline[me] : undefined; + const roundTrip = effect.get(this.truck.roundTripMs); + q(".rtt").textContent = roundTrip == null ? "Not measured" : `${Math.round(roundTrip)} ms`; + q(".lat").textContent = mine?.length + ? mine.map((l) => `${l.label} ${l.ms} ms`).join(" · ") + : "Not measured"; + this.#renderCollision(s); + }); + + // Overlay: your requested input (cyan, dashed) against the truck's + // measured steering (white, solid), with the gap between them shaded. + this.#signals.run((effect) => { + effect.get(this.truck.drive); + effect.get(this.truck.active); + effect.get(this.truck.status); + effect.get(focused); + this.#drawOverlay(); + }); + + // A collision's age is wall clock, not a status field, so it needs its own + // tick: without one the callout would freeze on a truck that stops moving + // and therefore stops changing its snapshot. + this.#signals.interval(() => this.#renderCollision(this.truck.status.peek()), Nano.fromMilli(Milli(1000))); + + // Repaint on layout/resize (the initial effect run happens before layout, + // so the overlay has zero size until the first observation fires). + const ro = new ResizeObserver(() => this.#drawOverlay()); + ro.observe(this.#overlay); + this.#signals.cleanup(() => ro.disconnect()); + } + + /** + * Show the truck's last collision, and call out a recent one. + * + * The age comes off the collision's own timestamp, so this holds no state + * beyond the last value the truck sent: every viewer ages the same impact + * the same way, whenever it joined, and a reload does not resurrect it as + * news the way locally-timed decay did. + * + * Takes the whole status because a missing collision means two different + * things. No status at all is a dropout - the signal goes undefined on every + * reconnect - and the last collision is held through it. A status that is + * present and omits the field is a truck that has hit nothing, including a + * restarted producer, and clears it. + */ + #renderCollision(status: TruckStatus | undefined) { + if (status) this.#collision = status.collision; + const q = (s: string) => (s === ".card" ? this.#card : (this.#card.querySelector(s) as HTMLElement)); + if (!this.#collision) { + q(".collision-row").hidden = true; + delete this.root.dataset.collision; + delete this.#panel.dataset.collision; + return; + } + const age = collisionAge(this.#collision, Date.now()); + q(".collision-row").hidden = false; + q(".collision").textContent = collisionLabel(this.#collision, age); + this.root.dataset.collision = age < COLLISION_ALERT_MS ? "recent" : "past"; + this.#panel.dataset.collision = this.root.dataset.collision; + } + + /** + * Repaint the trajectory overlay from current signal values. + * + * Reads with `peek` rather than taking arguments because the resize observer + * and the effect both call it, and threading four values through both is how + * the two callers drift apart. + */ + #drawOverlay() { + const canvas = this.#overlay; + const cssW = canvas.clientWidth; + const cssH = canvas.clientHeight; + if (cssW === 0 || cssH === 0) return; + + const dpr = window.devicePixelRatio || 1; + canvas.width = Math.round(cssW * dpr); + canvas.height = Math.round(cssH * dpr); + const ctx = canvas.getContext("2d"); + if (!ctx) return; + ctx.setTransform(dpr, 0, 0, dpr, 0, 0); + ctx.clearRect(0, 0, cssW, cssH); + + const status = this.truck.status.peek(); + const requested = this.truck.drive.peek(); + const driving = this.truck.active.peek() && status?.autonomy !== "autonomous"; + // A marker on the map is a preview at 240p; rails on it would be noise. + // Measured steering is worth seeing on a truck you are only watching, so + // it is drawn whenever the panel is open, autonomous or not. + const measured = focused.peek() === this.truck.id ? status?.measured : undefined; + + if (measured) { + // A visible gap turns red when feedback departs from the input. + if (driving) { + const gap = divergence(requested, measured); + drawDivergence(ctx, cssW, cssH, requested, measured, { + color: diverged(gap) ? "#ff4d55" : "#ffffff", + alpha: 0.16, + }); + } + drawGuide(ctx, cssW, cssH, measured, { color: "#ffffff", width: 2, alpha: 0.8 }); + } + + // Your requested input, shown while you hold control so you can see lead/lag. + if (driving) { + drawGuide(ctx, cssW, cssH, requested, { + color: "#50c8ff", + width: 2, + dash: [7, 6], + alpha: 0.9, + }); + } + // The solid pedal arrow represents measured truck feedback. + if (measured || driving) drawDriveHud(ctx, cssW, cssH, driving ? requested : undefined, measured); + } + + focusMarker() { + required(".pin", this.root).focus(); + } + + dispose() { + closeCameraChooser(); + this.#signals.close(); + this.truck.close(); + this.root.remove(); + this.#panel.remove(); + if (hovered.peek() === this.truck.id) hovered.set(undefined); + poses.delete(this.truck.id); + towns.delete(this.truck.id); + scheduleRefit(); + } +} + +// A development-only, disconnected fixture for reviewing layouts without a GPU. +if (preview) { + required("#preview-notice").hidden = false; + const { populatePreview } = await import("./preview.ts"); + const cleanup = populatePreview((path) => { + const truck = new Truck({ broadcastId: path, connection, driving, focused, hovered }); + const tile = new TruckTile(truck); + tiles.set(path, tile); + vehicles.set(tiles.size); + refreshFocusLayout(); + return truck; + }); + signals.cleanup(cleanup); +} + +window.addEventListener("pagehide", (event) => { + if (event.persisted) return; + for (const tile of tiles.values()) tile.dispose(); + signals.close(); + connection.close(); +}); diff --git a/demo/carla/web/src/map.test.ts b/demo/carla/web/src/map.test.ts new file mode 100644 index 00000000000..55a22cef96c --- /dev/null +++ b/demo/carla/web/src/map.test.ts @@ -0,0 +1,181 @@ +import { describe, expect, test } from "bun:test"; +import { + expandToAspect, + fitPoints, + fleetTown, + mapView, + needsRefit, + pathData, + project, + type RoadMap, + scaleBar, + speedLabel, + type View, + viewBox, +} from "./map.ts"; + +const square: View = { minX: -100, minY: -100, maxX: 100, maxY: 100 }; + +describe("expandToAspect", () => { + test("widens a square view for a landscape box", () => { + expect(expandToAspect(square, 2)).toEqual({ minX: -200, minY: -100, maxX: 200, maxY: 100 }); + }); + + test("heightens a square view for a portrait box", () => { + expect(expandToAspect(square, 0.5)).toEqual({ minX: -100, minY: -200, maxX: 100, maxY: 200 }); + }); + + test("only ever grows, so nothing already in view is cropped out", () => { + const wide: View = { minX: 0, minY: 0, maxX: 400, maxY: 100 }; + expect(expandToAspect(wide, 2)).toEqual({ minX: 0, minY: -50, maxX: 400, maxY: 150 }); + }); + + test("leaves the view alone before the map box has been laid out", () => { + expect(expandToAspect(square, Number.NaN)).toEqual(square); + expect(expandToAspect(square, 0)).toEqual(square); + }); +}); + +describe("project", () => { + test("puts the view's corners on the box's corners", () => { + expect(project(square, { x: -100, y: -100 })).toEqual({ left: 0, top: 0 }); + expect(project(square, { x: 100, y: 100 })).toEqual({ left: 100, top: 100 }); + }); + + test("maps CARLA's southward y onto a downward screen without a flip", () => { + expect(project(square, { x: 0, y: 50 })).toEqual({ left: 50, top: 75 }); + }); +}); + +describe("fitPoints", () => { + test("has nothing to fit before a truck reports a position", () => { + expect(fitPoints([])).toBeUndefined(); + }); + + test("floors the span so a nose-to-tail convoy is not a wall of truck", () => { + const view = fitPoints([ + { x: 0, y: 0 }, + { x: 12, y: 0 }, + ]); + expect(view).toBeDefined(); + expect((view as View).maxX - (view as View).minX).toBeCloseTo(300); + expect((view as View).maxY - (view as View).minY).toBeCloseTo(300); + }); + + test("centres and pads a fleet spread wider than the floor", () => { + const view = fitPoints([ + { x: -500, y: 0 }, + { x: 500, y: 0 }, + ]) as View; + expect(view.minX + view.maxX).toBeCloseTo(0); + expect(view.maxX - view.minX).toBeCloseTo(1200); + }); +}); + +describe("needsRefit", () => { + test("fits once there is something to fit", () => { + expect(needsRefit(undefined, [])).toBe(false); + expect(needsRefit(undefined, [{ x: 0, y: 0 }])).toBe(true); + }); + + test("holds the view still while the trucks drive around inside it", () => { + expect(needsRefit(square, [{ x: 40, y: -40 }])).toBe(false); + }); + + test("refits when a truck reaches the edge", () => { + expect(needsRefit(square, [{ x: 95, y: 0 }])).toBe(true); + expect(needsRefit(square, [{ x: 0, y: -300 }])).toBe(true); + }); +}); + +describe("scaleBar", () => { + test("picks a round length that fits inside a quarter of the view", () => { + expect(scaleBar({ minX: 0, minY: 0, maxX: 1000, maxY: 1000 }).metres).toBe(200); + expect(scaleBar({ minX: 0, minY: 0, maxX: 300, maxY: 300 }).metres).toBe(50); + }); + + test("reports the fraction of the view the bar covers", () => { + expect(scaleBar({ minX: 0, minY: 0, maxX: 1000, maxY: 1000 }).fraction).toBeCloseTo(0.2); + }); +}); + +describe("baked road maps", () => { + const map: RoadMap = { + name: "Town10HD_Opt", + bbox: [-10, -20, 30, 40], + features: [ + { + geometry: { + coordinates: [ + [ + [0, 0], + [10, 5], + ], + [[1, 1]], + ], + }, + }, + ], + }; + + test("draws every lane as one path, skipping a lane with no length", () => { + expect(pathData(map)).toBe("M0 0 L10 5"); + }); + + test("takes the drawn view straight from the baked bounds", () => { + expect(mapView(map)).toEqual({ minX: -10, minY: -20, maxX: 30, maxY: 40 }); + expect(viewBox(mapView(map))).toBe("-10 -20 40 60"); + }); + + test("gives a town of one straight road something to draw on", () => { + // A single east-west road bakes to a bbox with no height at all; drawn + // as-is that is a zero SVG viewBox dimension and a non-finite scale bar. + const road = mapView({ ...map, bbox: [0, 50, 400, 50] }); + expect(road).toEqual({ minX: 0, maxX: 400, minY: 45, maxY: 55 }); + expect(scaleBar(road).fraction).toBeCloseTo(0.25); + + const northSouth = mapView({ ...map, bbox: [50, 0, 50, 400] }); + expect(northSouth).toEqual({ minX: 45, maxX: 55, minY: 0, maxY: 400 }); + expect(Number.isFinite(scaleBar(northSouth).fraction)).toBe(true); + }); +}); + +describe("fleetTown", () => { + test("has no town to show before any truck reports one", () => { + expect(fleetTown([])).toBeUndefined(); + }); + + test("shows the town most of the fleet is in", () => { + expect(fleetTown(["Town10HD_Opt", "Town04", "Town10HD_Opt"])).toBe("Town10HD_Opt"); + }); + + test("breaks a tie by name rather than by announcement order", () => { + expect(fleetTown(["Town10HD_Opt", "Town04"])).toBe("Town04"); + expect(fleetTown(["Town04", "Town10HD_Opt"])).toBe("Town04"); + }); +}); + +test("speed is labelled in km/h, which is how a truck reads", () => { + expect(speedLabel(0)).toBe("0 km/h"); + expect(speedLabel(13.9)).toBe("50 km/h"); +}); + +test("aerial imagery and roads fit together without shifting their coordinate origin", () => { + const map: RoadMap = { + name: "site", + bbox: [0, 0, 100, 100], + features: [], + aerial: { image: "site.aerial.png", bbox: [-20, -10, 120, 110] }, + }; + const fitted = mapView(map); + expect(fitted).toEqual({ minX: -20, minY: -10, maxX: 120, maxY: 110 }); + expect(project(fitted, { x: 50, y: 50 })).toEqual({ left: 50, top: 50 }); + map.aerial = { image: "site.aerial.png", bbox: [20, 20, 80, 80] }; + expect(mapView(map)).toEqual({ minX: 0, minY: 0, maxX: 100, maxY: 100 }); +}); + +test("map padding leaves room for road shoulders without moving the world origin", () => { + const map: RoadMap = { name: "site", bbox: [0, 0, 100, 100], features: [] }; + expect(mapView(map, 0.06)).toEqual({ minX: -6, minY: -6, maxX: 106, maxY: 106 }); + expect(project(mapView(map, 0.06), { x: 50, y: 50 })).toEqual({ left: 50, top: 50 }); +}); diff --git a/demo/carla/web/src/map.ts b/demo/carla/web/src/map.ts new file mode 100644 index 00000000000..857d2b449dc --- /dev/null +++ b/demo/carla/web/src/map.ts @@ -0,0 +1,195 @@ +/** + * The fleet map: baked CARLA road geometry, and the projection that puts truck + * poses on top of it. + * + * Everything here is in CARLA world METRES (x east, y south), which is also what + * the trucks put on their `status` track, so roads and markers need no common + * reference beyond this module. Screen y grows downward like CARLA's y, so the + * projection is a scale with no flip, and a yaw in degrees is a CSS rotation. + * + * The view is always widened to the container's exact aspect ratio rather than + * letterboxed by the SVG, because the markers are HTML positioned in percent: + * the two only line up if the viewBox and the box agree. + * + * @module + */ + +/** One baked town, written by the simulator's `moq_carla_sim.roadmap`. */ +export interface RoadMap { + name: string; + /** [minX, minY, maxX, maxY], metres. */ + bbox: [number, number, number, number]; + features: { geometry: { coordinates: [number, number][][] } }[]; + /** Calibrated top-down image in the same world metres as the roads. */ + aerial?: { image: string; bbox: [number, number, number, number] }; +} + +/** A rectangle of CARLA world metres, as drawn. */ +export interface View { + minX: number; + minY: number; + maxX: number; + maxY: number; +} + +/** Something with a position on the map. */ +export interface Located { + x: number; + y: number; +} + +/** Metres across the smallest auto-fitted view, so a convoy is not a wall of truck. */ +const MIN_SPAN = 250; + +/** Fraction of the span left empty around auto-fitted trucks. */ +const PADDING = 0.2; + +/** + * Metres, the narrowest a baked town may be drawn on either axis. + * + * A town whose lanes are all collinear - one straight road, which is what a + * hand-authored OpenDRIVE tends to look like - bakes to a bbox with zero span on + * an axis. Left alone that reaches the SVG as a zero viewBox dimension and makes + * the scale bar's fraction non-finite. + */ +const MIN_MAP_SPAN = 10; + +/** The rectangle a baked town covers, never thinner than it can be drawn. */ +export function mapView(map: RoadMap, padding = 0): View { + const [leftX, topY, rightX, bottomY] = map.bbox; + const aerial = map.aerial?.bbox ?? map.bbox; + const minX = Math.min(leftX, aerial[0]); + const minY = Math.min(topY, aerial[1]); + const maxX = Math.max(rightX, aerial[2]); + const maxY = Math.max(bottomY, aerial[3]); + const [left, right] = widen(minX, maxX, MIN_MAP_SPAN); + const [top, bottom] = widen(minY, maxY, MIN_MAP_SPAN); + const margin = Math.max(right - left, bottom - top) * padding; + return { minX: left - margin, maxX: right + margin, minY: top - margin, maxY: bottom + margin }; +} + +/** Grow an axis around its centre until it spans at least `span`. */ +function widen(low: number, high: number, span: number): [number, number] { + if (high - low >= span) return [low, high]; + const centre = (low + high) / 2; + return [centre - span / 2, centre + span / 2]; +} + +/** + * The town the map shows when a relay carries trucks from more than one. + * + * CARLA coordinates from different towns are unrelated, so the map can only be + * one of them: the town most trucks are in, ties broken by name so the choice is + * stable rather than a function of announcement order. Everyone else waits + * beside the map, the same place a truck with no position goes. + */ +export function fleetTown(towns: Iterable): string | undefined { + const counts = new Map(); + for (const town of towns) counts.set(town, (counts.get(town) ?? 0) + 1); + + let best: string | undefined; + let most = 0; + for (const [town, count] of [...counts].sort(([a], [b]) => (a < b ? -1 : 1))) { + if (count > most) { + most = count; + best = town; + } + } + return best; +} + +/** A square view around whatever positions we have, until a town is baked. */ +export function fitPoints(points: readonly Located[]): View | undefined { + if (points.length === 0) return undefined; + let minX = Infinity; + let minY = Infinity; + let maxX = -Infinity; + let maxY = -Infinity; + for (const { x, y } of points) { + minX = Math.min(minX, x); + maxX = Math.max(maxX, x); + minY = Math.min(minY, y); + maxY = Math.max(maxY, y); + } + // Square and floored: a convoy sits nose to tail, and fitting tightly to + // three trucks 12 m apart would zoom the map to a stretch of one road. + const span = Math.max(maxX - minX, maxY - minY, MIN_SPAN) * (1 + PADDING); + const x = (minX + maxX) / 2; + const y = (minY + maxY) / 2; + return { minX: x - span / 2, maxX: x + span / 2, minY: y - span / 2, maxY: y + span / 2 }; +} + +/** + * Whether an auto-fitted view has to be recomputed for these positions. + * + * Refitting on every pose would slide the whole map under the driver at 10 Hz. + * Instead the view holds still until a truck reaches `margin` of the edge. + */ +export function needsRefit(view: View | undefined, points: readonly Located[], margin = 0.08): boolean { + if (!view) return points.length > 0; + if (points.length === 0) return false; + const insetX = (view.maxX - view.minX) * margin; + const insetY = (view.maxY - view.minY) * margin; + return points.some( + ({ x, y }) => + x < view.minX + insetX || x > view.maxX - insetX || y < view.minY + insetY || y > view.maxY - insetY, + ); +} + +/** Widen the shorter axis until the view matches the container it is drawn in. */ +export function expandToAspect(view: View, aspect: number): View { + const width = view.maxX - view.minX; + const height = view.maxY - view.minY; + if (!Number.isFinite(aspect) || aspect <= 0 || width <= 0 || height <= 0) return view; + const x = (view.minX + view.maxX) / 2; + const y = (view.minY + view.maxY) / 2; + const spanX = width / height < aspect ? height * aspect : width; + const spanY = width / height < aspect ? height : width / aspect; + return { minX: x - spanX / 2, maxX: x + spanX / 2, minY: y - spanY / 2, maxY: y + spanY / 2 }; +} + +/** Where a position sits in the view, as CSS percentages. */ +export function project(view: View, point: Located): { left: number; top: number } { + const width = view.maxX - view.minX; + const height = view.maxY - view.minY; + return { + left: width > 0 ? ((point.x - view.minX) / width) * 100 : 50, + top: height > 0 ? ((point.y - view.minY) / height) * 100 : 50, + }; +} + +/** The SVG viewBox covering a view, in metres. */ +export function viewBox(view: View): string { + return `${view.minX} ${view.minY} ${view.maxX - view.minX} ${view.maxY - view.minY}`; +} + +/** Every lane in one SVG path, in metres. */ +export function pathData(map: RoadMap): string { + const parts: string[] = []; + for (const feature of map.features) { + for (const line of feature.geometry.coordinates) { + if (line.length < 2) continue; + parts.push(line.map(([x, y], index) => `${index ? "L" : "M"}${x} ${y}`).join(" ")); + } + } + return parts.join(" "); +} + +/** + * A round scale-bar length and the fraction of the view's width it covers. + * + * Round means 1, 2, or 5 times a power of ten, the largest that still fits in a + * quarter of the view, so the bar stays readable at every zoom. + */ +export function scaleBar(view: View, fraction = 0.25): { metres: number; fraction: number } { + const width = view.maxX - view.minX; + const budget = width * fraction; + const magnitude = 10 ** Math.floor(Math.log10(budget)); + const metres = [5, 2, 1].map((step) => step * magnitude).find((length) => length <= budget) ?? magnitude; + return { metres, fraction: metres / width }; +} + +/** A truck's speed as the map labels it. */ +export function speedLabel(metresPerSecond: number): string { + return `${Math.round(metresPerSecond * 3.6)} km/h`; +} diff --git a/demo/carla/web/src/maps/README.md b/demo/carla/web/src/maps/README.md new file mode 100644 index 00000000000..7bc01240899 --- /dev/null +++ b/demo/carla/web/src/maps/README.md @@ -0,0 +1,62 @@ +# Baked road maps + +One `.json` per CARLA town, drawn under the truck markers on the map. +Bake against a running simulator and commit the result: + +```bash +cd demo/carla/sim +./python.sh -m moq_carla_sim.roadmap --out ../web/src/maps +``` + +The file is a GeoJSON `FeatureCollection` of lane centrelines, but its +coordinates are **CARLA world metres** (x east, y south), not WGS84 degrees: +the same axes the trucks publish on `status.pose`, so the player projects both +with one transform. See `demo/carla/sim/moq_carla_sim/roadmap.py` for why, and +`map.ts` for the projection. + +The player matches the file name against the town each truck reports. Nothing +here is required: without a match it fits the view around the trucks themselves +and says so. + +## Aerial imagery + +Town10HD\_Opt includes a calibrated WebP capture. Its manifest preserves the +capture metadata and image checksum alongside the player's `name`, `image`, +and `bbox` fields. The supplied image already has its horizontal flip applied; +image right is +x and image down is +y. Do not flip it again. + +On the machine running CARLA, open a truck camera in the player to activate +rendering, then run: + +```bash +cd demo/carla/sim +./python.sh -m moq_carla_sim.aerial +``` + +The capture reads the loaded town and its baked road bounds. It creates one +temporary RGB camera and removes it even when capture fails; it never ticks, +loads a map, moves trucks, or changes world/render settings. `--carla-host`, +`--carla-port`, `--timeout`, `--resolution`, `--padding`, `--ground-z`, and `--fov` +allow an explicit capture configuration. The frame wait defaults to 30 seconds. + +Copy both `.aerial.png` and `.aerial.json` into this directory. +The manifest's `image` is a relative basename and `bbox` is +`[minX, minY, maxX, maxY]` in world metres, with +x right and +y down. It also +records `name`, image `width`/`height`, `groundZ`, the camera transform and FOV, +and the simulator `frame`. The PNG and manifest are a pair: ship them together. +WebP is also supported using `.aerial.webp` and the matching manifest. +Every visitor downloads the image, and the map fits the view to the roads +without zooming further, so ship at most 2560x1440 WebP (Town10HD\_Opt is about +350 KB) and update the manifest's size and checksum to match: + +```bash +ffmpeg -i .aerial.png -vf scale=2560:1440 -c:v libwebp -quality 70 .aerial.webp +``` + +Run `just fix` afterwards so the manifests match the repository's formatting. + +This is a perspective screenshot calibrated on the `--ground-z` plane, not an +orthophoto. Default narrow FOV limits building lean, but elevated terrain still +shifts. Check the result visually against the baked roads and actual truck +positions before shipping. Rendering or world streaming can leave absent +scenery; a successfully written file does not establish visual alignment. diff --git a/demo/carla/web/src/maps/Town10HD_Opt.aerial.json b/demo/carla/web/src/maps/Town10HD_Opt.aerial.json new file mode 100644 index 00000000000..7eb811dc812 --- /dev/null +++ b/demo/carla/web/src/maps/Town10HD_Opt.aerial.json @@ -0,0 +1,54 @@ +{ + "version": 1, + "map": "Carla/Maps/Town10HD_Opt", + "image": "Town10HD_Opt.aerial.webp", + "image_sha256": "a131ed672676660a9146a638ea4b40acbbc09f234d4c9d147dc83fbab5b1ec96", + "units": "metres", + "image_size": { + "width": 2560, + "height": 1440 + }, + "camera": { + "location": { + "x": -2.306732177734375, + "y": 36.241275787353516, + "z": 381.4052090099687 + }, + "rotation_degrees": { + "pitch": -90.0, + "yaw": 90.0, + "roll": 0.0 + }, + "horizontal_fov_degrees": 60.0 + }, + "projection": { + "type": "nadir_pinhole_ground_plane", + "ground_z": 0.0, + "world_bounds_at_image_edges": { + "min_x": -222.51113226996526, + "min_y": -87.62369926452635, + "max_x": 217.8976679144965, + "max_y": 160.10625083923338 + }, + "image_axes": { + "left_to_right": "carla_x_increasing", + "top_to_bottom": "carla_y_increasing" + }, + "normalized_mapping": { + "left": "(x - min_x) / (max_x - min_x)", + "top": "(y - min_y) / (max_y - min_y)" + } + }, + "capture": { + "road_bounds": { + "min_x": -114.59522247314453, + "min_y": -68.72904205322266, + "max_x": 109.98175811767578, + "max_y": 141.2115936279297 + }, + "padding_fraction": 0.18, + "horizontal_flip_applied": true + }, + "name": "Town10HD_Opt", + "bbox": [-222.51113226996526, -87.62369926452635, 217.8976679144965, 160.10625083923338] +} diff --git a/demo/carla/web/src/maps/Town10HD_Opt.aerial.webp b/demo/carla/web/src/maps/Town10HD_Opt.aerial.webp new file mode 100644 index 00000000000..0cf70f44cef Binary files /dev/null and b/demo/carla/web/src/maps/Town10HD_Opt.aerial.webp differ diff --git a/demo/carla/web/src/maps/Town10HD_Opt.json b/demo/carla/web/src/maps/Town10HD_Opt.json new file mode 100644 index 00000000000..79611c386a9 --- /dev/null +++ b/demo/carla/web/src/maps/Town10HD_Opt.json @@ -0,0 +1,501 @@ +{ + "type": "FeatureCollection", + "name": "Town10HD_Opt", + "bbox": [-114.2, -68.7, 110.0, 141.1], + "features": [ + { + "type": "Feature", + "properties": { "kind": "lane" }, + "geometry": { + "type": "MultiLineString", + "coordinates": [ + [[-27.0, 66.2], [-32.4, 65.5], [-38.6, 62.0], [-41.2, 57.7], [-41.7, 52.2]], + [[-48.7, 51.7], [-48.6, 81.7]], + [[-52.2, 51.7], [-52.1, 81.7]], + [[57.4, 16.8], [27.4, 16.8]], + [[57.4, 13.3], [27.4, 13.3]], + [[26.4, 28.3], [56.4, 28.3]], + [[26.4, 24.8], [56.4, 24.8]], + [[43.9, 41.9], [44.1, 36.3], [46.5, 31.9], [50.8, 29.1], [56.0, 28.3]], + [[-88.6, 12.9], [-96.3, 12.6], [-102.9, 9.8], [-106.0, 5.8], [-106.7, -1.3]], + [ + [-110.2, -2.3], + [-110.2, 9.9], + [-109.1, 14.2], + [-106.9, 18.1], + [-103.8, 21.3], + [-99.9, 23.5], + [-95.4, 24.4], + [-89.4, 24.4] + ], + [[-107.2, 43.9], [-106.7, -2.1]], + [[99.0, 53.2], [98.7, 58.8], [96.3, 63.3], [92.1, 65.9], [86.7, 66.4]], + [ + [83.0, 24.9], + [95.4, 24.4], + [99.6, 22.7], + [103.1, 20.1], + [105.9, 16.7], + [108.7, 10.9], + [109.8, 4.5], + [109.9, -3.5] + ], + [ + [106.1, 42.2], + [106.1, 28.1], + [105.2, 23.7], + [103.0, 19.7], + [99.8, 16.6], + [95.9, 14.5], + [91.5, 13.5], + [83.4, 13.4] + ], + [[83.0, 28.4], [90.6, 28.7], [95.4, 30.9], [98.5, 34.8], [99.1, 42.1]], + [[109.6, 42.2], [109.9, -3.8]], + [[106.1, 42.2], [106.4, -3.8]], + [[102.9, -4.5], [102.6, 41.5]], + [[99.4, -4.5], [99.1, 41.5]], + [[-28.5, 130.0], [-33.7, 129.0], [-38.0, 126.1], [-40.8, 121.6], [-41.5, 116.3]], + [[-52.0, 115.0], [-53.2, 122.0], [-56.5, 126.0], [-63.0, 128.9], [-70.2, 129.0]], + [ + [-48.5, 115.0], + [-48.9, 120.5], + [-50.1, 123.9], + [-54.7, 129.5], + [-61.0, 132.8], + [-64.5, 133.2], + [-70.1, 132.6] + ], + [[-52.1, -43.4], [-52.3, -17.4]], + [[-48.6, -43.4], [-48.8, -17.4]], + [[-45.3, -17.2], [-45.1, -43.2]], + [[-45.3, -17.2], [-45.1, -43.2]], + [[-41.8, -17.2], [-41.6, -43.2]], + [[-41.8, -17.2], [-41.6, -43.2]], + [[57.4, 28.3], [81.4, 28.4]], + [[57.4, 28.3], [81.4, 28.4]], + [[57.4, 24.8], [81.4, 24.9]], + [[57.4, 24.8], [81.4, 24.9]], + [[83.1, 16.9], [59.1, 16.8]], + [[83.1, 13.4], [59.1, 13.3]], + [[-48.8, -1.7], [-47.4, 8.7], [-43.4, 18.5], [-38.3, 22.8], [-34.0, 24.3], [-29.7, 24.6]], + [[-110.2, -2.3], [-110.2, 16.0], [-107.4, 22.2], [-104.0, 25.2], [-100.0, 27.2], [-89.5, 27.9]], + [[109.3, 83.0], [109.5, 55.0]], + [[105.8, 82.9], [106.0, 54.9]], + [[99.4, -4.5], [99.1, 3.1], [96.9, 7.8], [91.3, 12.0], [84.2, 13.4]], + [[-28.7, 28.1], [25.3, 28.3]], + [[-28.7, 28.1], [25.3, 28.3]], + [[-28.7, 24.6], [25.3, 24.8]], + [[26.4, 16.8], [-27.6, 16.6]], + [[26.4, 16.8], [-27.6, 16.6]], + [[26.4, 13.3], [-27.6, 13.1]], + [[26.4, 13.3], [-27.6, 13.1]], + [[-88.6, 12.9], [-94.1, 12.3], [-100.1, 8.7], [-102.7, 4.3], [-103.2, -1.1]], + [ + [-66.8, 24.5], + [-60.6, 24.4], + [-56.4, 23.4], + [-48.9, 18.9], + [-46.1, 15.6], + [-43.2, 9.8], + [-42.1, 5.7], + [-41.8, -0.6] + ], + [[-78.1, 28.0], [-68.1, 28.0]], + [[-78.1, 28.0], [-68.1, 28.0]], + [[-78.1, 24.5], [-68.1, 24.5]], + [[-78.1, 24.5], [-68.1, 24.5]], + [[-78.1, 24.5], [-68.1, 24.5]], + [[-66.8, 16.5], [-76.8, 16.5]], + [[-66.8, 13.0], [-76.8, 13.0]], + [ + [-88.7, 16.4], + [-101.1, 17.1], + [-105.2, 18.8], + [-108.7, 21.5], + [-111.4, 24.9], + [-113.8, 31.1], + [-114.2, 43.4] + ], + [[-88.7, 27.9], [-78.7, 28.0]], + [[-88.7, 24.4], [-78.7, 24.5]], + [[-78.0, 16.5], [-88.0, 16.4]], + [[-78.0, 16.5], [-88.0, 16.4]], + [[-78.0, 13.0], [-88.0, 12.9]], + [[-78.0, 13.0], [-88.0, 12.9]], + [[7.4, -67.9], [-4.5, -68.0]], + [[7.4, -64.4], [-4.6, -64.5]], + [[-5.3, -61.1], [6.7, -60.9]], + [[-5.4, -57.6], [6.7, -57.4]], + [[-41.8, -4.8], [-41.8, -16.8]], + [[-45.3, -4.8], [-45.3, -16.8]], + [[-48.8, -17.2], [-48.8, -5.2]], + [[-52.3, -17.2], [-52.3, -5.2]], + [[-41.8, -1.7], [-41.8, -3.7]], + [[-45.3, -1.7], [-45.3, -3.7]], + [[-48.8, -4.8], [-48.8, -2.8]], + [[-48.8, -4.8], [-48.8, -2.8]], + [[-48.8, -4.8], [-48.8, -2.8]], + [[-52.3, -4.8], [-52.3, -2.8]], + [[-52.3, -4.8], [-52.3, -2.8]], + [[-41.5, 114.9], [-41.6, 82.9]], + [[-41.5, 114.9], [-41.6, 82.9]], + [[-45.0, 115.0], [-45.1, 83.0]], + [[-48.6, 82.6], [-48.5, 114.6]], + [[-52.1, 82.7], [-52.0, 114.7]], + [[-28.7, 16.6], [-66.7, 16.5]], + [[-28.7, 13.1], [-66.7, 13.0]], + [[109.9, -9.3], [110.0, -21.3]], + [[106.4, -9.4], [106.5, -21.3]], + [[103.0, -22.7], [102.9, -10.7]], + [[99.5, -22.8], [99.4, -10.7]], + [ + [-66.8, 24.5], + [-60.5, 24.2], + [-56.3, 22.8], + [-49.5, 17.5], + [-46.9, 13.8], + [-45.5, 9.6], + [-45.3, -0.7] + ], + [[106.1, 42.2], [105.3, 29.8], [101.3, 22.1], [97.8, 19.2], [93.7, 17.4], [83.3, 16.9]], + [[-67.2, -58.2], [-29.2, -57.8]], + [[-67.1, -61.7], [-29.1, -61.3]], + [[83.0, 24.9], [95.5, 24.4], [101.3, 20.7], [103.7, 17.0], [105.7, 10.9], [106.4, -3.4]], + [ + [-45.2, 42.5], + [-45.9, 32.2], + [-48.3, 26.2], + [-50.8, 22.6], + [-54.0, 19.6], + [-58.0, 17.6], + [-66.5, 16.5] + ], + [[109.5, 53.3], [109.6, 43.3]], + [[106.0, 53.3], [106.1, 43.3]], + [[106.0, 53.3], [106.1, 43.3]], + [[106.0, 53.3], [106.1, 43.3]], + [[102.6, 42.2], [102.5, 52.2]], + [[99.1, 42.1], [99.0, 52.1]], + [[99.1, 42.1], [99.0, 52.1]], + [[40.5, 52.6], [40.3, 58.2], [38.1, 62.7], [33.9, 65.6], [28.6, 66.3]], + [[-41.7, 42.5], [-41.2, 37.1], [-38.8, 32.3], [-34.8, 29.1], [-29.8, 28.1]], + [[-27.8, -64.8], [-65.8, -65.2]], + [[-27.7, -68.3], [-65.7, -68.7]], + [[109.9, -4.4], [109.9, -8.4]], + [[106.4, -4.4], [106.4, -8.4]], + [[102.9, -9.4], [102.9, -5.4]], + [[99.4, -9.4], [99.4, -5.4]], + [[99.4, -9.4], [99.4, -5.4]], + [[-27.8, -64.8], [-38.3, -64.1], [-43.9, -60.3], [-47.9, -52.5], [-48.6, -44.1]], + [[58.1, 66.3], [28.1, 66.3]], + [[-0.1, 130.2], [-28.1, 130.0]], + [[-0.1, 130.2], [-28.1, 130.0]], + [[-0.1, 130.2], [-28.1, 130.0]], + [[-0.2, 133.7], [-28.2, 133.5]], + [[-28.6, 137.0], [-0.6, 137.2]], + [[-28.6, 140.5], [-0.6, 140.7]], + [ + [-70.5, 128.9], + [-80.3, 125.1], + [-88.6, 118.8], + [-94.9, 110.2], + [-99.8, 100.2], + [-102.4, 91.3], + [-103.9, 82.2], + [-103.7, 44.7] + ], + [ + [-71.3, 132.3], + [-82.0, 128.2], + [-89.7, 122.6], + [-95.9, 115.3], + [-102.3, 103.2], + [-105.8, 92.1], + [-107.3, 82.5], + [-107.2, 44.6] + ], + [ + [-71.3, 132.3], + [-82.0, 128.2], + [-89.7, 122.6], + [-95.9, 115.3], + [-102.3, 103.2], + [-105.8, 92.1], + [-107.3, 82.5], + [-107.2, 44.6] + ], + [ + [-110.7, 43.9], + [-110.9, 82.0], + [-109.4, 92.1], + [-105.8, 103.9], + [-99.2, 116.6], + [-92.6, 124.6], + [-82.5, 131.9], + [-72.8, 135.5] + ], + [ + [-114.2, 43.8], + [-114.2, 84.4], + [-112.4, 95.0], + [-108.2, 107.2], + [-102.2, 118.5], + [-95.1, 127.1], + [-84.1, 135.0], + [-73.6, 138.9] + ], + [ + [-103.2, -2.2], + [-102.6, -27.5], + [-100.9, -34.4], + [-97.2, -42.4], + [-92.0, -49.4], + [-85.2, -54.5], + [-77.3, -57.5], + [-68.6, -58.2] + ], + [ + [-103.2, -2.2], + [-102.6, -27.5], + [-100.9, -34.4], + [-97.2, -42.4], + [-92.0, -49.4], + [-85.2, -54.5], + [-77.3, -57.5], + [-68.6, -58.2] + ], + [ + [-106.7, -2.2], + [-106.5, -22.2], + [-105.7, -29.9], + [-103.6, -37.3], + [-99.2, -45.8], + [-93.1, -53.1], + [-85.3, -58.4], + [-78.1, -60.9], + [-68.6, -61.7] + ], + [ + [-67.1, -65.2], + [-77.4, -64.6], + [-87.3, -61.3], + [-94.3, -56.7], + [-101.3, -49.0], + [-106.3, -39.9], + [-109.2, -30.0], + [-110.2, -3.7] + ], + [ + [-67.1, -65.2], + [-77.4, -64.6], + [-87.3, -61.3], + [-94.3, -56.7], + [-101.3, -49.0], + [-106.3, -39.9], + [-109.2, -30.0], + [-110.2, -3.7] + ], + [ + [-67.1, -65.2], + [-77.4, -64.6], + [-87.3, -61.3], + [-94.3, -56.7], + [-101.3, -49.0], + [-106.3, -39.9], + [-109.2, -30.0], + [-110.2, -3.7] + ], + [ + [-67.0, -68.7], + [-78.0, -68.1], + [-88.9, -64.4], + [-96.6, -59.4], + [-104.2, -51.0], + [-109.5, -41.3], + [-112.7, -30.6], + [-113.4, -24.0], + [-113.7, -3.8] + ], + [[-28.5, 130.0], [-62.5, 129.8], [-69.5, 129.1]], + [[-28.5, 133.5], [-62.5, 133.3], [-70.2, 132.6]], + [[-27.8, -57.8], [-5.8, -57.6]], + [[-27.8, -61.3], [-5.8, -61.1]], + [[-5.3, -64.5], [-27.3, -64.8]], + [[-5.3, -64.5], [-27.3, -64.8]], + [[-5.3, -68.0], [-27.3, -68.3]], + [[-67.2, -58.2], [-59.8, -57.4], [-55.3, -54.7], [-52.6, -50.4], [-52.1, -45.1]], + [[-72.0, 135.7], [-63.8, 136.8], [-29.8, 137.0]], + [[-72.8, 139.1], [-63.9, 140.3], [-29.8, 140.5]], + [[-0.2, 140.7], [17.8, 141.0]], + [[-0.2, 137.2], [17.8, 137.5]], + [[19.4, 134.0], [1.4, 133.7]], + [[19.4, 130.5], [1.4, 130.2]], + [[44.0, 52.5], [43.9, 42.5]], + [[40.4, 41.9], [40.5, 51.9]], + [[40.4, 41.9], [40.5, 51.9]], + [ + [110.0, -22.6], + [109.6, -29.4], + [108.1, -36.0], + [103.5, -46.4], + [99.6, -51.9], + [94.9, -56.8], + [89.4, -60.8], + [83.5, -64.1], + [77.1, -66.4], + [68.1, -67.8], + [7.8, -67.9] + ], + [ + [106.5, -22.7], + [105.3, -33.0], + [100.6, -44.5], + [92.6, -54.1], + [82.0, -60.9], + [76.1, -63.0], + [67.9, -64.3], + [7.8, -64.4] + ], + [ + [7.4, -60.9], + [67.3, -60.9], + [78.5, -58.5], + [88.5, -53.0], + [96.3, -44.7], + [101.3, -34.4], + [103.0, -23.1] + ], + [ + [7.4, -57.4], + [67.1, -57.4], + [77.3, -55.3], + [86.3, -50.2], + [93.4, -42.7], + [98.0, -33.4], + [99.5, -23.1] + ], + [[-41.6, 82.6], [-41.2, 77.2], [-38.5, 72.9], [-34.0, 70.3], [-28.6, 69.7]], + [[86.0, 66.4], [60.0, 66.3]], + [[86.0, 66.4], [60.0, 66.3]], + [[58.0, 69.8], [84.0, 69.9]], + [[102.5, 53.2], [102.3, 81.2]], + [[99.0, 53.2], [98.8, 81.2]], + [[-41.6, -43.3], [-40.7, -48.6], [-38.0, -53.2], [-34.0, -56.6], [-28.9, -57.8]], + [[-110.2, -2.3], [-110.7, 43.7]], + [[-113.7, -2.3], [-114.2, 43.7]], + [[83.0, 28.4], [90.7, 28.9], [97.5, 31.6], [100.1, 33.8], [101.9, 36.7], [102.6, 42.0]], + [ + [-45.1, -43.3], + [-45.2, -51.5], + [-46.0, -55.8], + [-50.1, -63.4], + [-53.5, -66.3], + [-57.6, -68.1], + [-66.0, -68.7] + ], + [[27.1, 66.3], [-26.9, 66.2]], + [[-27.0, 69.7], [27.0, 69.8]], + [[-27.0, 69.7], [27.0, 69.8]], + [[-28.5, 130.0], [-35.8, 129.0], [-40.6, 126.4], [-43.9, 122.5], [-45.0, 115.4]], + [ + [-45.1, -43.3], + [-45.4, -51.7], + [-46.8, -55.9], + [-49.3, -59.6], + [-52.7, -62.5], + [-56.7, -64.4], + [-65.2, -65.2] + ], + [[27.1, 69.8], [57.1, 69.8]], + [[-41.7, 51.7], [-41.7, 43.7]], + [[-41.7, 51.7], [-41.7, 43.7]], + [[-45.2, 51.7], [-45.2, 43.7]], + [[-45.2, 51.7], [-45.2, 43.7]], + [[-48.7, 42.6], [-48.7, 50.6]], + [[-52.2, 42.6], [-52.2, 50.6]], + [[-28.7, 13.1], [-33.9, 12.3], [-38.2, 9.4], [-41.0, 4.9], [-41.8, -0.4]], + [[-66.8, 28.0], [-61.0, 28.0], [-56.3, 29.8], [-53.2, 33.8], [-52.2, 40.9]], + [ + [-88.7, 16.4], + [-99.1, 17.3], + [-103.1, 19.4], + [-106.3, 22.5], + [-108.7, 26.1], + [-110.2, 30.3], + [-110.7, 42.6] + ], + [[40.5, 52.6], [41.2, 61.0], [44.8, 66.9], [48.9, 69.2], [57.4, 69.8]], + [[-107.2, 43.9], [-106.6, 34.4], [-103.2, 28.2], [-100.5, 26.0], [-97.2, 24.7], [-89.7, 24.4]], + [[-41.6, -43.3], [-41.2, -50.9], [-39.0, -55.7], [-33.7, -60.2], [-28.6, -61.3]], + [[58.1, 66.3], [52.6, 65.8], [48.0, 63.3], [45.0, 59.1], [44.0, 53.9]], + [[-41.6, 82.6], [-41.7, 52.6]], + [[-45.1, 82.6], [-45.2, 52.6]], + [[27.1, 69.8], [33.6, 69.2], [39.4, 65.9], [43.2, 60.3], [44.0, 53.7]], + [[26.4, 28.3], [31.8, 28.8], [36.4, 31.3], [39.5, 35.5], [40.4, 40.7]], + [[-41.7, 42.5], [-41.8, -1.5]], + [[-45.2, 42.5], [-45.3, -1.5]], + [[-52.3, -1.7], [-53.2, 5.5], [-56.1, 9.9], [-60.5, 12.5], [-65.9, 13.0]], + [[86.0, 69.9], [91.6, 70.0], [96.1, 72.3], [98.6, 76.5], [98.8, 82.0]], + [[-28.7, 16.6], [-37.2, 17.5], [-41.2, 19.6], [-44.4, 22.8], [-48.1, 30.6], [-48.7, 40.9]], + [[-103.7, 43.9], [-102.8, 36.7], [-100.0, 32.2], [-95.7, 29.1], [-90.6, 27.9]], + [[-66.8, 28.0], [-28.8, 28.1]], + [[-66.8, 24.5], [-28.8, 24.6]], + [ + [19.3, 141.0], + [67.7, 141.1], + [76.7, 139.5], + [87.1, 134.8], + [92.6, 130.6], + [97.3, 125.7], + [104.3, 114.4], + [108.3, 101.8], + [109.3, 84.9] + ], + [ + [19.4, 137.5], + [67.5, 137.6], + [75.7, 136.1], + [85.2, 131.8], + [90.2, 128.0], + [94.6, 123.5], + [101.1, 112.9], + [104.9, 101.1], + [105.8, 84.9] + ], + [ + [102.3, 82.9], + [101.7, 98.6], + [98.7, 109.8], + [91.8, 121.3], + [83.3, 128.9], + [74.7, 132.8], + [67.2, 134.1], + [21.3, 134.0] + ], + [ + [98.8, 82.9], + [98.3, 98.1], + [95.4, 108.4], + [89.1, 119.1], + [81.4, 125.9], + [73.7, 129.4], + [66.9, 130.6], + [21.3, 130.5] + ], + [ + [-48.8, -1.7], + [-48.5, 12.6], + [-45.5, 20.7], + [-42.7, 24.2], + [-39.0, 26.7], + [-34.6, 28.0], + [-30.4, 28.1] + ], + [[-48.8, -1.7], [-48.7, 42.3]], + [[-52.3, -1.7], [-52.2, 42.3]] + ] + } + } + ] +} diff --git a/demo/carla/web/src/maps/index.ts b/demo/carla/web/src/maps/index.ts new file mode 100644 index 00000000000..39c522b55a6 --- /dev/null +++ b/demo/carla/web/src/maps/index.ts @@ -0,0 +1,48 @@ +/** Build-time map assets share the world-coordinate projection used by telemetry. */ +import type { RoadMap } from "../map.ts"; + +interface AerialManifest { + name: string; + image: string; + bbox: [number, number, number, number]; +} + +const BAKED = import.meta.glob(["./*.json", "!./*.aerial.json"], { import: "default" }); +const AERIALS = import.meta.glob("./*.aerial.json", { import: "default" }); +const IMAGES = import.meta.glob(["./*.aerial.png", "./*.aerial.webp"], { + query: "?url", + import: "default", + eager: true, +}); + +/** Unknown towns fall back to positions; an unreadable aerial keeps the roads. */ +export async function loadRoadMap(name: string): Promise { + const load = BAKED[`./${name}.json`]; + if (!load) return undefined; + const map = await load(); + const aerial = AERIALS[`./${name}.aerial.json`]; + if (!aerial) return map; + const manifest = await aerial(); + const image = IMAGES[`./${manifest.image}`]; + const bbox = manifest.bbox; + if ( + manifest.name !== name || + !image || + bbox.length !== 4 || + !bbox.every(Number.isFinite) || + bbox[2] <= bbox[0] || + bbox[3] <= bbox[1] + ) { + throw new Error(`Invalid aerial manifest for ${name}`); + } + try { + // Decode before adopting the layer: a failed asset must leave a usable road map. + const bitmap = new Image(); + bitmap.src = image; + await bitmap.decode(); + return { ...map, aerial: { image, bbox } }; + } catch (error) { + console.warn(`Aerial image for ${name} unavailable; using roads`, error); + return map; + } +} diff --git a/demo/carla/web/src/paths.test.ts b/demo/carla/web/src/paths.test.ts new file mode 100644 index 00000000000..56da4345593 --- /dev/null +++ b/demo/carla/web/src/paths.test.ts @@ -0,0 +1,27 @@ +import { describe, expect, test } from "bun:test"; +import { driverPath, parseCamera } from "./paths.ts"; + +describe("parseCamera", () => { + test("accepts exactly carla//.hang", () => { + expect(parseCamera("carla/truck1/front.hang")).toEqual({ truck: "truck1", id: "truck1/front" }); + }); + + test("ignores other broadcasts that share the relay", () => { + for (const path of [ + "truck1/front.hang", + "carla/front.hang", + "carla/truck1/front", + "carla/truck1/rear/extra.hang", + "other/truck1/front.hang", + "viewer/carla/truck1/abc", + ]) { + expect(parseCamera(path)).toBeUndefined(); + } + }); +}); + +describe("driverPath", () => { + test("roots every driver of a truck under viewer/carla/", () => { + expect(driverPath("truck1", "abc") as string).toBe("viewer/carla/truck1/abc"); + }); +}); diff --git a/demo/carla/web/src/paths.ts b/demo/carla/web/src/paths.ts new file mode 100644 index 00000000000..9c9394d80e7 --- /dev/null +++ b/demo/carla/web/src/paths.ts @@ -0,0 +1,33 @@ +/** + * The broadcast paths shared with the simulator. + * + * Trucks publish one media broadcast per camera at `carla/{truck}/{camera}.hang`. + * Drivers publish their `control` track at `viewer/carla/{truck}/{driver}`, a + * prefix the demo relay lets anonymous viewers publish under. + * + * @module + */ +import * as Moq from "@moq/net"; + +/** Every camera broadcast, and nothing else that shares the relay. */ +export const CAMERAS = Moq.Path.Pattern.parse("carla/*/*.hang"); + +/** One announced camera. */ +export interface Camera { + /** The truck publishing it, which is also its control scope. */ + truck: string; + /** Stable per-camera id, e.g. "truck1/front". */ + id: string; +} + +/** Parse a camera broadcast path, or `undefined` for anything else. */ +export function parseCamera(path: string): Camera | undefined { + if (!CAMERAS.matches(path)) return undefined; + const [, truck, file] = path.split("/"); + return { truck, id: `${truck}/${file.slice(0, -".hang".length)}` }; +} + +/** Where one driver of a truck publishes its control broadcast. */ +export function driverPath(truck: string, driver: string): Moq.Path.Valid { + return Moq.Path.from(`viewer/carla/${truck}/${driver}`); +} diff --git a/demo/carla/web/src/presence.test.ts b/demo/carla/web/src/presence.test.ts new file mode 100644 index 00000000000..d61fd6ea995 --- /dev/null +++ b/demo/carla/web/src/presence.test.ts @@ -0,0 +1,21 @@ +import { describe, expect, test } from "bun:test"; +import { presence } from "./presence.ts"; + +describe("presence", () => { + test("a connected relay with nothing announced means the simulator is offline", () => { + expect(presence("connected", 0)).toBe("offline"); + }); + + test("clears as soon as one vehicle is announced", () => { + expect(presence("connected", 1)).toBe("online"); + }); + + test("does not call the simulator offline before the relay answers", () => { + expect(presence("connecting", 0)).toBe("connecting"); + expect(presence("disconnected", 0)).toBe("connecting"); + }); + + test("keeps vehicles shown regardless of the session", () => { + expect(presence("disconnected", 2)).toBe("online"); + }); +}); diff --git a/demo/carla/web/src/presence.ts b/demo/carla/web/src/presence.ts new file mode 100644 index 00000000000..0eaf269fbb0 --- /dev/null +++ b/demo/carla/web/src/presence.ts @@ -0,0 +1,16 @@ +import type * as Moq from "@moq/net"; + +/** What the map has to show: vehicles, or why there are none. */ +export type Presence = "online" | "offline" | "connecting"; + +/** + * Whether the simulator is publishing, judged from the relay session and the + * vehicles announced on it. + * + * Only a connected session can say nothing is announced: before that, an empty + * map means the relay has not answered yet. + */ +export function presence(connection: Moq.Connection.Status, vehicles: number): Presence { + if (vehicles > 0) return "online"; + return connection === "connected" ? "offline" : "connecting"; +} diff --git a/demo/carla/web/src/preview.ts b/demo/carla/web/src/preview.ts new file mode 100644 index 00000000000..4fec2ed2f6d --- /dev/null +++ b/demo/carla/web/src/preview.ts @@ -0,0 +1,45 @@ +import * as Json from "@moq/json"; +import * as Moq from "@moq/net"; +import type { TruckStatus } from "./schemas.ts"; +import type { Truck } from "./truck.ts"; + +/** Disconnected fixtures, loaded only by Vite's development server at ?preview. */ +export function populatePreview(add: (path: Moq.Path.Valid) => Truck) { + const cleanup: (() => void)[] = []; + for (const [index, pose] of [ + { x: 30, y: 20, yaw: 0, speed: 5.4 }, + { x: 35, y: 22, yaw: 10, speed: 3.8 }, + { x: -40, y: 90, yaw: 90, speed: 0 }, + ].entries()) { + const truck = add(Moq.Path.from(`carla/truck${index + 1}/front.hang`)); + const origin = new Moq.Broadcast.Producer(); + const producer = new Json.Snapshot.Producer({ + track: origin.createTrack("status"), + compression: "deflate", + }); + (truck.broadcast.out.active as Moq.Signals.Signal).set(origin.consume()); + producer.update({ + value: { + accepted: { steering: 0, throttle: 0, brake: 1, gear: "neutral" }, + measured: { + steering: 0.15, + throttle: index === 2 ? 0 : 0.35, + brake: index === 2 ? 1 : 0, + gear: "forward", + }, + autonomy: "autonomous", + estopped: false, + map: "Town10HD_Opt", + pose, + media_pipeline: {}, + }, + }); + cleanup.push(() => { + producer.finish(); + origin.close(); + }); + } + return () => { + for (const close of cleanup) close(); + }; +} diff --git a/demo/carla/web/src/schemas.test.ts b/demo/carla/web/src/schemas.test.ts new file mode 100644 index 00000000000..8e446a590d0 --- /dev/null +++ b/demo/carla/web/src/schemas.test.ts @@ -0,0 +1,31 @@ +import { describe, expect, test } from "bun:test"; +import { TruckStatusSchema } from "./schemas.ts"; + +const status = { + accepted: { steering: 0, throttle: 0, brake: 1, gear: "neutral" }, + estopped: false, + media_pipeline: {}, +}; + +describe("TruckStatusSchema", () => { + test("defaults the optional fields", () => { + const parsed = TruckStatusSchema.parse(status); + expect(parsed.autonomy).toBe("unknown"); + expect(parsed.estop_by).toBeUndefined(); + expect(TruckStatusSchema.parse({ ...status, estopped: true, estop_by: "bystander" }).estop_by).toBe( + "bystander", + ); + }); + + test("ignores fields it does not know", () => { + const parsed = TruckStatusSchema.parse({ ...status, mode: "simulated", capabilities: ["respawn"] }); + expect(parsed).not.toHaveProperty("mode"); + expect(parsed.accepted.brake).toBe(1); + }); + + test("keeps an added autonomy value from rejecting all status", () => { + const parsed = TruckStatusSchema.parse({ ...status, autonomy: "remote" }); + expect(parsed.autonomy).toBe("unknown"); + expect(parsed.accepted.brake).toBe(1); + }); +}); diff --git a/demo/carla/web/src/schemas.ts b/demo/carla/web/src/schemas.ts new file mode 100644 index 00000000000..474aa3e527e --- /dev/null +++ b/demo/carla/web/src/schemas.ts @@ -0,0 +1,92 @@ +import { z } from "zod/mini"; + +/** Accept a string wire enum without letting an added producer value reject the whole object. */ +function openEnum(values: readonly Value[]) { + const known = new Set(values); + return z.pipe( + z.string(), + z.transform((value: string): Value | "unknown" => (known.has(value) ? (value as Value) : "unknown")), + ); +} + +/** Transmission gear, mirrored from the truck's `drive::Gear`. */ +export const GearSchema = z.enum(["neutral", "forward", "reverse"]); +export type Gear = z.infer; + +/** The drive state the truck is currently honoring. */ +export const DriveStateSchema = z.object({ + steering: z.number(), + throttle: z.number(), + brake: z.number(), + gear: GearSchema, +}); +export type DriveState = z.infer; + +/** + * Where a truck is, in CARLA world metres: x east, y south, yaw clockwise from + * +x, speed in m/s. Deliberately not latitude and longitude - see + * `demo/carla/sim/moq_carla_sim/roadmap.py`. + */ +export const PoseSchema = z.object({ + x: z.number(), + y: z.number(), + yaw: z.number(), + speed: z.number(), +}); +export type Pose = z.infer; + +/** + * How the truck actually responded, in its own frame: acceleration in m/s² + * (longitudinal positive forward, lateral positive right) and yaw rate in + * deg/s. + */ +export const MotionSchema = z.object({ + yaw_rate: z.number(), + accel_lon: z.number(), + accel_lat: z.number(), +}); +export type Motion = z.infer; + +/** + * The truck's most recent contact. + * + * `at` is unix milliseconds at the instant contact began. An absolute instant + * rather than an age, so a parked truck's snapshot stays unchanged - and so a + * viewer who opens the page long after the impact ages it correctly instead of + * treating the retained snapshot as a fresh crash. + */ +export const CollisionSchema = z.object({ + at: z.number(), + other: z.string(), + impulse: z.number(), +}); +export type Collision = z.infer; + +/** A camera receive/decode/render latency component echoed by the truck. */ +export const LatencyEntrySchema = z.object({ + label: z.string(), + ms: z.number(), +}); + +/** Per-update telemetry the truck publishes on the `status` track. */ +export const TruckStatusSchema = z.object({ + accepted: DriveStateSchema, + // Open, so a producer that adds a value degrades to `unknown` instead of + // rejecting every status from it. + autonomy: z._default(openEnum(["autonomous", "manual"]), "unknown"), + estopped: z.boolean(), + estop_by: z.optional(z.string()), + driver: z.optional(z.string()), + sequence: z.optional(z.number()), + media_pipeline: z.record(z.string(), z.array(LatencyEntrySchema)), + location: z.optional(z.string()), + // Absent until the simulator's first physics tick reports where the vehicle is. + map: z.optional(z.string()), + pose: z.optional(PoseSchema), + // `accepted` is the command the gate let through; these are what the + // vehicle did with it, and arrive whoever is driving. + measured: z.optional(DriveStateSchema), + motion: z.optional(MotionSchema), + collision: z.optional(CollisionSchema), +}); +export type TruckStatus = z.infer; diff --git a/demo/carla/web/src/style.css b/demo/carla/web/src/style.css new file mode 100644 index 00000000000..4486ba475e7 --- /dev/null +++ b/demo/carla/web/src/style.css @@ -0,0 +1,828 @@ +@import "./calibration.css"; +* { + box-sizing: border-box; + margin: 0; + padding: 0; +} +/* The same palette as the MoQ Boy demo. */ +:root { + color-scheme: dark; + --bg: #0a0a0a; + --panel: #141414; + --line: #333; + --text: #e0e0e0; + --muted: #888; + --dim: #555; + --accent: #8bac0f; + --accent-hover: #b4d63a; + --danger: #ed7169; + --ok: #8bac0f; + font-family: system-ui, sans-serif; +} +body { + background: var(--bg); + color: var(--text); + min-width: 320px; + min-height: 100vh; + display: flex; + flex-direction: column; +} +[hidden] { + /* biome-ignore lint/complexity/noImportantStyles: Hidden controls must override layout display rules. */ + display: none !important; +} +button, +summary, +a { + -webkit-tap-highlight-color: transparent; +} +button { + font: inherit; + cursor: pointer; +} +button:focus-visible, +a:focus-visible, +summary:focus-visible { + outline: 2px solid var(--accent); + outline-offset: 4px; +} +button:disabled { + opacity: 0.5; + cursor: not-allowed; +} +header { + padding: 0.75rem 1.5rem; + display: flex; + align-items: center; + gap: 1rem; + border-bottom: 1px solid var(--line); +} +header h1 { + font-size: 1.1rem; + font-weight: 600; + color: var(--accent); +} + +.status { + margin-left: auto; + font: + 12px ui-monospace, + monospace; + color: var(--muted); + display: flex; + align-items: center; + gap: 8px; +} +.status::before, +.status-dot { + content: ""; + width: 6px; + height: 6px; + background: currentColor; + border-radius: 50%; + display: inline-block; +} +.status[data-state="connected"] { + color: var(--ok); +} +.status[data-state="disconnected"] { + color: var(--danger); +} +.wheel { + border: 1px solid var(--line); + background: transparent; + color: var(--text); + padding: 8px 12px; + border-radius: 5px; + font-size: 12px; +} +.wheel[data-driving="true"] { + border-color: var(--accent); + color: var(--accent); +} +main { + width: 100%; + max-width: 1760px; + padding: 1rem 1.5rem; + margin: 0 auto; + flex: 1; +} + +.map-panel { + border: 1px solid var(--line); + border-radius: 8px; + overflow: hidden; +} +#map { + position: relative; + height: max(420px, calc(100dvh - 180px)); + background-color: #202020; + background-image: + linear-gradient(#ffffff04 1px, transparent 1px), linear-gradient(90deg, #ffffff04 1px, transparent 1px), + radial-gradient(ellipse at 40% 60%, #292929, #191919); + background-size: + 40px 40px, + 40px 40px, + auto; +} +#roads, +#markers { + position: absolute; + inset: 0; + width: 100%; + height: 100%; +} +#roads { + overflow: hidden; +} +#roads use { + fill: none; + stroke-linecap: round; + stroke-linejoin: round; +} +.road-shoulder { + stroke: #111111; + stroke-width: 6.5; +} +.road-surface { + stroke: #555555; + stroke-width: 4.2; +} +.road-line { + stroke: #aaaaaa; + stroke-width: 0.25; + stroke-dasharray: 2 3; + opacity: 0.55; +} +.compass { + position: absolute; + top: 20px; + right: 20px; + font: + 10px ui-monospace, + monospace; + color: #dddddd; + text-align: center; +} +.compass span { + display: block; + font-size: 28px; +} +#scale { + position: absolute; + left: 20px; + bottom: 20px; + color: #c7c7c7; + font: + 10px ui-monospace, + monospace; +} +.bar { + border: 1px solid currentColor; + border-top: none; + height: 5px; + margin-top: 6px; +} + +#map-note { + position: absolute; + top: 50%; + left: 50%; + transform: translate(-50%, -50%); + font-size: 13px; + line-height: 1.7; + text-align: center; + width: min(80%, 320px); + color: var(--muted); +} +/* Anchor previews to the truck while leaving the marker available to click. */ +.truck { + --preview-width: 238px; +} +#markers .truck { + position: absolute; +} +.pin { + position: absolute; + transform: translate(-50%, -50%); + width: 28px; + height: 28px; + background: var(--accent); + color: #0a0a0a; + border: 2px solid #222222; + border-radius: 7px; + box-shadow: + 0 0 0 3px #8bac0f30, + 0 3px 12px #0008; + z-index: 1; +} +.pin:hover, +.pin:focus-visible { + z-index: 3; + background: var(--accent-hover); +} +.arrow { + display: block; + margin: auto; + width: 15px; + height: 10px; + background: currentColor; + clip-path: polygon(0 0, 65% 0, 100% 50%, 65% 100%, 0 100%); +} +.pin-label { + position: absolute; + bottom: 35px; + left: 50%; + transform: translateX(-50%); + color: var(--text); + background: #151515ed; + padding: 5px 7px; + border: 1px solid var(--line); + border-radius: 3px; + font: + 10px ui-monospace, + monospace; + white-space: nowrap; + pointer-events: none; + opacity: 0; +} +.pin:hover .pin-label, +.pin:focus-visible .pin-label { + opacity: 1; +} +.camera-chooser { + margin: auto; + width: min(320px, 90vw); + padding: 16px; + border: 1px solid var(--line); + border-radius: 8px; + color: var(--text); + background: var(--panel); +} +.camera-chooser::backdrop { + background: #0009; +} +.camera-chooser button { + display: block; + width: 100%; + padding: 12px; + margin-top: 8px; + text-align: left; +} +.card { + background: var(--panel); + border: 1px solid var(--line); + border-radius: 8px; + overflow: hidden; +} +.truck:not([data-focused="true"]) .card { + position: absolute; + left: 50%; + bottom: 24px; + transform: translateX(-50%); + width: var(--preview-width); + z-index: 4; + box-shadow: 0 12px 36px #0006; +} +/* Bridge the gap so leaving the pin for its card never leaves the truck. */ +.truck[data-preview="true"]::before { + content: ""; + position: absolute; + left: 50%; + bottom: 0; + width: var(--preview-width); + height: 24px; + transform: translateX(-50%); +} +.estop-btn { + color: #ffd8d4; + background: #4b211f; + border: 1px solid #bd5a52; +} +.estop-btn[data-reset="true"] { + color: var(--ok); + border-color: var(--ok); + background: #1c2408; +} +.truck:not([data-focused="true"]) .video-wrap > .estop-btn { + position: absolute; + top: 8px; + right: 8px; + z-index: 2; + padding: 5px 7px; + font-size: 10px; +} +#markers .truck[data-preview="true"] { + z-index: 4; +} + +#backdrop { + position: fixed; + inset: 0; + background: #0a0a0ac9; + backdrop-filter: blur(5px); + z-index: 5; +} +.truck[data-focused="true"] .card { + position: fixed; + top: 50%; + left: 50%; + transform: translate(-50%, -50%); + width: min(1100px, 94vw, max(560px, calc((94dvh - 140px) * 16 / 9))); + max-height: 94dvh; + overflow-y: auto; + z-index: 6; + box-shadow: 0 28px 100px #0008; +} +.truck[data-active="true"] .card { + border-color: var(--accent); +} +.video-wrap { + position: relative; + aspect-ratio: 16 / 9; + background: #000; +} +.truck canvas { + display: block; + width: 100%; + height: 100%; + object-fit: contain; +} +.truck .overlay { + position: absolute; + inset: 0; + pointer-events: none; +} +.badge { + color: var(--muted); + font-size: 10px; + font-weight: 500; + text-transform: uppercase; + letter-spacing: 1px; +} +.badge[data-live="true"] { + color: var(--ok); +} +.badge[data-live="true"]::before { + content: ""; + display: inline-block; + width: 5px; + height: 5px; + margin-right: 6px; + border-radius: 50%; + background: var(--ok); +} +.video-state { + position: absolute; + left: 50%; + top: 55%; + transform: translate(-50%, -50%); + font-size: 12px; + color: var(--muted); + text-align: center; + width: 80%; +} +.close-btn { + position: absolute; + right: 14px; + top: 14px; + width: 32px; + height: 32px; + border-radius: 50%; + background: #0a0a0acc; + border: 1px solid var(--muted); + color: white; + font-size: 22px; + z-index: 2; +} +.body { + padding: 16px 22px; +} +.name-row, +.actions { + display: flex; + align-items: center; + gap: 10px; +} +.name { + font-size: 24px; + font-weight: 700; + text-transform: uppercase; + letter-spacing: 0.5px; +} +.speed { + display: flex; + align-items: baseline; + gap: 8px; + color: #fff; +} +.actions { + margin-left: auto; +} +.actions button { + border-radius: 4px; + font-size: 11px; + font-weight: 600; + padding: 10px 13px; + min-height: 36px; +} +.control-btn { + color: #0a0a0a; + background: var(--accent); + border: 1px solid var(--accent); +} +.respawn-btn { + background: transparent; + color: var(--text); + border: 1px solid var(--line); +} +.telemetry { + margin-top: 0; +} +.metric { + display: grid; + grid-template-columns: 1fr auto; + align-items: baseline; + gap: 5px 12px; +} +.metric > span:not(.pedal-track) { + font-size: 10px; + font-weight: 700; + letter-spacing: 1px; + text-transform: uppercase; + color: #ddd; +} +.metric strong { + font-size: 14px; + font-weight: 500; +} +meter { + display: block; + width: 100%; + height: 4px; + appearance: none; + border: 0; + background: var(--line); +} +meter::-webkit-meter-bar { + height: 4px; + border: 0; + background: var(--line); +} +meter::-webkit-meter-optimum-value { + background: var(--ok); +} +.brake-meter::-webkit-meter-optimum-value { + background: var(--danger); +} +.guide-key { + position: absolute; + bottom: 22px; + left: 50%; + transform: translateX(-50%); + display: flex; + gap: 16px; + font-size: 10px; + color: #ddd; + text-shadow: 0 1px 4px #000; +} +.actual-key::before, +.request-key::before { + content: ""; + display: inline-block; + width: 18px; + border-top: 2px solid var(--text); + vertical-align: middle; + margin-right: 6px; +} +.request-key::before { + border-top: 2px dashed #50c8ff; +} +.diagnostics { + margin-top: 10px; + font-size: 10px; + color: var(--muted); +} +summary { + cursor: pointer; +} +.row { + display: flex; + justify-content: space-between; + gap: 18px; + padding-top: 10px; + font-size: 11px; + overflow-wrap: anywhere; +} +.row span:last-child { + text-align: right; +} +.hint { + font-size: 11px; + color: var(--muted); + line-height: 1.6; + margin-top: 14px; +} +.mode.estopped, +.collision { + color: var(--danger); +} +.truck:not([data-focused="true"]) .telemetry, +.truck:not([data-focused="true"]) .hint, +.truck:not([data-focused="true"]) .actions, +.truck:not([data-focused="true"]) .close-btn { + display: none; +} +.truck:not([data-focused="true"]) .body { + padding: 12px; +} +.truck:not([data-focused="true"]) .name, +.truck:not([data-focused="true"]) .speed { + font-size: 11px; +} +.truck[data-collision="recent"] .pin { + border-color: var(--danger); + box-shadow: 0 0 0 2px #ed716940; +} +.truck[data-collision="recent"] .card { + border-color: var(--danger); +} +footer { + padding: 0.75rem 1.5rem; + border-top: 1px solid var(--line); + display: flex; + justify-content: space-between; + align-items: start; + color: var(--dim); + font-size: 0.75rem; + line-height: 1.7; + gap: 24px; +} +footer a { + color: var(--accent); + text-decoration: none; +} +footer a:hover { + text-decoration: underline; +} +footer details { + max-width: 600px; + flex: 1; + color: var(--muted); +} +footer p { + margin-top: 10px; +} +#offline { + position: absolute; + inset: 0; + z-index: 2; + display: grid; + place-content: center; + gap: 0.75rem; + padding: 1.5rem; + text-align: center; + background: #0a0a0acc; + /* Announcements land a moment after the session connects; fading in late keeps a + simulator that is online from flashing the offline message. */ + animation: reveal 0.3s ease 1s both; +} +#offline h2 { + font-size: 1.1rem; + font-weight: 600; + color: var(--accent); +} +#offline p { + max-width: 420px; + font-size: 0.85rem; + line-height: 1.7; + color: var(--muted); +} +#offline a { + color: var(--accent); +} +@keyframes reveal { + from { + opacity: 0; + } + to { + opacity: 1; + } +} +kbd { + border: 1px solid var(--line); + border-radius: 3px; + padding: 1px 4px; + color: var(--text); +} +#preview-notice { + background: var(--panel); + color: var(--accent); + padding: 10px 14px; + border: 1px solid var(--line); + border-radius: 8px; + font: + 10px ui-monospace, + monospace; + margin-bottom: 20px; +} +@media (max-width: 1000px) { + .truck { + --preview-width: 220px; + } +} +@media (max-width: 700px) { + header { + flex-wrap: wrap; + gap: 12px; + padding-inline: 1rem; + } + main { + padding: 1rem; + } + + #map { + height: 380px; + } + + .body { + padding: 16px; + } + .name-row { + flex-wrap: wrap; + } + .actions { + width: 100%; + margin: 6px 0 0; + } + .actions button { + flex: 1; + } + + .metric:nth-child(-n + 2) { + border-bottom: 1px solid var(--line); + } + .guide-key { + flex-wrap: wrap; + gap: 10px; + } + footer { + flex-wrap: wrap; + gap: 14px; + padding-inline: 1rem; + } + footer details { + order: 3; + flex-basis: 100%; + } +} + +.pedal-track { + position: relative; + display: block; + height: 4px; + margin-top: 3px; +} +.request-position { + position: absolute; + top: -3px; + height: 10px; + border-left: 2px solid #50c8ff; + transform: translateX(-1px); +} + +#roads[data-aerial="true"] use { + display: none; +} + +.hud-title { + position: absolute; + top: 22px; + left: 24px; + display: flex; + flex-direction: column; + gap: 6px; + text-shadow: 0 2px 8px #000; +} +.hud-pedals { + position: absolute; + bottom: 26px; + left: 24px; + width: 160px; + display: flex; + flex-direction: column; + gap: 14px; + text-shadow: 0 1px 6px #000; +} +.hud-motion { + position: absolute; + bottom: 22px; + right: 24px; + display: flex; + align-items: center; + gap: 18px; + text-shadow: 0 2px 8px #000; +} +.speed-value { + font-size: 64px; + line-height: 1; + font-weight: 700; + font-variant-numeric: tabular-nums; + letter-spacing: -2px; +} +.speed > span { + font-size: 11px; + color: #ddd; +} +.gear { + display: grid; + place-items: center; + width: 48px; + height: 54px; + border: 1px solid #ffffff70; + font-size: 28px; + color: var(--accent); + background: #1118; +} +.mode { + font-size: 12px; + text-transform: uppercase; + letter-spacing: 1px; + color: var(--muted); +} +.pedal-track { + grid-column: 1 / -1; +} +.video-wrap::after { + content: ""; + position: absolute; + inset: 0; + background: linear-gradient(#0006, transparent 25%, transparent 60%, #0009); + pointer-events: none; +} +.hud-title, +.hud-pedals, +.hud-motion, +.guide-key, +.video-state { + z-index: 1; + pointer-events: none; +} +.truck:not([data-focused="true"]) .hud-pedals, +.truck:not([data-focused="true"]) .hud-motion, +.truck:not([data-focused="true"]) .body { + display: none; +} +.truck:not([data-focused="true"]) .hud-title { + top: 12px; + left: 12px; + right: 65px; +} +.truck:not([data-focused="true"]) .name { + font-size: 13px; +} +@media (max-width: 700px) { + .hud-title { + top: 14px; + left: 14px; + } + .name { + font-size: 17px; + } + .hud-pedals { + bottom: 16px; + left: 14px; + width: 95px; + gap: 8px; + } + .metric strong { + font-size: 11px; + } + .metric > span:not(.pedal-track) { + font-size: 8px; + } + .hud-motion { + bottom: 14px; + right: 14px; + gap: 8px; + } + .speed-value { + font-size: 38px; + } + .speed > span { + font-size: 8px; + } + .gear { + width: 28px; + height: 34px; + font-size: 20px; + } + .guide-key { + bottom: 5px; + font-size: 8px; + gap: 10px; + } + .body { + padding: 12px; + } +} diff --git a/demo/carla/web/src/truck.test.ts b/demo/carla/web/src/truck.test.ts new file mode 100644 index 00000000000..2cd1905e793 --- /dev/null +++ b/demo/carla/web/src/truck.test.ts @@ -0,0 +1,481 @@ +import { describe, expect, test } from "bun:test"; +import * as Json from "@moq/json"; +import * as Moq from "@moq/net"; +import { applyKeys, REST } from "./input.ts"; +import type { TruckStatus } from "./schemas.ts"; +import { nextGear, SafetyCommandBroadcast, shiftGear, Truck } from "./truck.ts"; + +describe("Truck video demand", () => { + test("only hovered or focused cameras subscribe, and focus suppresses other previews", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const hovered = new Moq.Signals.Signal(undefined); + const trucks = ["carla/truck1/front.hang", "carla/truck2/front.hang"].map( + (id) => + new Truck({ + broadcastId: Moq.Path.from(id), + connection, + driving, + focused, + hovered, + }), + ); + const settle = () => new Promise((resolve) => setTimeout(resolve, 0)); + const demand = () => trucks.map((truck) => truck.videoDecoder.in.enabled.peek()); + try { + await settle(); + expect(demand()).toEqual([false, false]); + hovered.set(trucks[0].id); + await settle(); + expect(demand()).toEqual([true, false]); + expect(trucks[0].videoSource.in.target.peek()).toEqual({ name: "sd" }); + hovered.set(trucks[1].id); + await settle(); + expect(demand()).toEqual([false, true]); + focused.set(trucks[0].id); + await settle(); + expect(demand()).toEqual([true, false]); + expect(trucks[0].videoSource.in.target.peek()).toEqual({ name: "hd" }); + focused.set(undefined); + hovered.set(undefined); + await settle(); + expect(demand()).toEqual([false, false]); + expect(driving.peek()).toBeUndefined(); + } finally { + for (const truck of trucks) truck.close(); + connection.close(); + } + }); +}); + +describe("Truck status", () => { + function status(throttle: number): TruckStatus { + return { + accepted: { steering: 0.25, throttle, brake: 0, gear: "forward" }, + autonomy: "manual", + estopped: false, + media_pipeline: {}, + }; + } + + function statusOrigin() { + const origin = new Moq.Broadcast.Producer(); + const track = origin.createTrack("status"); + const producer = new Json.Snapshot.Producer({ track, compression: "deflate" }); + return { origin, producer }; + } + + test("clears valid telemetry when the status track ends", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + const { origin, producer } = statusOrigin(); + + try { + const available = truck.statusState.changed(); + (truck.broadcast.out.active as Moq.Signals.Signal).set( + origin.consume(), + ); + producer.update({ value: status(0.5) }); + expect(await available).toBe("available"); + expect(truck.status.peek()?.accepted.throttle).toBe(0.5); + + const unavailable = truck.statusState.changed(); + producer.finish(); + expect(await unavailable).toBe("unavailable"); + expect(truck.status.peek()).toBeUndefined(); + } finally { + producer.finish(); + origin.close(); + truck.close(); + connection.close(); + } + }); + + test("a status from before the truck saw an e-stop does not unlatch it", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + const { origin, producer } = statusOrigin(); + const next = async (update: TruckStatus) => { + const changed = truck.status.changed(); + producer.update({ value: update }); + await changed; + }; + + try { + (truck.broadcast.out.active as Moq.Signals.Signal).set( + origin.consume(), + ); + await next(status(0.5)); + truck.emergencyStop(); + await next(status(0.4)); + expect(truck.estopped.peek()).toBe(true); + + // Once echoed, the truck's status is authoritative again. + await next({ ...status(0), estopped: true }); + await next(status(0)); + expect(truck.estopped.peek()).toBe(false); + } finally { + producer.finish(); + origin.close(); + truck.close(); + connection.close(); + } + }); + + test("marks status unavailable when an active broadcast disappears", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + const { origin, producer } = statusOrigin(); + const active = truck.broadcast.out.active as Moq.Signals.Signal; + + try { + active.set(origin.consume()); + producer.update({ value: status(0.5) }); + while (truck.statusState.peek() !== "available") await truck.statusState.changed(); + + const unavailable = truck.statusState.changed(); + active.set(undefined); + expect(await unavailable).toBe("unavailable"); + expect(truck.status.peek()).toBeUndefined(); + } finally { + producer.finish(); + origin.close(); + truck.close(); + connection.close(); + } + }); + + test("a canceled status consumer cannot overwrite its replacement", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + const first = statusOrigin(); + const second = statusOrigin(); + const active = truck.broadcast.out.active as Moq.Signals.Signal; + + try { + active.set(first.origin.consume()); + first.producer.update({ value: status(0.25) }); + while (truck.statusState.peek() !== "available") await truck.statusState.changed(); + + const transitions: string[] = []; + const unsubscribe = truck.statusState.subscribe((state) => transitions.push(state)); + try { + active.set(second.origin.consume()); + second.producer.update({ value: status(0.75) }); + while (truck.status.peek()?.accepted.throttle !== 0.75) await truck.status.changed(); + await Promise.resolve(); + await Promise.resolve(); + expect(truck.statusState.peek()).toBe("available"); + expect(truck.status.peek()?.accepted.throttle).toBe(0.75); + expect(transitions).not.toContain("unavailable"); + } finally { + unsubscribe(); + } + } finally { + first.producer.finish(); + second.producer.finish(); + first.origin.close(); + second.origin.close(); + truck.close(); + connection.close(); + } + }); +}); + +describe("Truck.setDrive", () => { + function truck() { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const t = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + return { t, connection }; + } + + // Guards setDrive's contract: local state advances however small the step, + // while the epsilon throttles only the wire. The frame loop's own test lives + // in input.test.ts's stepDrive suite; this one calls setDrive directly and so + // cannot catch a gate added at the call site. + test("accumulates sub-epsilon frames instead of discarding them", () => { + const { t, connection } = truck(); + try { + const dt = 1 / 120; + for (let i = 0; i < 120; i++) { + const cur = t.drive.peek(); + t.setDrive(applyKeys(cur, new Set(["d"]), dt)); + } + // One second at 120 Hz arrives below the publish epsilon every frame. + expect(t.drive.peek().steering).toBeCloseTo(0.45, 2); + } finally { + t.close(); + connection.close(); + } + }); + + test("a single sub-epsilon frame still moves the local state", () => { + const { t, connection } = truck(); + try { + t.setDrive(applyKeys(t.drive.peek(), new Set(["d"]), 1 / 60)); + expect(t.drive.peek().steering).toBeGreaterThan(0); + } finally { + t.close(); + connection.close(); + } + }); + + test("preserves measured controls while claiming control", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal("truck1/front"); + const t = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + try { + const measured = { steering: -0.35, throttle: 0.6, brake: 0.1, gear: "forward" } as const; + t.status.set({ + accepted: { ...REST }, + autonomy: "autonomous", + estopped: false, + media_pipeline: {}, + measured, + }); + expect(t.beginTakeover()).toBe(true); + driving.set(t.id); + await Promise.resolve(); + expect(t.active.peek()).toBe(true); + expect(t.drive.peek()).toEqual(measured); + } finally { + t.close(); + connection.close(); + } + }); + + test("refuses a takeover before the truck reports status", () => { + const { t, connection } = truck(); + try { + expect(t.beginTakeover()).toBe(false); + expect(t.drive.peek()).toEqual(REST); + } finally { + t.close(); + connection.close(); + } + }); +}); + +describe("Truck safety controls", () => { + test("a viewer e-stop does not claim the driving session", () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + + try { + truck.emergencyStop(); + expect(driving.peek()).toBeUndefined(); + expect(truck.active.peek()).toBe(false); + expect(truck.estopped.peek()).toBe(true); + } finally { + truck.close(); + connection.close(); + } + }); + + test("one safety snapshot reaches every current truck consumer", async () => { + const safety = new SafetyCommandBroadcast({ + cmd: { type: "estop" }, + sequence: 7, + timestamps: [], + }); + const front = new Json.Snapshot.Consumer>({ + track: safety.broadcast.track("control").subscribe(), + compression: "deflate", + }); + const rear = new Json.Snapshot.Consumer>({ + track: safety.broadcast.track("control").subscribe(), + compression: "deflate", + }); + + try { + expect((await front.next())?.value).toEqual({ type: "estop", sequence: 7, timestamps: [] }); + expect((await rear.next())?.value).toEqual({ type: "estop", sequence: 7, timestamps: [] }); + } finally { + safety.close(); + } + }); + + test("a focused viewer resets only after the truck confirms the latch", () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + + try { + truck.estopped.set(true); + truck.resetEstop(); + expect(truck.estopped.peek()).toBe(true); + + truck.status.set({ + accepted: { ...REST }, + autonomy: "unknown", + estopped: true, + estop_by: "bystander", + media_pipeline: {}, + }); + truck.resetEstop(); + expect(driving.peek()).toBeUndefined(); + expect(truck.estopped.peek()).toBe(false); + } finally { + truck.close(); + connection.close(); + } + }); + + test("the tile safety action exits a confirmed e-stop without claiming control", () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal(undefined); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + + try { + truck.toggleEmergencyStop(); + expect(truck.estopped.peek()).toBe(true); + truck.status.set({ + accepted: { ...REST }, + autonomy: "manual", + estopped: true, + media_pipeline: {}, + }); + + truck.toggleEmergencyStop(); + + expect(truck.estopped.peek()).toBe(false); + expect(driving.peek()).toBeUndefined(); + } finally { + truck.close(); + connection.close(); + } + }); +}); + +describe("Truck respawn", () => { + test("requires the truck's current lease and clears old input", async () => { + const connection = new Moq.Connection({ enabled: false }); + const driving = new Moq.Signals.Signal(undefined); + const focused = new Moq.Signals.Signal("truck1/front"); + const truck = new Truck({ + broadcastId: Moq.Path.from("carla/truck1/front.hang"), + connection, + driving, + focused, + }); + + try { + const active = truck.active.changed(); + driving.set(truck.id); + expect(await active).toBe(true); + + truck.driverId.set("mine"); + const status = { + accepted: { ...REST }, + autonomy: "manual", + estopped: false, + driver: "someone-else", + media_pipeline: {}, + } satisfies TruckStatus; + truck.status.set(status); + expect(truck.respawn()).toBe(false); + truck.status.set({ ...status, driver: "mine" }); + truck.estopped.set(true); + expect(truck.respawn()).toBe(false); + truck.estopped.set(false); + truck.setDrive({ steering: 0.5, throttle: 0.8, brake: 0, gear: "forward" }); + + expect(truck.respawn()).toBe(true); + expect(truck.drive.peek()).toEqual(REST); + expect(truck.respawning.peek()).toBe(true); + truck.setDrive({ throttle: 1 }); + expect(truck.drive.peek()).toEqual(REST); + + const released = driving.changed(); + truck.status.set({ ...status, driver: undefined }); + expect(await released).toBeUndefined(); + expect(truck.respawning.peek()).toBe(false); + } finally { + truck.close(); + connection.close(); + } + }); +}); + +describe("nextGear", () => { + test("cycles N → F → R → N", () => { + expect(nextGear("neutral")).toBe("forward"); + expect(nextGear("forward")).toBe("reverse"); + expect(nextGear("reverse")).toBe("neutral"); + }); +}); + +describe("shiftGear", () => { + test("steps one gear at a time", () => { + expect(shiftGear("neutral", 1)).toBe("forward"); + expect(shiftGear("neutral", -1)).toBe("reverse"); + expect(shiftGear("reverse", 1)).toBe("neutral"); + expect(shiftGear("forward", -1)).toBe("neutral"); + }); + + test("never wraps forward into reverse", () => { + expect(shiftGear("forward", 1)).toBe("forward"); + expect(shiftGear("reverse", -1)).toBe("reverse"); + }); +}); diff --git a/demo/carla/web/src/truck.ts b/demo/carla/web/src/truck.ts new file mode 100644 index 00000000000..d1d97661656 --- /dev/null +++ b/demo/carla/web/src/truck.ts @@ -0,0 +1,502 @@ +/** + * The non-UI truck steering session backend. + * + * Shaped like moq-boy's `Game`: a video pipeline pulled from `@moq/watch`, a + * `control` track we publish, and a `status` track we subscribe to for + * round-trip confirmation. The UI layer reads signals from this class to render + * video, the control panel, and telemetry. + * + * @module + */ +import { Milli, Nano } from "@moq/clock"; +import * as Json from "@moq/json"; +import * as Moq from "@moq/net"; +import * as Watch from "@moq/watch"; +import { changed, REST, takeoverState } from "./input.ts"; +import { driverPath, parseCamera } from "./paths.ts"; +import type { DriveState, Gear, TruckStatus } from "./schemas.ts"; +import { TruckStatusSchema } from "./schemas.ts"; + +/** Configuration for creating a Truck session. */ +export interface TruckConfig { + /** Broadcast path as announced by the relay, e.g. "carla/truck1/front.hang". */ + broadcastId: Moq.Path.Valid; + /** MoQ connection to the relay. */ + connection: Moq.Connection; + /** Shared signal tracking which truck is currently being driven. */ + driving: Moq.Signals.Signal; + /** Shared signal tracking which truck is currently focused. */ + focused: Moq.Signals.Signal; + /** Only the hovered camera streams a fleet preview. */ + hovered?: Moq.Signals.Signal; +} + +/** A control command with captured timestamps, published via the command signal. */ +interface Command { + cmd: Record; + sequence: number; + timestamps: { label: string; ts: number }[]; +} + +/** A retained safety snapshot that fans out to every truck consumer. */ +export class SafetyCommandBroadcast { + readonly broadcast: Moq.Broadcast.Producer; + readonly #producer: Json.Snapshot.Producer>; + + constructor(command: Command, broadcast = new Moq.Broadcast.Producer()) { + this.broadcast = broadcast; + const track = this.broadcast.createTrack("control"); + this.#producer = new Json.Snapshot.Producer>({ track, compression: "deflate" }); + this.#producer.update({ + value: { ...command.cmd, sequence: command.sequence, timestamps: command.timestamps }, + }); + } + + close() { + this.#producer.finish(); + this.broadcast.close(); + } +} + +const CONTROL_HEARTBEAT = Nano.fromMilli(Milli(250)); +const MAX_PENDING_ACKS = 64; + +/** + * A single truck streaming + steering session (the non-UI backend). + */ +export class Truck { + /** Stable per-camera id, e.g. "truck1/front". */ + readonly id: string; + /** The truck this camera belongs to, shared by all of its cameras' control. */ + readonly name: string; + readonly broadcastId: Moq.Path.Valid; + readonly #signals = new Moq.Signals.Effect(); + readonly #driving: Moq.Signals.Signal; + + // Reactive state exposed to the UI. + readonly active = new Moq.Signals.Signal(false); + readonly status = new Moq.Signals.Signal(undefined); + readonly statusState = new Moq.Signals.Signal<"pending" | "available" | "unavailable">("pending"); + readonly driverId = new Moq.Signals.Signal(undefined); + readonly estopped = new Moq.Signals.Signal(false); + readonly respawning = new Moq.Signals.Signal(false); + readonly roundTripMs = new Moq.Signals.Signal(undefined); + + // The driver's locally-requested drive state (what we are sending). + readonly drive = new Moq.Signals.Signal({ ...REST }); + + // Watch API objects, exposed so the UI can attach a canvas. + readonly broadcast: Watch.Broadcast; + readonly sync: Watch.Sync; + readonly videoSource: Watch.Video.Source; + readonly videoDecoder: Watch.Video.Decoder; + readonly videoRenderer: Watch.Video.Renderer; + + // Internal control publishing state. + #command = new Moq.Signals.Signal(undefined); + #safetyCommand = new Moq.Signals.Signal(undefined); + #sequence = 0; + #sentAt = new Map(); + #sawStatusSource = false; + /** This player's e-stop, not yet echoed in a status. */ + #estopUnconfirmed = false; + + // The canvas is created by the UI layer after construction, so the renderer + // takes it as a signal input rather than a property set afterwards. + readonly #canvas = new Moq.Signals.Signal(undefined); + + constructor(config: TruckConfig) { + const { broadcastId, connection, driving, focused, hovered } = config; + const camera = parseCamera(broadcastId); + if (!camera) throw new Error(`not a camera broadcast: ${broadcastId}`); + this.broadcastId = broadcastId; + this.id = camera.id; + this.name = camera.truck; + this.#driving = driving; + + // Active when this truck is the one being driven. + this.#signals.run(this.#runActive.bind(this)); + this.#signals.run(this.#runControlLease.bind(this)); + this.#signals.run(this.#runRespawn.bind(this)); + this.#signals.interval(() => this.#heartbeat(), CONTROL_HEARTBEAT); + + // Video pipeline. + this.broadcast = new Watch.Broadcast({ + origin: connection.origin, + name: broadcastId, + enabled: true, + }); + this.#signals.cleanup(() => this.broadcast.close()); + + // The source produces the jitter estimate Sync reads, so it is built first + // to avoid a construction cycle. + const videoTarget = new Moq.Signals.Computed((effect) => ({ + name: effect.get(focused) === this.id ? "hd" : "sd", + })); + this.#signals.cleanup(() => videoTarget.close()); + this.videoSource = new Watch.Video.Source({ + broadcast: this.broadcast, + target: videoTarget, + supported: Watch.Video.Decoder.supported, + }); + this.#signals.cleanup(() => this.videoSource.close()); + + // Drive-by-wire wants the equivalent of ``: a + // zero jitter buffer plus an unpaced decoder, so each frame paints the moment + // it decodes instead of waiting on the playback clock. There is no audio to + // keep in step. + this.sync = new Watch.Sync({ + delay: "instant", + probe: connection.probe, + }); + this.#signals.cleanup(() => this.sync.close()); + + // Keep status subscribed while idle; only an inspected camera owns video demand. + const videoEnabled = new Moq.Signals.Signal(false); + this.#signals.run((effect) => { + const panel = effect.get(focused); + videoEnabled.set(panel === this.id || (!panel && hovered !== undefined && effect.get(hovered) === this.id)); + }); + this.videoDecoder = new Watch.Video.Decoder({ + source: this.videoSource, + sync: this.sync, + enabled: videoEnabled, + }); + this.#signals.cleanup(() => this.videoDecoder.close()); + + this.videoRenderer = new Watch.Video.Renderer({ decoder: this.videoDecoder, canvas: this.#canvas }); + this.#signals.cleanup(() => this.videoRenderer.close()); + + // Subscribe to the truck's status (telemetry) track. + this.#signals.run(this.#runStatus.bind(this)); + + // Publish control while this truck is being driven. + this.#signals.run(this.#runControl.bind(this, connection)); + this.#signals.run(this.#runSafetyControl.bind(this, connection)); + } + + /** Attach the canvas the UI created so the renderer can draw into it. */ + setCanvas(canvas: HTMLCanvasElement | undefined) { + this.#canvas.set(canvas); + } + + /** Prepare a bumpless handoff from the latest vehicle feedback. */ + beginTakeover(): boolean { + const status = this.status.peek(); + if (this.estopped.peek() || !status) return false; + this.drive.set(takeoverState(status.measured)); + this.#sentDrive = undefined; + return true; + } + + /** Merge a partial drive update and publish it. */ + setDrive(patch: Partial) { + if (this.estopped.peek() || this.respawning.peek()) return; + const next = { ...this.drive.peek(), ...patch }; + // The local state ALWAYS advances. Only the wire is throttled: a caller + // integrating at frame rate hands us deltas far below the epsilon, and + // dropping those on the floor would leave steering motionless. + this.drive.set(next); + if (this.#sentDrive && this.#sentDrive.gear === next.gear && !changed(this.#sentDrive, next)) return; + this.#sendDrive(next); + } + + /** Publish a drive command and remember it as the epsilon's baseline. */ + #sendDrive(state: DriveState) { + // Copy: the heartbeat hands us the signal's live object, and an in-place + // mutation of it would leave the epsilon comparing a value to itself. + this.#sentDrive = { ...state }; + this.#send({ type: "drive", ...state }); + } + + /** Request an emergency stop. */ + /** + * Latch e-stop on this truck even without holding the driving lease. A viewer + * sends it on a one-shot safety broadcast, leaving the driving session alone. + */ + emergencyStop() { + this.respawning.set(false); + this.drive.set({ ...REST }); + this.#sentDrive = undefined; + this.#estopUnconfirmed = true; + this.estopped.set(true); + if (this.active.peek()) { + this.#send({ type: "estop" }); + } else { + this.#sendSafety({ type: "estop" }); + } + } + + /** Toggle the safety action exposed by a truck tile. */ + toggleEmergencyStop() { + if (this.estopped.peek() || this.status.peek()?.estopped) { + this.resetEstop(); + } else { + this.emergencyStop(); + } + } + + /** Explicitly clear the e-stop latch into the full-brake state. */ + resetEstop() { + // Wait for the authoritative echo. Otherwise a quick e-stop then reset can + // replace the stop command before the truck ever receives and latches it. + if (!this.status.peek()?.estopped) return; + this.respawning.set(false); + this.drive.set({ ...REST }); + this.#sentDrive = undefined; + this.estopped.set(false); + if (this.active.peek()) { + this.#send({ type: "reset_estop" }); + } else { + this.#sendSafety({ type: "reset_estop" }); + } + } + + /** Return the truck to a spawn point while this player holds its lease. */ + respawn(): boolean { + const status = this.status.peek(); + if ( + !this.active.peek() || + this.respawning.peek() || + this.estopped.peek() || + !status || + status.estopped || + status.driver !== this.driverId.peek() + ) { + return false; + } + // Pause drive traffic until status confirms the server cleared the lease. + // Otherwise the heartbeat could reclaim and brake the fresh spawn. + this.respawning.set(true); + this.drive.set({ ...REST }); + this.#sentDrive = undefined; + this.#send({ type: "respawn" }); + return true; + } + + /** The drive state last put on the wire, so the publish epsilon measures against it. */ + #sentDrive: DriveState | undefined; + + #send(cmd: Record) { + if (cmd.type === "estop" || cmd.type === "reset_estop") this.#safetyCommand.set(undefined); + const command = this.#makeCommand(cmd); + this.#sentAt.set(command.sequence, performance.now()); + while (this.#sentAt.size > MAX_PENDING_ACKS) { + const oldest = this.#sentAt.keys().next().value; + if (oldest == null) break; + this.#sentAt.delete(oldest); + } + this.#command.set(command, true); + } + + #sendSafety(cmd: Record) { + this.#safetyCommand.set(this.#makeCommand(cmd), true); + } + + #makeCommand(cmd: Record): Command { + this.#sequence = (this.#sequence % Number.MAX_SAFE_INTEGER) + 1; + return { cmd, sequence: this.#sequence, timestamps: this.#timestamps() }; + } + + #heartbeat() { + if (!this.active.peek() || this.respawning.peek()) return; + if (this.estopped.peek()) { + this.#send({ type: "estop" }); + } else { + // Also the floor under the publish epsilon: whatever it held back + // reaches the truck within one heartbeat. + this.#sendDrive(this.drive.peek()); + } + } + + /** Collect media timestamps at each pipeline stage for latency measurement. */ + #timestamps(): { label: string; ts: number }[] { + const entries: { label: string; ts: number }[] = []; + const received = this.sync.out.timestamp.peek(); + if (received != null) entries.push({ label: "received", ts: received }); + const decoded = this.videoDecoder.out.timestamp.peek(); + if (decoded != null) entries.push({ label: "decoded", ts: decoded }); + const rendered = this.videoRenderer.out.timestamp.peek(); + if (rendered != null) entries.push({ label: "rendered", ts: rendered }); + return entries; + } + + close() { + this.#signals.close(); + } + + // --- Effect callbacks --- + + #runActive(effect: Moq.Signals.Effect) { + this.active.set(effect.get(this.#driving) === this.id); + } + + #runControlLease(effect: Moq.Signals.Effect) { + if (!effect.get(this.active)) { + // Releasing control closes the driving broadcast, which may not have + // delivered this player's e-stop yet: carry it on the safety broadcast, + // which stays up until a status echoes the latch. + if (this.#estopUnconfirmed) this.#sendSafety({ type: "estop" }); + this.respawning.set(false); + this.drive.set({ ...REST }); + this.#command.set(undefined); + return; + } + + // Inactive already reset the state. Resetting again on activation races + // the input that claimed control: held keys recover on the next frame, but + // an edge-triggered gear press would be silently lost. + this.#heartbeat(); + } + + #runRespawn(effect: Moq.Signals.Effect) { + if (!effect.get(this.respawning)) return; + const status = effect.get(this.status); + const driverId = effect.get(this.driverId); + if (!status || status.driver === driverId) return; + this.respawning.set(false); + if (this.#driving.peek() === this.id) this.#driving.set(undefined); + } + + #runStatus(effect: Moq.Signals.Effect) { + const active = effect.get(this.broadcast.out.active); + this.status.set(undefined); + if (!active) { + this.statusState.set(this.#sawStatusSource ? "unavailable" : "pending"); + return; + } + this.#sawStatusSource = true; + this.statusState.set("pending"); + + const statusTrack = active.track("status").subscribe({ priority: 10 }); + effect.cleanup(() => statusTrack.close()); + + const consumer = new Json.Snapshot.Consumer({ + track: statusTrack, + schema: TruckStatusSchema, + compression: "deflate", + }); + + effect.spawn(async () => { + for (;;) { + let status: TruckStatus | undefined; + try { + status = (await consumer.latest())?.value; + if (effect.abort.aborted) break; + } catch (err) { + if (effect.abort.aborted) break; + // Refuse a malformed track rather than guess: control stays + // unavailable until the broadcast is announced afresh. + console.warn("Invalid status JSON:", err); + this.status.set(undefined); + this.statusState.set("unavailable"); + break; + } + if (!status) { + this.status.set(undefined); + this.statusState.set("unavailable"); + break; + } + // A status sent before the truck saw this player's e-stop must not + // unlatch it: the heartbeat would then send drive frames, and a + // snapshot track keeps only the newest, so the stop could be lost. + if (status.estopped) this.#estopUnconfirmed = false; + if (!this.#estopUnconfirmed) this.estopped.set(status.estopped); + this.#acknowledgeSafety(status); + this.#acknowledge(status); + this.status.set(status); + this.statusState.set("available"); + } + }); + } + + #runControl(connection: Moq.Connection, effect: Moq.Signals.Effect) { + const conn = effect.get(connection.origin); + if (!conn) return; + + if (!effect.get(this.active)) return; + + const driverId = crypto.randomUUID().replaceAll("-", "").slice(0, 12); + this.driverId.set(driverId); + + const driverBroadcast = conn.createBroadcast(driverPath(this.name, driverId)); + effect.cleanup(() => { + driverBroadcast.close(); + this.driverId.set(undefined); + this.roundTripMs.set(undefined); + this.#sentAt.clear(); + }); + + const track = driverBroadcast.createTrack("control"); + const producer = new Json.Snapshot.Producer>({ track, compression: "deflate" }); + effect.cleanup(() => producer.finish()); + effect.run(this.#runControlTrack.bind(this, track, producer)); + // A fresh epoch per lease: the truck binds its single driver to this path and epoch. + driverBroadcast.announce({ epoch: Moq.Epoch.mint() }); + } + + #runSafetyControl(connection: Moq.Connection, effect: Moq.Signals.Effect) { + const conn = effect.get(connection.origin); + const command = effect.get(this.#safetyCommand); + if (!conn || !command) return; + + const driverId = `safety-${crypto.randomUUID().replaceAll("-", "").slice(0, 12)}`; + const broadcast = conn.createBroadcast(driverPath(this.name, driverId)); + const safety = new SafetyCommandBroadcast(command, broadcast); + broadcast.announce({ epoch: Moq.Epoch.mint() }); + effect.cleanup(() => safety.close()); + } + + #runControlTrack( + track: Moq.Track.Producer, + producer: Json.Snapshot.Producer>, + effect: Moq.Signals.Effect, + ) { + if (effect.get(track.closed) !== undefined) return; + + const command = effect.get(this.#command); + if (!command) return; + + producer.update({ value: { ...command.cmd, sequence: command.sequence, timestamps: command.timestamps } }); + } + + #acknowledge(status: TruckStatus) { + if (status.driver !== this.driverId.peek() || status.sequence == null) return; + const sentAt = this.#sentAt.get(status.sequence); + if (sentAt == null) return; + this.roundTripMs.set(performance.now() - sentAt); + for (const sequence of this.#sentAt.keys()) { + if (sequence <= status.sequence) this.#sentAt.delete(sequence); + } + } + + #acknowledgeSafety(status: TruckStatus) { + const command = this.#safetyCommand.peek(); + if (!command) return; + const type = command.cmd.type; + if ((type === "estop" && status.estopped) || (type === "reset_estop" && !status.estopped)) { + this.#safetyCommand.set(undefined); + } + } +} + +/** Cycle gear N → F → R → N for a UI button. */ +export function nextGear(gear: Gear): Gear { + return gear === "neutral" ? "forward" : gear === "forward" ? "reverse" : "neutral"; +} + +/** Gear order for the shift paddles, from reverse up through forward. */ +const GEARS: readonly Gear[] = ["reverse", "neutral", "forward"]; + +/** + * Step one gear in `direction` without wrapping, for a wheel's shift paddles. + * + * Unlike {@link nextGear}, this never rolls forward straight into reverse - a + * paddle at the end of its range does nothing rather than slamming the + * transmission through neutral. + */ +export function shiftGear(gear: Gear, direction: 1 | -1): Gear { + const index = GEARS.indexOf(gear); + if (index < 0) return gear; + return GEARS[Math.max(0, Math.min(GEARS.length - 1, index + direction))] ?? gear; +} diff --git a/demo/carla/web/src/vite-env.d.ts b/demo/carla/web/src/vite-env.d.ts new file mode 100644 index 00000000000..4ef6324115a --- /dev/null +++ b/demo/carla/web/src/vite-env.d.ts @@ -0,0 +1,9 @@ +/// + +interface ImportMetaEnv { + readonly VITE_RELAY_URL?: string; +} + +interface ImportMeta { + readonly env: ImportMetaEnv; +} diff --git a/demo/carla/web/src/wheel.test.ts b/demo/carla/web/src/wheel.test.ts new file mode 100644 index 00000000000..82e832efd8e --- /dev/null +++ b/demo/carla/web/src/wheel.test.ts @@ -0,0 +1,402 @@ +import { describe, expect, test } from "bun:test"; +import { REST } from "./input.ts"; +import type { DriveState } from "./schemas.ts"; +import { + applyWheel, + arbitrate, + Calibration, + DEFAULT_PROFILE, + failSafe, + looksLikeWheel, + type PadLike, + pickAxis, + pickButton, + readPedal, + readSteer, + shortName, + WHEEL_RATES, + Wheel, + type WheelSample, +} from "./wheel.ts"; + +const DT = 1 / 60; + +/** Build a pad snapshot with the given axes and pressed button indices. */ +function pad(axes: number[], pressed: number[] = [], id = "Test Wheel", index = 0): PadLike { + const buttons = Array.from({ length: 8 }, (_, i) => ({ pressed: pressed.includes(i) })); + return { id, index, axes, buttons, mapping: "", connected: true }; +} + +/** Chase a wheel target for `seconds`, the way the drive loop does. */ +function chase( + state: DriveState, + target: { steering: number; throttle: number; brake: number }, + seconds: number, +): DriveState { + let s = { ...state }; + for (let t = 0; t < seconds; t += DT) { + s = { ...s, ...applyWheel(s, target, DT) }; + } + return s; +} + +describe("readSteer", () => { + const map = DEFAULT_PROFILE.steer; + + test("maps the rim straight through", () => { + expect(readSteer([0], map)).toBe(0); + expect(readSteer([1], map)).toBeCloseTo(1); + expect(readSteer([-1], map)).toBeCloseTo(-1); + }); + + test("ignores wobble around center but still reaches full lock", () => { + expect(readSteer([0.01], map)).toBe(0); + expect(readSteer([-0.01], map)).toBe(0); + expect(readSteer([0.5], map)).toBeGreaterThan(0.4); + }); + + test("handles an inverted rim from calibration", () => { + const inverted = { axis: 0, rest: 0, full: -1 }; + expect(readSteer([-1], inverted)).toBeCloseTo(1); + expect(readSteer([1], inverted)).toBeCloseTo(-1); + }); + + test("clamps past the calibrated stops", () => { + expect(readSteer([2], map)).toBeCloseTo(1); + expect(readSteer([-2], map)).toBeCloseTo(-1); + }); + + test("a missing axis reads as centered rather than NaN", () => { + expect(readSteer([], map)).toBe(0); + }); +}); + +describe("readPedal", () => { + // The common layout: released at +1, floored at -1. + const map = DEFAULT_PROFILE.throttle; + + test("released is zero and floored is one", () => { + expect(readPedal([0, 1], map)).toBe(0); + expect(readPedal([0, -1], map)).toBe(1); + }); + + test("reaches the stops despite deadzone and saturation", () => { + expect(readPedal([0, 0.97], map)).toBe(0); + expect(readPedal([0, -0.97], map)).toBe(1); + expect(readPedal([0, 0], map)).toBeGreaterThan(0.4); + expect(readPedal([0, 0], map)).toBeLessThan(0.6); + }); + + test("never goes negative or past one", () => { + expect(readPedal([0, 2], map)).toBe(0); + expect(readPedal([0, -2], map)).toBe(1); + }); + + test("handles a pedal that rests at zero and rises to one", () => { + const rising = { axis: 1, rest: 0, full: 1 }; + expect(readPedal([0, 0], rising)).toBe(0); + expect(readPedal([0, 1], rising)).toBe(1); + }); +}); + +describe("applyWheel", () => { + test("tracks the rim absolutely, unlike the hold-style keys", () => { + const held = chase({ ...REST }, { steering: 0.6, throttle: 0, brake: 1 }, 2); + expect(held.steering).toBeCloseTo(0.6); + // Letting the rim return to center returns the steering with it. + expect(chase(held, { steering: 0, throttle: 0, brake: 1 }, 2).steering).toBeCloseTo(0); + }); + + test("rate-limits a yank without lagging normal steering", () => { + // One frame cannot cross more than the rate allows. + const yanked = applyWheel({ ...REST }, { steering: 1, throttle: 0, brake: 1 }, DT); + expect(yanked.steering).toBeCloseTo(WHEEL_RATES.steer * DT); + // But a hand-speed sweep is passed through within the same frame. + const sweep = applyWheel({ ...REST }, { steering: 0.02, throttle: 0, brake: 1 }, DT); + expect(sweep.steering).toBeCloseTo(0.02); + }); + + test("pedals drive throttle and brake independently", () => { + const rolling = chase({ ...REST }, { steering: 0, throttle: 0.8, brake: 0 }, 2); + expect(rolling.throttle).toBeCloseTo(0.8); + expect(rolling.brake).toBeCloseTo(0); + }); + + test("clamps a miscalibrated sample into range", () => { + const out = applyWheel({ ...REST }, { steering: 5, throttle: 5, brake: -5 }, 10); + expect(out.steering).toBe(1); + expect(out.throttle).toBe(1); + expect(out.brake).toBe(0); + }); +}); + +/** A wheel reading, defaulting to centered and at rest. */ +function sample(patch: Partial = {}): WheelSample { + return { + steering: 0, + throttle: 0, + brake: 0, + pressed: new Set(), + moved: false, + ...patch, + }; +} + +describe("arbitrate", () => { + test("a wheel sitting still does not take control from the keyboard", () => { + expect(arbitrate("keyboard", sample()).source).toBe("keyboard"); + }); + + test("moving the wheel takes control", () => { + expect(arbitrate("keyboard", sample({ steering: 0.3, moved: true })).source).toBe("wheel"); + }); + + test("a depressed throttle cannot be what hands over control", () => { + // A foot resting on the pedal while the rim is bumped must not arm it. + const armed = arbitrate("keyboard", sample({ steering: 0.3, throttle: 0.5, moved: true })); + expect(armed.source).toBe("keyboard"); + // Releasing the pedal and moving again does. + expect(arbitrate("keyboard", sample({ steering: 0.4, moved: true })).source).toBe("wheel"); + }); + + test("an armed wheel keeps control while it sits still", () => { + expect(arbitrate("wheel", sample({ steering: 0.5 })).source).toBe("wheel"); + // Including with the throttle down - the arming rule is for handover only. + expect(arbitrate("wheel", sample({ throttle: 0.8 })).source).toBe("wheel"); + }); + + test("losing an armed wheel falls back to the keyboard and reports the loss", () => { + expect(arbitrate("wheel", undefined)).toEqual({ source: "keyboard", lost: true }); + }); + + test("losing a wheel that was never armed is not a loss to act on", () => { + expect(arbitrate("keyboard", undefined)).toEqual({ source: "keyboard", lost: false }); + }); + + test("the loss only fires on the transition, not every tick after", () => { + let src = arbitrate("wheel", undefined); + expect(src.lost).toBe(true); + src = arbitrate(src.source, undefined); + expect(src.lost).toBe(false); + }); +}); + +describe("failSafe", () => { + test("cuts the throttle and brakes when the wheel vanishes mid-drive", () => { + const rolling: DriveState = { steering: 0.4, throttle: 0.9, brake: 0, gear: "forward" }; + const safe = failSafe(rolling); + expect(safe.throttle).toBe(0); + expect(safe.brake).toBe(1); + }); + + test("holds steering rather than centering it", () => { + // Unplugging mid-corner must not trade a runaway for a swerve. + const cornering: DriveState = { steering: -0.7, throttle: 0.5, brake: 0, gear: "forward" }; + expect(failSafe(cornering).steering).toBe(-0.7); + expect(failSafe(cornering).steering).not.toBe(REST.steering); + }); + + test("is idempotent, so a repeat cannot make things worse", () => { + const rolling: DriveState = { steering: 0.4, throttle: 0.9, brake: 0, gear: "forward" }; + const once = { ...rolling, ...failSafe(rolling) }; + expect(failSafe(once)).toEqual(failSafe(rolling)); + }); +}); + +describe("looksLikeWheel", () => { + test("matches known wheels by name", () => { + expect(looksLikeWheel(pad([0, 1, 1], [], "Logitech G29 Driving Force Racing Wheel"))).toBe(true); + expect(looksLikeWheel(pad([0, 1, 1], [], "Thrustmaster T300RS"))).toBe(true); + }); + + test("accepts an unfamiliar unmapped device with enough axes", () => { + expect(looksLikeWheel(pad([0, 1, 1], [], "Unknown HID device"))).toBe(true); + }); + + test("rejects a standard gamepad", () => { + const gamepad: PadLike = { ...pad([0, 0, 0, 0], [], "Xbox Controller"), mapping: "standard" }; + expect(looksLikeWheel(gamepad)).toBe(false); + }); +}); + +describe("pickAxis", () => { + test("picks the axis that moved furthest, not the first that moved", () => { + // Turning the rim cross-talks into the pedal axes a little. + expect(pickAxis([0, 1, 1], [0.3, 0.2, 1], new Set())).toBe(1); + }); + + test("ignores axes already bound to another control", () => { + expect(pickAxis([0, 1, 1], [0.3, 0.2, 1], new Set([1]))).toBe(0); + }); + + test("returns nothing when the driver didn't move enough", () => { + expect(pickAxis([0, 1, 1], [0.05, 0.98, 1], new Set())).toBeUndefined(); + }); +}); + +describe("pickButton", () => { + test("finds a newly pressed button", () => { + expect(pickButton([false, false], [false, true])).toBe(1); + }); + + test("ignores buttons already held at the baseline", () => { + expect(pickButton([true, false], [true, false])).toBeUndefined(); + }); + + test("ignores buttons already bound", () => { + expect(pickButton([false, false], [true, true], new Set([0]))).toBe(1); + }); +}); + +describe("Calibration", () => { + /** Walk the axis half of the sequence for a wheel with inverted pedals. */ + function walkAxes(cal: Calibration) { + cal.update(pad([0, 1, 1])); + expect(cal.capture()).toBe(true); // rest + cal.update(pad([1, 1, 1])); + expect(cal.capture()).toBe(true); // full right lock + cal.update(pad([0, -1, 1])); + expect(cal.capture()).toBe(true); // throttle floored + cal.update(pad([0, 1, -1])); + expect(cal.capture()).toBe(true); // brake floored + } + + test("learns the axis layout from the driver's movements", () => { + const cal = new Calibration(); + walkAxes(cal); + expect(cal.skip()).toBe(true); // gear up + expect(cal.skip()).toBe(true); // gear down + expect(cal.skip()).toBe(true); // e-stop + expect(cal.done).toBe(true); + + const profile = cal.result(); + expect(profile).toBeDefined(); + if (!profile) return; + expect(profile.steer).toEqual({ axis: 0, rest: 0, full: 1 }); + expect(profile.throttle).toEqual({ axis: 1, rest: 1, full: -1 }); + expect(profile.brake).toEqual({ axis: 2, rest: 1, full: -1 }); + expect(profile.gearUp).toBeUndefined(); + + // And the learned profile reads back the way it was demonstrated. + expect(readSteer([1, 1, 1], profile.steer)).toBeCloseTo(1); + expect(readPedal([0, -1, 1], profile.throttle)).toBe(1); + expect(readPedal([0, 1, 1], profile.brake)).toBe(0); + }); + + test("a control that barely moves is rejected, leaving the step up", () => { + const cal = new Calibration(); + cal.update(pad([0, 1, 1])); + cal.capture(); + const step = cal.step; + cal.update(pad([0.05, 1, 1])); + expect(cal.capture()).toBe(false); + expect(cal.step).toBe(step); + }); + + test("required steps cannot be skipped", () => { + const cal = new Calibration(); + expect(cal.skip()).toBe(false); + expect(cal.index).toBe(0); + }); + + test("buttons bind on the press itself", () => { + const cal = new Calibration(); + walkAxes(cal); + cal.update(pad([0, 1, 1], [4])); + expect(cal.index).toBe(5); + cal.update(pad([0, 1, 1], [4, 5])); + expect(cal.index).toBe(6); + expect(cal.skip()).toBe(true); + + const profile = cal.result(); + expect(profile?.gearUp).toBe(4); + expect(profile?.gearDown).toBe(5); + expect(profile?.estop).toBeUndefined(); + }); + + test("a button held across a step boundary does not bind twice", () => { + const cal = new Calibration(); + walkAxes(cal); + cal.update(pad([0, 1, 1], [4])); // binds gear up + cal.update(pad([0, 1, 1], [4])); // still held: must not bind gear down + expect(cal.result()).toBeUndefined(); + expect(cal.index).toBe(5); + }); + + test("an incomplete walk yields no profile", () => { + const cal = new Calibration(); + cal.update(pad([0, 1, 1])); + cal.capture(); + expect(cal.result()).toBeUndefined(); + }); +}); + +describe("Wheel button edges", () => { + /** Drive a Wheel against a scripted sequence of pad states. */ + function polling(states: PadLike[]): (WheelSample | undefined)[] { + const pads = [...states]; + const original = navigator.getGamepads; + // @ts-expect-error - stubbing the Gamepad API for the duration of the test. + navigator.getGamepads = () => [pads.shift() ?? null]; + try { + const wheel = new Wheel(); + return states.map(() => wheel.poll()); + } finally { + navigator.getGamepads = original; + } + } + + test("a paddle already held at connect is not a press", () => { + // Plugging in while resting a hand on the shift paddle must not shift. + const [first, second] = polling([ + pad([0, 1, 1], [4]), // adopted with the paddle already down + pad([0, 1, 1], [4]), // still down + ]); + expect(first?.pressed.has("gearUp")).toBe(false); + expect(second?.pressed.has("gearUp")).toBe(false); + }); + + test("releasing and pressing that same paddle does register", () => { + const samples = polling([pad([0, 1, 1], [4]), pad([0, 1, 1], []), pad([0, 1, 1], [4])]); + expect(samples[2]?.pressed.has("gearUp")).toBe(true); + }); + + test("a press is an edge, not a level", () => { + const samples = polling([pad([0, 1, 1], []), pad([0, 1, 1], [4]), pad([0, 1, 1], [4])]); + expect(samples[1]?.pressed.has("gearUp")).toBe(true); + expect(samples[2]?.pressed.has("gearUp")).toBe(false); + }); + + test("connecting does not report movement", () => { + const [first] = polling([pad([0.5, 1, 1]), pad([0.5, 1, 1])]); + expect(first?.moved).toBe(false); + }); + + test("reports loss before adopting a different connected wheel", () => { + const first = pad([0, 1, 1], [], "Wheel A", 0); + const replacement = pad([0.8, -1, 1], [], "Wheel B", 1); + const snapshots: (PadLike | null)[][] = [[first, replacement], [replacement], [replacement]]; + const original = navigator.getGamepads; + // @ts-expect-error - stubbing the Gamepad API for the duration of the test. + navigator.getGamepads = () => snapshots.shift() ?? []; + try { + const wheel = new Wheel(); + expect(wheel.poll()).toBeDefined(); + expect(wheel.id).toBe("Wheel A"); + expect(wheel.poll()).toBeUndefined(); + expect(wheel.id).toBeUndefined(); + const adopted = wheel.poll(); + expect(wheel.id).toBe("Wheel B"); + expect(adopted?.moved).toBe(false); + } finally { + navigator.getGamepads = original; + } + }); +}); + +describe("shortName", () => { + test("strips the vendor/product noise browsers append", () => { + expect(shortName("Logitech G29 (Vendor: 046d Product: c24f)")).toBe("Logitech G29"); + expect(shortName("Plain Wheel")).toBe("Plain Wheel"); + }); +}); diff --git a/demo/carla/web/src/wheel.ts b/demo/carla/web/src/wheel.ts new file mode 100644 index 00000000000..53d5ee64074 --- /dev/null +++ b/demo/carla/web/src/wheel.ts @@ -0,0 +1,644 @@ +/** + * Steering wheel + pedal support via the Gamepad API. + * + * A wheel is fundamentally different from the keyboard mapping in + * {@link module:input}: the keys are *hold*-style nudges that integrate over + * time, while a wheel and its pedals are **absolute** - the rim angle *is* the + * steering angle and a pedal's travel *is* the throttle. So this module maps + * positions straight through rather than integrating them, and the only + * time-domain shaping is a generous rate limit ({@link WHEEL_RATES}) that a + * human hand can't hit in normal driving. It exists to smooth two things: a + * violent yank of the rim, and the step that happens the instant the wheel + * takes over from the keyboard. + * + * Browsers report wheels with no consistent axis order, polarity, or range - + * it varies by model, driver, and OS. So there is a default layout that fits + * the common Logitech/Thrustmaster arrangement, plus a calibration pass that + * learns the real layout by watching the driver move each control. Learned + * profiles are stored per device id in `localStorage`. + * + * @module + */ + +import { z } from "zod/mini"; +import { moveToward } from "./input.ts"; +import type { DriveState } from "./schemas.ts"; + +/** How one physical axis maps onto a normalized control. */ +export const AxisMapSchema = z.object({ + /** Index into `Gamepad.axes`. */ + axis: z.number(), + /** Raw value at rest: wheel centered, pedal up. */ + rest: z.number(), + /** Raw value at full travel: full right lock, pedal floored. */ + full: z.number(), +}); +export type AxisMap = z.infer; + +/** A complete device layout: which axis/button drives which control. */ +export const WheelProfileSchema = z.object({ + steer: AxisMapSchema, + throttle: AxisMapSchema, + brake: AxisMapSchema, + /** `Gamepad.buttons` indices; omitted when the driver skipped the binding. */ + gearUp: z.optional(z.number()), + gearDown: z.optional(z.number()), + estop: z.optional(z.number()), +}); +export type WheelProfile = z.infer; + +/** + * The layout most direct-input wheels report before calibration: rim on axis 0 + * centered at zero, pedals on axes 1 and 2 resting at +1 and reading -1 when + * floored, shift paddles on buttons 4 and 5. This is a starting guess, not a + * promise - {@link Calibration} replaces it per device. + */ +export const DEFAULT_PROFILE: WheelProfile = { + steer: { axis: 0, rest: 0, full: 1 }, + throttle: { axis: 1, rest: 1, full: -1 }, + brake: { axis: 2, rest: 1, full: -1 }, + gearUp: 4, + gearDown: 5, +}; + +/** + * Rate limits in normalized units per second, deliberately much faster than + * {@link module:input.RATES}: the wheel is absolute, so clamping it hard would + * desync the rim from the commanded angle - the driver's hands would say one + * thing and the truck another. These only bite on a yank or a source handoff. + */ +export const WHEEL_RATES = { + /** Full lock to full lock in ~1.3 s. */ + steer: 1.5, + /** A floored pedal reaches full requested throttle in ~0.67 s. */ + throttle: 1.5, + /** Brake application stays fast. */ + brake: 3, +}; + +/** Ignore rim wobble this close to center, and rescale so full lock still reaches 1. */ +export const STEER_DEADZONE = 0.03; +/** Pedal travel below this reads as fully released... */ +export const PEDAL_DEADZONE = 0.05; +/** ...and above this as fully applied, so worn pedals can still reach the stops. */ +export const PEDAL_SATURATION = 0.95; + +/** Movement (in normalized units) that counts as the driver touching the wheel. */ +export const MOVE_EPSILON = 0.02; + +/** + * The wheel refuses to take over from the keyboard while the throttle is + * depressed, so a pedal resting under someone's foot can never be what hands + * control to the wheel. Applies to the handoff only; once armed, the pedal is + * honored normally. + */ +export const ARM_MAX_THROTTLE = 0.05; + +/** A control that can be bound to a wheel button. */ +export type WheelButton = "gearUp" | "gearDown" | "estop"; + +/** One poll of the wheel, already normalized into drive units. */ +export interface WheelSample { + /** -1 (full left) .. 1 (full right). */ + steering: number; + /** 0 .. 1. */ + throttle: number; + /** 0 .. 1. */ + brake: number; + /** Buttons that went down on *this* poll, as rising edges. */ + pressed: ReadonlySet; + /** Whether any axis moved perceptibly since the previous poll. */ + moved: boolean; +} + +/** The subset of `Gamepad` this module reads, so tests can pass plain objects. */ +export interface PadLike { + id: string; + index: number; + axes: readonly number[]; + buttons: readonly { pressed: boolean }[]; + mapping?: string; + connected?: boolean; +} + +function clamp(v: number, lo: number, hi: number): number { + return Math.min(hi, Math.max(lo, v)); +} + +/** Normalize a raw axis into 0..1 of its calibrated travel (may over/undershoot). */ +export function readAxis(axes: readonly number[], map: AxisMap): number { + const raw = axes[map.axis]; + const span = map.full - map.rest; + if (raw === undefined || Math.abs(span) < 1e-6) return 0; + return (raw - map.rest) / span; +} + +/** Read the rim as signed steering, with a rescaled center deadzone. */ +export function readSteer(axes: readonly number[], map: AxisMap): number { + const v = clamp(readAxis(axes, map), -1, 1); + const mag = Math.abs(v); + if (mag <= STEER_DEADZONE) return 0; + return Math.sign(v) * ((mag - STEER_DEADZONE) / (1 - STEER_DEADZONE)); +} + +/** Read a pedal as 0..1, with rescaled deadzone and saturation at the stops. */ +export function readPedal(axes: readonly number[], map: AxisMap): number { + const v = clamp(readAxis(axes, map), 0, 1); + if (v <= PEDAL_DEADZONE) return 0; + if (v >= PEDAL_SATURATION) return 1; + return (v - PEDAL_DEADZONE) / (PEDAL_SATURATION - PEDAL_DEADZONE); +} + +/** + * Move the drive state toward the wheel's absolute position over `dt` seconds. + * + * Mirrors {@link module:input.applyKeys} in shape so the caller can swap input + * sources, but the semantics are the opposite: the sample is a *target*, not a + * nudge, and the rate limit is only a safety ceiling on how fast it is chased. + */ +export function applyWheel( + current: DriveState, + sample: Pick, + dt: number, +): Pick { + return { + steering: moveToward(current.steering, clamp(sample.steering, -1, 1), WHEEL_RATES.steer * dt), + throttle: moveToward(current.throttle, clamp(sample.throttle, 0, 1), WHEEL_RATES.throttle * dt), + brake: moveToward(current.brake, clamp(sample.brake, 0, 1), WHEEL_RATES.brake * dt), + }; +} + +/** Which input source currently owns the drive axes. */ +export type InputSource = "keyboard" | "wheel"; + +/** The outcome of one arbitration tick. */ +export interface Arbitration { + source: InputSource; + /** + * True on the single tick an armed wheel disappeared, so the caller can fail + * safe exactly once rather than pinning the axes every frame afterwards. + */ + lost: boolean; +} + +/** + * Decide which source owns the axes this tick. + * + * The keyboard holds them by default. A wheel takes over only once it moves + * *and* its throttle is released, so a pedal resting under a foot can never be + * what hands it control. Handing control back to the keyboard is the caller's + * job (any drive key does it), since the wheel can't observe that. + */ +export function arbitrate(current: InputSource, sample: WheelSample | undefined): Arbitration { + if (!sample) return { source: "keyboard", lost: current === "wheel" }; + if (current !== "wheel" && sample.moved && sample.throttle <= ARM_MAX_THROTTLE) { + return { source: "wheel", lost: false }; + } + return { source: current, lost: false }; +} + +/** + * The command to fall back to when an armed wheel vanishes mid-drive. + * + * Losing the wheel is not the same as releasing the keys. Held keys are a + * steady state the driver chose and can still change; a wheel that has + * unplugged has taken the control surface that was setting throttle with it, so + * holding its last command is the runaway case. Cut the throttle and brake. + * + * Steering deliberately *holds* rather than centering the way e-stop does: the + * driver has lost their input device, and trading uncommanded throttle for an + * uncommanded swerve mid-corner is not an improvement. + */ +export function failSafe(current: DriveState): Pick { + return { steering: current.steering, throttle: 0, brake: 1 }; +} + +/** Device-id fragments that identify a wheel outright, lowercased. */ +const WHEEL_HINTS = [ + "wheel", + "racing", + "driving force", + "g25", + "g27", + "g29", + "g920", + "g923", + "g pro", + "logitech momo", + "thrustmaster", + "t150", + "t248", + "t300", + "t500", + "tmx", + "fanatec", + "clubsport", + "csl elite", + "simucube", + "moza", +]; + +/** + * Whether a pad is plausibly a wheel. + * + * Wheels rarely fit the browser's "standard" gamepad mapping, so anything + * unmapped with a rim plus two pedals' worth of axes qualifies even when the id + * is unfamiliar. A false positive here costs a calibration pass, not safety - + * the wheel still has to move before it takes control of anything. + */ +export function looksLikeWheel(pad: PadLike): boolean { + const id = pad.id.toLowerCase(); + if (WHEEL_HINTS.some((hint) => id.includes(hint))) return true; + return pad.mapping !== "standard" && pad.axes.length >= 3; +} + +/** Read the connected pads, tolerating browsers without the Gamepad API. */ +function scanPads(): PadLike[] { + if (typeof navigator === "undefined" || !navigator.getGamepads) return []; + const pads: PadLike[] = []; + for (const pad of navigator.getGamepads()) { + if (pad?.connected) pads.push(pad); + } + return pads; +} + +function storage(): Storage | undefined { + // Private browsing modes can throw on access rather than return null. + try { + return window.localStorage; + } catch { + return undefined; + } +} + +const STORAGE_PREFIX = "carla.wheel:"; + +/** + * Tracks the connected wheel, its calibration, and its per-frame readings. + * + * Polling (rather than listening for `gamepadconnected`) keeps this in step + * with the driving loop and handles the browser's requirement that a pad only + * becomes visible after the user actually moves it. + */ +export class Wheel { + #index: number | undefined; + #id: string | undefined; + #profile: WheelProfile = DEFAULT_PROFILE; + #calibrated = false; + #previous: { steering: number; throttle: number; brake: number } | undefined; + #buttons: boolean[] = []; + + /** The connected wheel's device id, or undefined when none is present. */ + get id(): string | undefined { + return this.#id; + } + + /** Whether the active profile was learned from this device rather than guessed. */ + get calibrated(): boolean { + return this.#calibrated; + } + + /** The live pad, for the calibration UI. */ + pad(): PadLike | undefined { + if (this.#index === undefined) return undefined; + return scanPads().find((pad) => pad.index === this.#index && pad.id === this.#id); + } + + /** + * Read the wheel's current position, or undefined when none is connected. + * + * Call once per frame: the rising-edge button set and the `moved` flag are + * both differences against the previous call. + */ + poll(): WheelSample | undefined { + const pads = scanPads(); + let pad = pads.find((candidate) => candidate.index === this.#index && candidate.id === this.#id); + if (!pad && this.#index !== undefined) { + // Report the armed device as lost for one full arbitration tick before + // adopting a replacement. Otherwise another connected wheel can inherit + // control (and its current pedal positions) without independently arming. + this.#forget(); + return undefined; + } + pad ??= pads.find(looksLikeWheel); + if (!pad) return undefined; + if (this.#index === undefined) this.#adopt(pad); + + const steering = readSteer(pad.axes, this.#profile.steer); + const throttle = readPedal(pad.axes, this.#profile.throttle); + const brake = readPedal(pad.axes, this.#profile.brake); + + // The first poll of a device only establishes the baseline: merely + // plugging a wheel in must not be what takes control away from the + // keyboard, so `moved` stays false until there is something to compare. + const previous = this.#previous; + const moved = + previous !== undefined && + (Math.abs(previous.steering - steering) > MOVE_EPSILON || + Math.abs(previous.throttle - throttle) > MOVE_EPSILON || + Math.abs(previous.brake - brake) > MOVE_EPSILON); + this.#previous = { steering, throttle, brake }; + + const pressed = new Set(); + for (const [control, index] of this.#bindings()) { + const down = pad.buttons[index]?.pressed ?? false; + if (down && !this.#buttons[index]) pressed.add(control); + } + this.#buttons = pad.buttons.map((button) => button.pressed); + + return { steering, throttle, brake, pressed, moved }; + } + + /** Install a freshly calibrated profile and persist it for this device. */ + save(profile: WheelProfile) { + this.#profile = profile; + this.#calibrated = true; + // The old baseline was computed through the old profile, so comparing + // against it would report a large synthetic "movement" on the next poll + // and arm the wheel without the driver having touched it. + this.#previous = undefined; + if (!this.#id) return; + try { + storage()?.setItem(STORAGE_PREFIX + this.#id, JSON.stringify(profile)); + } catch { + // A full or unavailable store just costs the driver a recalibration. + } + } + + *#bindings(): Generator<[WheelButton, number]> { + const { gearUp, gearDown, estop } = this.#profile; + if (gearUp !== undefined) yield ["gearUp", gearUp]; + if (gearDown !== undefined) yield ["gearDown", gearDown]; + if (estop !== undefined) yield ["estop", estop]; + } + + #adopt(pad: PadLike) { + this.#index = pad.index; + this.#id = pad.id; + this.#previous = undefined; + // Baseline the buttons from the device itself rather than from nothing: + // a paddle already held when the wheel is adopted is a held button, not + // a press, and must not read as a rising edge on the first poll. + this.#buttons = pad.buttons.map((button) => button.pressed); + + const stored = loadProfile(pad.id); + this.#profile = stored ?? DEFAULT_PROFILE; + this.#calibrated = stored !== undefined; + } + + #forget() { + this.#index = undefined; + this.#id = undefined; + this.#profile = DEFAULT_PROFILE; + this.#calibrated = false; + this.#previous = undefined; + this.#buttons = []; + } +} + +function loadProfile(id: string): WheelProfile | undefined { + let raw: string | null | undefined; + try { + raw = storage()?.getItem(STORAGE_PREFIX + id); + } catch { + return undefined; + } + if (!raw) return undefined; + try { + const parsed = WheelProfileSchema.safeParse(JSON.parse(raw)); + return parsed.success ? parsed.data : undefined; + } catch { + // Corrupt JSON; the driver can recalibrate. + return undefined; + } +} + +/** Minimum axis travel that counts as "the driver moved this control". */ +export const CAPTURE_THRESHOLD = 0.25; + +/** + * Pick the axis that moved furthest from its resting value. + * + * Wheels cross-talk - turning the rim wiggles the pedal axes a little - so the + * winner is the largest deviation rather than the first one over the line, and + * axes already bound to another control are excluded. + */ +export function pickAxis( + baseline: readonly number[], + current: readonly number[], + exclude: ReadonlySet = new Set(), +): number | undefined { + let best: number | undefined; + let bestDelta = CAPTURE_THRESHOLD; + for (let axis = 0; axis < current.length; axis++) { + if (exclude.has(axis)) continue; + const delta = Math.abs((current[axis] ?? 0) - (baseline[axis] ?? 0)); + if (delta > bestDelta) { + best = axis; + bestDelta = delta; + } + } + return best; +} + +/** Pick the first button that went down since the baseline snapshot. */ +export function pickButton( + baseline: readonly boolean[], + current: readonly boolean[], + exclude: ReadonlySet = new Set(), +): number | undefined { + for (let index = 0; index < current.length; index++) { + if (exclude.has(index)) continue; + if (current[index] && !baseline[index]) return index; + } + return undefined; +} + +/** One prompt in the calibration sequence. */ +export interface CalibrationStep { + key: "rest" | "steer" | "throttle" | "brake" | WheelButton; + kind: "rest" | "axis" | "button"; + prompt: string; + /** Whether the driver may move past this step without binding anything. */ + optional: boolean; +} + +export const CALIBRATION_STEPS: readonly CalibrationStep[] = [ + { + key: "rest", + kind: "rest", + prompt: "Center the wheel and take your feet off the pedals, then press Capture.", + optional: false, + }, + { + key: "steer", + kind: "axis", + prompt: "Hold the wheel at full RIGHT lock, then press Capture.", + optional: false, + }, + { + key: "throttle", + kind: "axis", + prompt: "Press the THROTTLE pedal all the way down and hold it, then press Capture.", + optional: false, + }, + { + key: "brake", + kind: "axis", + prompt: "Press the BRAKE pedal all the way down and hold it, then press Capture.", + optional: false, + }, + { + key: "gearUp", + kind: "button", + prompt: "Press the paddle or button for SHIFT UP.", + optional: true, + }, + { + key: "gearDown", + kind: "button", + prompt: "Press the paddle or button for SHIFT DOWN.", + optional: true, + }, + { + key: "estop", + kind: "button", + prompt: "Press the button you want to use for E-STOP.", + optional: true, + }, +]; + +/** + * The calibration walk: snapshot the device at rest, then learn one control per + * step from how far it moves away from that snapshot. + * + * Kept free of DOM so the sequencing is testable; the UI feeds it {@link update} + * once per frame and calls {@link capture} / {@link skip} from its buttons. + */ +export class Calibration { + #step = 0; + #axes: number[] = []; + #buttons: boolean[] = []; + #restAxes: number[] = []; + /** Buttons held when the current step began, so a held paddle can't self-bind. */ + #stepButtons: boolean[] = []; + #draft: Partial = {}; + + /** The step awaiting input, or undefined once calibration is complete. */ + get step(): CalibrationStep | undefined { + return CALIBRATION_STEPS[this.#step]; + } + + get index(): number { + return this.#step; + } + + get total(): number { + return CALIBRATION_STEPS.length; + } + + get done(): boolean { + return this.#step >= CALIBRATION_STEPS.length; + } + + /** Feed the live pad state; call once per frame while calibrating. */ + update(pad: PadLike) { + this.#axes = [...pad.axes]; + this.#buttons = pad.buttons.map((button) => button.pressed); + + // Button steps bind on a rising edge - there is nothing to press Capture + // with when both hands are on a paddle. + const step = this.step; + if (step?.kind === "button") { + const index = pickButton(this.#stepButtons, this.#buttons, this.#usedButtons()); + if (index !== undefined) { + this.#draft[step.key as WheelButton] = index; + this.#advance(); + } + } + } + + /** + * Commit the current step from the latest {@link update}. + * + * Returns false when the control didn't move far enough to identify - the UI + * leaves the step up so the driver can push the pedal harder and retry. + */ + capture(): boolean { + const step = this.step; + if (!step) return false; + + if (step.kind === "rest") { + this.#restAxes = [...this.#axes]; + this.#advance(); + return true; + } + + if (step.kind === "axis") { + const axis = pickAxis(this.#restAxes, this.#axes, this.#usedAxes()); + if (axis === undefined) return false; + const map: AxisMap = { + axis, + rest: this.#restAxes[axis] ?? 0, + full: this.#axes[axis] ?? 0, + }; + this.#draft[step.key as "steer" | "throttle" | "brake"] = map; + this.#advance(); + return true; + } + + // Button steps bind from update(); Capture just moves on. + return this.skip(); + } + + /** Leave the current step unbound, if it is allowed to be. */ + skip(): boolean { + const step = this.step; + if (!step?.optional) return false; + this.#advance(); + return true; + } + + /** The finished profile, or undefined if the walk isn't complete. */ + result(): WheelProfile | undefined { + const { steer, throttle, brake } = this.#draft; + if (!this.done || !steer || !throttle || !brake) return undefined; + return { + steer, + throttle, + brake, + gearUp: this.#draft.gearUp, + gearDown: this.#draft.gearDown, + estop: this.#draft.estop, + }; + } + + #advance() { + this.#step++; + // Re-baseline held buttons so the press that ended one step doesn't also + // satisfy the next one. + this.#stepButtons = [...this.#buttons]; + } + + #usedAxes(): Set { + const used = new Set(); + for (const map of [this.#draft.steer, this.#draft.throttle, this.#draft.brake]) { + if (map) used.add(map.axis); + } + return used; + } + + #usedButtons(): Set { + const used = new Set(); + for (const index of [this.#draft.gearUp, this.#draft.gearDown, this.#draft.estop]) { + if (index !== undefined) used.add(index); + } + return used; + } +} + +/** A short human-readable device name, since ids carry vendor/product noise. */ +export function shortName(id: string): string { + const trimmed = id.replace(/\s*\((?:Vendor|STANDARD|Product).*$/i, "").trim(); + return trimmed.length > 0 ? trimmed : id; +} diff --git a/demo/carla/web/test/worklet.ts b/demo/carla/web/test/worklet.ts new file mode 100644 index 00000000000..106378820cd --- /dev/null +++ b/demo/carla/web/test/worklet.ts @@ -0,0 +1,6 @@ +import { mock } from "bun:test"; + +// Bun cannot load the blob-URL worklet import used by the audio decoder, and these tests never create an AudioContext. +mock.module("../../../../js/watch/src/audio/render-worklet.ts?worklet", () => ({ + default: async () => "blob:unused-worklet", +})); diff --git a/demo/carla/web/tsconfig.json b/demo/carla/web/tsconfig.json new file mode 100644 index 00000000000..fc5e2c491fa --- /dev/null +++ b/demo/carla/web/tsconfig.json @@ -0,0 +1,11 @@ +{ + "extends": "../../../js/tsconfig.json", + "compilerOptions": { + "module": "esnext", + "moduleResolution": "bundler", + "noEmit": true, + "allowImportingTsExtensions": true, + "types": ["bun"] + }, + "include": ["src", "test", "vite.config.ts", "../../../js/common/worklet.d.ts"] +} diff --git a/demo/carla/web/vite.config.ts b/demo/carla/web/vite.config.ts new file mode 100644 index 00000000000..725e402f28a --- /dev/null +++ b/demo/carla/web/vite.config.ts @@ -0,0 +1,8 @@ +import { defineConfig } from "vite"; +import { worklet } from "../../../js/common/vite-plugin-worklet.ts"; + +export default defineConfig({ + plugins: [worklet()], + build: { target: "esnext" }, + server: { hmr: false }, +}); diff --git a/demo/carla/web/wrangler.jsonc b/demo/carla/web/wrangler.jsonc new file mode 100644 index 00000000000..2328f2223c7 --- /dev/null +++ b/demo/carla/web/wrangler.jsonc @@ -0,0 +1,17 @@ +{ + // Cloudflare Wrangler configuration for the CARLA player (carla.moq.dev) + "$schema": "node_modules/wrangler/config-schema.json", + "name": "carla-moq-dev", + "compatibility_date": "2025-07-09", + "account_id": "dd618f5dbd5da77b8296f1613c301f5c", + "workers_dev": false, + "route": { + "pattern": "carla.moq.dev", + "custom_domain": true + }, + // Static assets only: the player is a Vite bundle, and unknown paths serve the page. + "assets": { + "directory": "./dist", + "not_found_handling": "single-page-application" + } +} diff --git a/doc/bin/demo.md b/doc/bin/demo.md index d1471186465..3e1b8d9436d 100644 --- a/doc/bin/demo.md +++ b/doc/bin/demo.md @@ -1,11 +1,12 @@ --- title: Demos -description: The web demo and MoQ Boy +description: The web demo, MoQ Boy, and CARLA --- # Demos -Both run locally from a checkout and live at [moq.dev](https://moq.dev). +Each runs locally from a checkout. The web demo and MoQ Boy also live at +[moq.dev](https://moq.dev); CARLA's simulator needs a Linux host with an NVIDIA GPU. ## Web demo @@ -48,3 +49,24 @@ and publisher), [`js/moq-boy`](https://github.com/moq-dev/moq/tree/main/js/moq-b (the `` element, published as `@moq/boy`), and [`demo/boy`](https://github.com/moq-dev/moq/tree/main/demo/boy). The same shape covers teleoperation: swap the emulator for a robot. + +## CARLA + +Teleoperation over MoQ: simulated trucks drive themselves through one +[CARLA](https://carla.org) world until you take one over from the browser. +The simulator needs a Linux host with an NVIDIA GPU and an image built by +[moq-dev/carla](https://github.com/moq-dev/carla); the player runs anywhere. + +```bash +just carla venv # once: the worker +CARLA_IMAGE= just carla server # leave the simulator running +just carla # relay + trucks + player +``` + +What it demonstrates: + +- **GPU to wire.** Camera images never leave the GPU: `moq-video` encodes them on the device that rendered them, and a camera renders only while someone subscribes. +- **Control with a lease.** A driver publishes a JSON `control` track under `viewer/carla/{truck}/`; the truck accepts one driver at a time, bound to the epoch it announced with, and returns to autonomy when the lease lapses. +- **Measured feedback.** Each truck's `status` track carries what the gate accepted and what the vehicle did, so the player draws requested against measured steering. + +Code: [`demo/carla`](https://github.com/moq-dev/moq/tree/main/demo/carla). diff --git a/justfile b/justfile index 2bd49054043..fc1402a16a1 100644 --- a/justfile +++ b/justfile @@ -29,6 +29,7 @@ mod drafts mod gh '.github' # Shortcuts to avoid `demo::` prefix. mod boy 'demo/boy' +mod carla 'demo/carla' mod pub 'demo/pub' mod relay 'demo/relay' mod sub 'demo/sub' diff --git a/package.json b/package.json index 32ea2c31bbf..af389ad94bb 100644 --- a/package.json +++ b/package.json @@ -45,6 +45,7 @@ "js/wasm", "demo/boy", "demo/web", + "demo/carla/web", "js/moq-boy", "test/audio-quality/clients/js", "test/interop/clients/js", diff --git a/quest/m2/README.md b/quest/m2/README.md index b74ef2f5517..879a3c43f79 100644 --- a/quest/m2/README.md +++ b/quest/m2/README.md @@ -51,6 +51,7 @@ ranked just below deterministic TS legs, which it requires. - [AudioToolbox encode](/quest/m2/audio-encode-audiotoolbox.md) - macOS and iOS encode AAC-LC - [CMAF surround Opus](/quest/m2/cmaf-opus-surround.md) - a released mp4-atom carries the `dOps` channel mapping table, and fMP4 import and export carry it through the OpusHead - [GPU CI](/quest/m2/gpu-ci.md) - NVIDIA tests run nightly on real hardware, and `just rs nvidia` runs them locally instead of skipping +- [Vendor-free CARLA bridge](/quest/m2/carla-bridge.md) - the CARLA demo's GPU bridge lets moq-video pick the encoder, and its stand-in test runs on AMD as well as NVIDIA - [V4L2 virtual camera](/quest/m2/v4l2-virtual-camera.md) - V4L2 capture runs nightly against the `vivid` virtual driver instead of only on a developer's camera - [Probe](/quest/m2/probe/README.md) - a session probes up to the bitrate it wants to send or receive, so a stopped simulcast rendition resumes and a viewer up-switches - [A finished PROBE stops reporting](/quest/m2/probe-end-stale.md) - the Rust receive estimate reads unknown once its PROBE stream ends diff --git a/quest/m2/carla-bridge.md b/quest/m2/carla-bridge.md new file mode 100644 index 00000000000..65e0231f49b --- /dev/null +++ b/quest/m2/carla-bridge.md @@ -0,0 +1,61 @@ +# [M] A vendor-free CARLA bridge + +## Goal + +`demo/carla/bridge` holds no vendor code. It hands moq-video one +`Surface::Vulkan` per capture with `Kind::Auto`, and moq-video picks the +import and encoder: NVENC on NVIDIA, Vulkan Video on AMD. The bridge names no +backend and no vendor ID. It encodes H.264 or H.265, and the worker publishes +whichever it is. `just carla bridge-gpu` runs the stand-in test on an AMD and +an NVIDIA host. + +This is the end-to-end test that moq-video's API carries a GPU frame from an +external producer to any vendor's encoder without a CPU round trip. CARLA +itself stays on NVIDIA; the stand-in proves the bridge elsewhere. + +## Plan + +moq-video owns the surface, the backend selection, the Vulkan Video backend, +and its hardware tests. This quest only removes what the bridge still pins. + +Already done: the bridge passes the plugin's device and driver UUID as +`encode::Config::input`, imports each slot with the exporter's +`memoryTypeIndex` and allocation size, recycles slots on moq-video's +completion, and takes keyframes from `Encoded::keyframe`. Its parameter-set +probe opens the camera's own backend with a CPU picture. + +- Selection: switch `stream.rs` `config` from `Kind::Named("nvenc")` to + `Kind::Auto`. A device no backend imports must be refused, never sent to + openh264; the workspace unifies `openh264` on, so check that refusal in a + test. +- Probe: a CPU picture only works on a backend that also takes CPU input. + Probe with the camera's surface type, or take parameter sets from the first + keyframe instead, whichever backend picks. +- Codec (H.264 and H.265): the probe's SPS/PPS filter becomes codec-generic. + - The worker names each rendition's codec in its request, and + `Event::Config` echoes it. + - Both sides reject trailing bytes and the worker requires an exact hello + `VERSION`, so defaults cannot make this compatible. Recommended: bump + `VERSION`, so a mismatched bridge and worker fail at hello naming both + versions; test the refusal both ways. Open, if mixed versions must run: + negotiate at hello and keep the v1 layout for H.264. + - `sim/moq_carla_sim/bridge.py` publishes `AVC3` or `HEV1` instead of + hardcoding `AVC3`. +- Scaling: one render still feeds the 426x240 and 1280x720 renditions, + scaled on the GPU. GPU-side copies are allowed; a CPU round trip is not. +- Test: the stand-in (`tests/standin/mod.rs`) stops requiring vendor + `0x10de` and renders on the GPU `gpu.sh` found, by PCI vendor. + - `bridge/gpu.sh` gains an AMD branch that runs the test with the host's + RADV ICD visible to the Nix Vulkan loader. + - An Intel-only host still skips: the plugin exports `OPAQUE_FD`, which + VA-API cannot take. + - Run H.264 and H.265 on AMD, and the NVIDIA branch on a CARLA host. +- Docs: `demo/carla/README.md` and the crate description stop saying NVENC. + +## Required + +- [Vulkan Video encode on AMD](/quest/m2/vulkan-encode.md) - the AMD encoder this exercises + +## Related + +- [GPU CI](/quest/m2/gpu-ci.md) - runs the NVIDIA half nightly diff --git a/sh/dispatch.sh b/sh/dispatch.sh index 6b01fa521b6..2d63d1292c5 100755 --- a/sh/dispatch.sh +++ b/sh/dispatch.sh @@ -72,13 +72,16 @@ declare -A scope=( # The JS tests also read Rust's shared JSON vectors; sh/js/workspace.py # fails a check that reads a cross-tree input this pattern misses. # demo/pub is a Worker outside the Bun workspace, listed so Biome lints it. - [js]='^(js/|bench/browser/|doc/|drafts/|demo/(boy|pub|web)/|test/|rs/moq-wasm/|rs/.*\.json$|sh/js/|sh/rs/stats-docs\.py$|package\.json$|bun\.lock(b)?$|biome\.jsonc$)' + [js]='^(js/|bench/browser/|doc/|drafts/|demo/(boy|pub|web|carla/web)/|test/|rs/moq-wasm/|rs/.*\.json$|sh/js/|sh/rs/stats-docs\.py$|package\.json$|bun\.lock(b)?$|biome\.jsonc$)' # Workers with lockfiles outside the Bun workspace. [workers]='^(infra/apt/|infra/rpm/|infra/moq-sh/|demo/pub/|sh/js/workers\.sh$)' [moq_sh]='^infra/moq-sh/' # sh/rs/select.sh widens to the whole workspace for inputs every crate shares. - [rs]='^(rs/|sh/rs/|Cargo\.(toml|lock)$|rust-toolchain\.toml$|\.config/nextest\.toml$)' + [rs]='^(rs/|demo/carla/bridge/|sh/rs/|Cargo\.(toml|lock)$|rust-toolchain\.toml$|\.config/nextest\.toml$)' [bench]='^bench/' + # The CARLA demo's simulator worker and launchers. Its player is a JS + # package and its GPU bridge a Rust crate, so those modules cover them. + [carla]='^demo/carla/(sim/|justfile$)' [drafts]='^(drafts/|sh/drafts/|doc/\.vitepress/drafts\.ts$)' # Quest documents form one graph, so any change validates the whole tree, # as does a bump of the flake-pinned validator. @@ -149,6 +152,7 @@ declare -A tools=( [moq_sh]='dash' [rs]='cargo jq' [bench]='cargo' + [carla]='uv' [drafts]='bun kramdown-rfc xml2rfc' [quest]='quest' [drill]='' @@ -184,8 +188,8 @@ declare -A compiles=( ) case "$action" in - check | ci-check) modules=(js workers moq_sh drafts rs bench quest drill py kt swift go dart cpp obs_compile obs flake ts_paths markdown shell toml nix justfile gh) ;; - fix) modules=(js rs py dart obs markdown shell toml nix justfile) ;; + check | ci-check) modules=(js workers moq_sh drafts rs bench carla quest drill py kt swift go dart cpp obs_compile obs flake ts_paths markdown shell toml nix justfile gh) ;; + fix) modules=(js rs py carla dart obs markdown shell toml nix justfile) ;; ci-test) modules=(js rs py) ;; esac diff --git a/sh/rs/select.sh b/sh/rs/select.sh index 868d427ec06..795e6eacce7 100755 --- a/sh/rs/select.sh +++ b/sh/rs/select.sh @@ -25,7 +25,7 @@ select_packages() { return fi - grep -q '^rs/' "$list" || return 0 + grep -qE '^(rs|demo)/' "$list" || return 0 # `--no-deps` keeps this to workspace members and off the network. local metadata @@ -34,20 +34,27 @@ select_packages() { # A seed is the first path segment under `rs/`, so only a crate whose # manifest sits directly at `rs//Cargo.toml` can be one. That drops # the `*-fuzz` crates (at `rs//fuzz`), which a diff to their crate would - # otherwise select as a dependent and compile libFuzzer for. The separator - # is normalized first so Windows paths split too. - metadata=$(jq '.packages |= map((.manifest_path |= gsub("\\\\"; "/")) | select(.manifest_path | split("/")[-3] == "rs"))' <<<"$metadata") - - # A changed path is a seed only because every crate directory is named - # after its crate. Under-selecting checks nothing, so fall back to all. - if jq -e '.packages[] | select((.manifest_path | split("/")[-2]) != .name)' <<<"$metadata" >/dev/null; then + # otherwise select as a dependent and compile libFuzzer for. A demo's crate + # sits at `demo///Cargo.toml` and is a seed for its directory. + # The separator is normalized first so Windows paths split too. + metadata=$(jq '.packages |= map((.manifest_path |= gsub("\\\\"; "/")) | select((.manifest_path | split("/")) as $p | $p[-3] == "rs" or $p[-4] == "demo"))' <<<"$metadata") + + # A changed path under `rs/` is a seed only because every crate directory + # there is named after its crate. Under-selecting checks nothing, so fall + # back to all. + if jq -e '.packages[] | (.manifest_path | split("/")) as $p | select($p[-3] == "rs" and $p[-2] != .name)' <<<"$metadata" >/dev/null; then echo "rs: a crate directory no longer matches its crate name; selecting everything." >&2 echo ALL return fi local seeds edges selected - seeds=$(sed -n 's|^rs/\([^/]*\)/.*|\1|p' "$list" | sort -u) + seeds=$({ + sed -n 's|^rs/\([^/]*\)/.*|\1|p' "$list" + jq -r --rawfile list "$list" '($list | split("\n")) as $paths | .packages[] + | (.manifest_path | split("/")) as $p | select($p[-4] == "demo") + | select(any($paths[]; startswith("demo/\($p[-3])/\($p[-2])/"))) | .name' <<<"$metadata" + } | sort -u) edges=$(jq -r '.packages[] | .name as $name | .dependencies[] | "\($name) \(.name)"' <<<"$metadata") # A crate is rebuilt when anything it depends on changed, so walk the edges diff --git a/sh/rs/select.test.py b/sh/rs/select.test.py index b380c7c3f52..d8e721e3526 100644 --- a/sh/rs/select.test.py +++ b/sh/rs/select.test.py @@ -25,7 +25,7 @@ def selected(self, *paths, action="lint"): SELECT_CALLS=str(calls), BASH_ENV=os.devnull) subprocess.run(["bash", "sh/rs/select.sh", action, str(changed)], cwd=ROOT, env=env, check=True, capture_output=True, text=True) - return calls.read_text().splitlines() + return calls.read_text().splitlines() if calls.exists() else [] def test_nextest_launcher_selects_workspace(self): for action in ("check", "check-test", "lint", "test"): @@ -58,6 +58,13 @@ def test_feature_script_diff_checks_its_own_modes(self): self.assertEqual(self.selected("sh/rs/net-features.sh", action=action), ["rs net-features"]) + def test_demo_crate_follows_its_dependencies(self): + for path in ("demo/carla/bridge/src/lib.rs", "rs/moq-video/src/lib.rs"): + with self.subTest(path=path): + calls = self.selected(path, action="test") + self.assertTrue(any("/demo/carla/bridge#moq-carla-bridge" in call for call in calls), calls) + self.assertEqual(self.selected("demo/carla/web/src/main.ts"), []) + def test_shared_build_input_gets_all_permutations(self): calls = self.selected("Cargo.lock") self.assertIn("rs tokio-features --all", calls)