Files

Limer 9dab37e8c8 feat: 初始化 Go2 导航代码与配置

2026-10-14 11:08:44 +08:00

12 KiB

Raw Permalink Blame History

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.

Topics
Enable / Disable
Data Flow
Compressed Packet Format
WASM Decoder (libvoxel.wasm)
Rendering Pipeline
Robot Radar Animation
Performance
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 (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):

{
  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):

import lz4.block
decompressed = lz4.block.decompress(compressed_data, uncompressed_size=src_size)

Our WASM kernel 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 — 2,880,000 / 12 = 240k).

Per-face WASM wall-clock on the worker thread is sub-millisecond at typical packet sizes; throttling (see Performance) caps the main-thread geometry rebuild at ~6.7 Hz.

WASM Decoder (`libvoxel.wasm`)

Location: public/libvoxel.wasm (loaded by fetch inside the worker)

Loader: 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.

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.
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 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	Subscription + toggle wiring; routes topic payloads to `VoxelMap`
src/ui/scene/voxel-map.ts	`VoxelMap` class: worker owner, mesh builder, throttle, clear
src/workers/voxel-worker.ts	Web Worker: loads `libvoxel.wasm`, runs `_generate`, posts back typed arrays
public/libvoxel.wasm	Emscripten-compiled decoder (opaque binary)
public/models/axisColor4.png	Height-gradient lookup texture
src/ui/scene/robot-model.ts	Go2 GLTF model, `RadarBone` spin
src/ui/scene/scene.ts	Three.js scene owner (shared by robot + voxel map)
src/protocol/topics.ts	`RTC_TOPIC.LIDAR_ARRAY` / `LIDAR_STATE` / `ROBOT_ODOM` string constants
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

12 KiB Raw Permalink Blame History Unescape Escape