243 lines
12 KiB
Markdown
243 lines
12 KiB
Markdown
# LiDAR / Voxel Map — Unitree Go2
|
||
|
||
The control view renders the Go2's LiDAR output as a live 3D voxel map (SLAM). Compressed voxel packets arrive over WebRTC, are decoded off the UI thread by a WASM kernel, and displayed as a triangle mesh in Three.js alongside the 3D robot model.
|
||
|
||
## Table of Contents
|
||
|
||
- [Topics](#topics)
|
||
- [Enable / Disable](#enable--disable)
|
||
- [Data Flow](#data-flow)
|
||
- [Compressed Packet Format](#compressed-packet-format)
|
||
- [WASM Decoder (`libvoxel.wasm`)](#wasm-decoder-libvoxelwasm)
|
||
- [Rendering Pipeline](#rendering-pipeline)
|
||
- [Robot Radar Animation](#robot-radar-animation)
|
||
- [Performance](#performance)
|
||
- [Files](#files)
|
||
|
||
---
|
||
|
||
## Topics
|
||
|
||
All LiDAR traffic travels on the WebRTC DataChannel under the `rt/utlidar/*` namespace.
|
||
|
||
| Topic | Direction | Purpose |
|
||
|---|---|---|
|
||
| `rt/utlidar/switch` | client → robot | `'ON'` / `'OFF'` string toggles the LiDAR stream |
|
||
| `rt/utlidar/voxel_map_compressed` | robot → client | Compressed voxel map frames (ArrayBuffer) |
|
||
| `rt/utlidar/lidar_state` | robot → client | LiDAR health/status metadata (human-readable string) |
|
||
| `rt/utlidar/robot_pose` | robot → client | Robot odometry used as map origin |
|
||
|
||
Constants are defined in [src/protocol/topics.ts](../src/protocol/topics.ts) (`RTC_TOPIC.LIDAR_ARRAY`, `LIDAR_STATE`, `ROBOT_ODOM`). Subscription happens in `App.enableVideoAndSubscribe()`.
|
||
|
||
---
|
||
|
||
## Enable / Disable
|
||
|
||
The LiDAR has to be explicitly powered up — it doesn't stream by default.
|
||
|
||
**Enable:** `setLidarToggle(true)` publishes the string `"ON"` to `rt/utlidar/switch` **five times at 100 ms intervals** (firmware occasionally drops the first packet; the repeat is a reliability fix) and starts the radar spinning animation on the 3D model.
|
||
|
||
**Disable:** publishes `"OFF"` once, stops the radar spin, and clears the voxel map via `voxelMap.clear()`.
|
||
|
||
The toggle is surfaced via the LiDAR icon in the control view's setting bar (between Radar and Volume). The state is **not** persisted across reconnects — each WebRTC session starts with LiDAR off.
|
||
|
||
---
|
||
|
||
## Data Flow
|
||
|
||
```
|
||
Robot LiDAR
|
||
└─► WebRTC DataChannel (rt/utlidar/voxel_map_compressed)
|
||
└─► DataChannelHandler (onmessage)
|
||
└─► VoxelMap.update(ArrayBuffer, origin, resolution)
|
||
└─► voxel-worker (postMessage) [off main thread]
|
||
└─► libvoxel.wasm._generate(...)
|
||
├─► positions (Uint8Array)
|
||
├─► uvs (Uint8Array)
|
||
└─► indices (Uint32Array)
|
||
└─► Three.js BufferGeometry + MeshBasicMaterial
|
||
└─► scene.add(mesh)
|
||
```
|
||
|
||
Each incoming frame creates a fresh mesh. The previous mesh is disposed so the map effectively "replaces" on every successful decode.
|
||
|
||
---
|
||
|
||
## Compressed Packet Format
|
||
|
||
What JavaScript hands to the decoder (alongside the raw bytes from the WebRTC message):
|
||
|
||
```ts
|
||
{
|
||
data: ArrayBuffer; // LZ4-compressed voxel blob
|
||
resolution: number; // voxel cell size in meters (e.g. 0.1)
|
||
origin: [number, number, number]; // world offset for the map
|
||
}
|
||
```
|
||
|
||
`resolution` and `origin` come from sibling fields in the envelope. `origin` updates as the robot moves, so the mesh stays in world coordinates each frame.
|
||
|
||
### Compression: LZ4 block format
|
||
|
||
The payload is **LZ4 block format**. The uncompressed size is provided out-of-band in the packet envelope (`src_size`), so the decoder is a direct call to `LZ4_decompress_safe(src, dst, srcSize, dstCapacity)`. No LZ4 frame header — each packet is a standalone block.
|
||
|
||
Equivalent Python reference (useful for a standalone decoder):
|
||
|
||
```python
|
||
import lz4.block
|
||
decompressed = lz4.block.decompress(compressed_data, uncompressed_size=src_size)
|
||
```
|
||
|
||
Our WASM kernel [`public/libvoxel.wasm`](../public/libvoxel.wasm) (9 520 bytes) embeds the LZ4 decoder alongside the geometry generator, so JS only exchanges opaque pointers with it.
|
||
|
||
### Decompressed payload layout — 3D occupancy bitfield
|
||
|
||
After LZ4 decompress, the buffer is a flat `uint8[]` array interpreted as a **3D occupancy grid**, bit-packed along the X axis, 128×128×Z voxels per map.
|
||
|
||
**Per-byte addressing** (exactly as implemented in the Python reference):
|
||
|
||
```
|
||
byte_index → (x, y, z):
|
||
z = byte_index // 0x800 # 0x800 = 2048 bytes per Z slice
|
||
n = byte_index % 0x800
|
||
y = n // 0x10 # 0x10 = 16 bytes per Y row
|
||
x_base = (n % 0x10) * 8
|
||
# within the byte, bit 7 (MSB) = +0, bit 0 (LSB) = +7
|
||
for bit in 0..7:
|
||
if byte & (1 << (7 - bit)):
|
||
emit voxel at (x_base + bit, y, z)
|
||
```
|
||
|
||
**Dimensions:**
|
||
|
||
| Axis | Extent | Why |
|
||
|---|---|---|
|
||
| X | 128 | `16 bytes per row × 8 bits` |
|
||
| Y | 128 | `0x800 / 0x10 = 128 rows per slice` |
|
||
| Z | variable | `decompressed_size / 0x800` slices — grows with map height |
|
||
|
||
**Bit convention:** MSB-first within each byte (`bit 0 (MSB) = x_base+0`, `bit 7 (LSB) = x_base+7`). A set bit = the voxel at that grid cell is **occupied** (hit by the LiDAR).
|
||
|
||
**Grid → world conversion** (same formula as the Python `bits_to_points`):
|
||
|
||
```
|
||
world_xyz = grid_xyz * resolution + origin
|
||
```
|
||
|
||
- `resolution` is typically `0.05 m` or `0.1 m` per voxel (depends on firmware config — the frontend just uses what the envelope provides)
|
||
- `origin` is the world-space corner of the grid, updated as the robot moves
|
||
|
||
### From occupancy bits to renderable geometry
|
||
|
||
The WASM kernel walks the occupancy bitfield and emits one quad per occupied voxel (face-per-voxel, not merged). For each face it writes:
|
||
|
||
- 4 vertices × 3 `uint8` grid coords (scaled client-side by `resolution`)
|
||
- 4 vertices × 2 `uint8` UV coords for the height-gradient texture lookup
|
||
- 6 `uint32` indices (two triangles per quad)
|
||
|
||
Plus a scalar `zNormalized` per vertex used by the kernel to drive the UV row (so color encodes height).
|
||
|
||
### Output geometry buffers (kernel → worker → main thread)
|
||
|
||
For a packet with `faceCount = N`:
|
||
|
||
| Buffer | Size bytes | Layout |
|
||
|---|---|---|
|
||
| `positions` | `N × 12` | 4 verts/face × 3 coords × **uint8** (grid-space, scale by `resolution`) |
|
||
| `uvs` | `N × 8` | 4 verts/face × 2 coords × **uint8** (2D texture lookup) |
|
||
| `indices` | `N × 24` | 6 indices/face × **uint32** (2 triangles, 3 indices each) |
|
||
|
||
So each `face` = one voxel wall quad (4 vertices, 2 triangles). Max packet size supported: **240,000 faces** per frame (set by the `_malloc` capacities in [voxel-worker.ts:31-38](../src/workers/voxel-worker.ts#L31-L38) — `2,880,000 / 12 = 240k`).
|
||
|
||
Per-face WASM wall-clock on the worker thread is sub-millisecond at typical packet sizes; throttling (see [Performance](#performance)) caps the main-thread geometry rebuild at ~6.7 Hz.
|
||
|
||
---
|
||
|
||
## WASM Decoder (`libvoxel.wasm`)
|
||
|
||
**Location:** [public/libvoxel.wasm](../public/libvoxel.wasm) (loaded by fetch inside the worker)
|
||
|
||
**Loader:** [src/workers/voxel-worker.ts](../src/workers/voxel-worker.ts) — a standard dedicated Web Worker that instantiates the module with Emscripten-style imports (memcpy, heap resize).
|
||
|
||
**Exports:**
|
||
|
||
| Symbol | Signature | Role |
|
||
|---|---|---|
|
||
| `_generate` | `(inputPtr, inputLen, decompressBufferSize, decompressBuffer, decompressedSize, positions, uvs, indices, faceCount, pointCount, zNormalized) → void` | Main kernel: decompresses + emits triangulated geometry |
|
||
| `_malloc` | `(size) → ptr` | Allocate inside WASM linear memory |
|
||
| `_free` | `(ptr) → void` | Release allocation |
|
||
| `c` | `WebAssembly.Memory` | Shared linear memory (read back as `Uint8Array`/`Uint32Array` views to extract output arrays) |
|
||
|
||
The output arrays are produced inside WASM memory; the worker copies them into typed arrays and `postMessage`'s the result back to the main thread (transferring the ArrayBuffers to avoid a copy).
|
||
|
||
---
|
||
|
||
## Rendering Pipeline
|
||
|
||
Implemented in [src/ui/scene/voxel-map.ts](../src/ui/scene/voxel-map.ts).
|
||
|
||
- **Geometry:** `THREE.BufferGeometry` with:
|
||
- `position` attribute — `Uint8Array`, 3 components per vertex (voxel-grid coords)
|
||
- `uv` attribute — `Uint8Array`, 2 components
|
||
- `index` — `Uint32Array`
|
||
- **Material:** `THREE.MeshBasicMaterial` (no lighting — the color is already baked) with a `NearestFilter` texture at [public/models/axisColor4.png](../public/models/axisColor4.png).
|
||
- **Coloring:** height-based via UV lookup. The WASM kernel computes a normalized Z per vertex and writes it into the UV so the texture pixel at that row gives the final color. Swapping the texture changes the palette.
|
||
- **Transform:** the mesh is scaled by `resolution` and translated to `origin`, so the visualization sits in the same world space as the Go2 model.
|
||
- **Culling:** `frustumCulled = false` — the map can be very wide and camera-frustum tests have falsely clipped large meshes before.
|
||
|
||
The voxel map and robot model share the same Scene3D instance; the PIP-swap feature just toggles which one is "featured" vs "thumbnail".
|
||
|
||
---
|
||
|
||
## Robot Radar Animation
|
||
|
||
A dummy skeletal bone named `RadarBone` in the Go2 GLTF rotates **1 full revolution per second** while LiDAR is on. Implemented in [src/ui/scene/robot-model.ts](../src/ui/scene/robot-model.ts) via `requestAnimationFrame`. The spin is purely cosmetic — there's no physical LiDAR position in the model; it mirrors the APK's "radar is active" visual cue.
|
||
|
||
`setRadarSpinning(enabled)` is called from the same LiDAR toggle handler so the animation stays in sync with the actual stream.
|
||
|
||
---
|
||
|
||
## Performance
|
||
|
||
| Aspect | Detail |
|
||
|---|---|
|
||
| Worker thread | Single dedicated worker — WASM decode off the UI thread, main thread only rebuilds the Three.js BufferGeometry |
|
||
| Throttle | `VoxelMap.update()` coalesces at 150 ms — if packets arrive faster, only the latest queued payload is processed. Older ones are discarded so the map never lags |
|
||
| Transfers | Typed-array buffers are sent via `postMessage` with `transfer: [...]` (zero-copy ownership handoff) |
|
||
| Allocation | Each decode allocates a fresh `BufferGeometry`. The previous mesh is disposed (material kept, it's reusable) so memory stays flat |
|
||
| GC pressure | High-rate voxel frames can spike if `resolution` is too small or the world is huge — typical rates (~5-10 Hz post-throttle) work well |
|
||
|
||
The mesh is **not** instanced; the decoder outputs cube-face triangles directly for each occupied voxel in a single draw call.
|
||
|
||
---
|
||
|
||
## Files
|
||
|
||
| File | Role |
|
||
|---|---|
|
||
| [src/ui/app.ts](../src/ui/app.ts) | Subscription + toggle wiring; routes topic payloads to `VoxelMap` |
|
||
| [src/ui/scene/voxel-map.ts](../src/ui/scene/voxel-map.ts) | `VoxelMap` class: worker owner, mesh builder, throttle, clear |
|
||
| [src/workers/voxel-worker.ts](../src/workers/voxel-worker.ts) | Web Worker: loads `libvoxel.wasm`, runs `_generate`, posts back typed arrays |
|
||
| [public/libvoxel.wasm](../public/libvoxel.wasm) | Emscripten-compiled decoder (opaque binary) |
|
||
| [public/models/axisColor4.png](../public/models/axisColor4.png) | Height-gradient lookup texture |
|
||
| [src/ui/scene/robot-model.ts](../src/ui/scene/robot-model.ts) | Go2 GLTF model, `RadarBone` spin |
|
||
| [src/ui/scene/scene.ts](../src/ui/scene/scene.ts) | Three.js scene owner (shared by robot + voxel map) |
|
||
| [src/protocol/topics.ts](../src/protocol/topics.ts) | `RTC_TOPIC.LIDAR_ARRAY` / `LIDAR_STATE` / `ROBOT_ODOM` string constants |
|
||
| [src/protocol/data-channel.ts](../src/protocol/data-channel.ts) | `subscribe(topic)` / `publish(topic, data)` helpers |
|
||
|
||
---
|
||
|
||
## Quick Reference
|
||
|
||
| Item | Value |
|
||
|---|---|
|
||
| Topic (data) | `rt/utlidar/voxel_map_compressed` |
|
||
| Topic (state) | `rt/utlidar/lidar_state` |
|
||
| Topic (pose) | `rt/utlidar/robot_pose` |
|
||
| Topic (toggle) | `rt/utlidar/switch` (payload: `"ON"` / `"OFF"`) |
|
||
| Toggle reliability | `"ON"` sent 5× at 100 ms |
|
||
| WASM kernel | `_generate(...)` in `libvoxel.wasm` |
|
||
| Throttle | 150 ms (coalesces) |
|
||
| Radar spin | 1 rev/s |
|
||
| Default resolution | 0.1 m per voxel |
|