From 776c1381d380bd532ad65541480331f9ec3cb1c3 Mon Sep 17 00:00:00 2001 From: joaner Date: Fri, 10 Jul 2026 18:21:54 +0800 Subject: [PATCH 1/5] perf: unblock PointCloud2 playback for large color+depth MCAPs Move parsing off the main thread, reuse GPU buffers, and enlarge the SAB ring so multi-MB point clouds no longer fall back to structured clone. --- src/features/panels/ThreeD/ThreeDPanel.tsx | 313 +++++++++++++++--- .../ThreeD/core/PointCloudParse.worker.ts | 65 ++++ .../ThreeD/core/pointCloudWorkerProtocol.ts | 36 ++ src/infra/workers/transports/SabTransport.ts | 4 +- src/shared/utils/pointCloud.test.ts | 156 +++++++++ src/shared/utils/pointCloud.ts | 193 ++++++++++- 6 files changed, 712 insertions(+), 55 deletions(-) create mode 100644 src/features/panels/ThreeD/core/PointCloudParse.worker.ts create mode 100644 src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts create mode 100644 src/shared/utils/pointCloud.test.ts diff --git a/src/features/panels/ThreeD/ThreeDPanel.tsx b/src/features/panels/ThreeD/ThreeDPanel.tsx index 5c4c14e..fdfc118 100644 --- a/src/features/panels/ThreeD/ThreeDPanel.tsx +++ b/src/features/panels/ThreeD/ThreeDPanel.tsx @@ -6,8 +6,13 @@ import type { MessagePipelineState } from '@/core/pipeline/store'; import { useMessagePipeline } from '@/core/pipeline/useMessagePipeline'; import { messageBus } from '@/core/pipeline/messageBus'; import { scheduleFrame } from '@/shared/utils/rafScheduler'; -import { parsePointCloud2 } from '@/shared/utils/pointCloud'; import type { PointCloudData } from '@/shared/utils/pointCloud'; +import PointCloudParseWorkerClass from './core/PointCloudParse.worker.ts?worker&inline'; +import type { + PointCloudFieldWire, + PointCloudParseRequest, + PointCloudParseResponse, +} from './core/pointCloudWorkerProtocol'; import { transformBvhPointToScene } from '@/shared/bvh/coordinates'; import { getScenePanelThemeColors, type ScenePanelThemeColors } from '@/features/panels/common/scenePanelTheme'; import { @@ -317,35 +322,271 @@ const ZUpGrid: React.FC = ({ }; // ── Point cloud ──────────────────────────────────────────────────── -// `useMemo` alone would leak the previous BufferGeometry on every new point -// cloud message (useMemo does not dispose the old value). We own the lifetime -// here and explicitly dispose the previous geometry when `data` changes, and -// when the component unmounts. +// Geometry/attributes are reused across frames when the point count is +// unchanged. Rebuilding BufferGeometry every message forced Three.js to +// recompute boundingSphere (full point scan) and made R3F reconcile a new +// object identity each tick. +type PointCloudGpuState = { + geometry: THREE.BufferGeometry; + position: THREE.BufferAttribute; + color: THREE.BufferAttribute | null; + pointCount: number; +}; + +function disposePointCloudGpu(state: PointCloudGpuState | null): void { + if (!state) return; + state.geometry.dispose(); +} + +function applyPointCloudData( + prev: PointCloudGpuState | null, + data: PointCloudData, + options?: { takeOwnership?: boolean }, +): PointCloudGpuState { + const pointCount = data.positions.length / 3; + const hasColors = data.colors != undefined && data.colors.length === data.positions.length; + const takeOwnership = options?.takeOwnership === true; + + if (prev && prev.pointCount === pointCount) { + prev.position.array.set(data.positions); + prev.position.needsUpdate = true; + if (hasColors) { + if (prev.color) { + prev.color.array.set(data.colors!); + prev.color.needsUpdate = true; + } else { + const colorArray = takeOwnership ? data.colors! : data.colors!.slice(); + const colorAttr = new THREE.BufferAttribute(colorArray, 3); + colorAttr.setUsage(THREE.DynamicDrawUsage); + prev.geometry.setAttribute('color', colorAttr); + prev.color = colorAttr; + } + } else if (prev.color) { + prev.geometry.deleteAttribute('color'); + prev.color = null; + } + return prev; + } + + disposePointCloudGpu(prev); + const geometry = new THREE.BufferGeometry(); + const positionArray = takeOwnership ? data.positions : data.positions.slice(); + const position = new THREE.BufferAttribute(positionArray, 3); + position.setUsage(THREE.DynamicDrawUsage); + geometry.setAttribute('position', position); + let color: THREE.BufferAttribute | null = null; + if (hasColors) { + const colorArray = takeOwnership ? data.colors! : data.colors!.slice(); + color = new THREE.BufferAttribute(colorArray, 3); + color.setUsage(THREE.DynamicDrawUsage); + geometry.setAttribute('color', color); + } + // Compute once; we disable frustum culling so a stale sphere cannot hide + // a moving cloud after in-place attribute updates. + geometry.computeBoundingSphere(); + return { geometry, position, color, pointCount }; +} + +/** Low-frequency clouds (LaserScan / OccupancyGrid) that arrive via React props. */ const PointCloud = ({ data, color, size }: { data: PointCloudData; color: string; size: number }) => { - const geometryRef = useRef(null); + const gpuRef = useRef(null); const [geometry, setGeometry] = useState(null); + const [hasVertexColors, setHasVertexColors] = useState(false); + const { invalidate } = useThree(); + + useEffect(() => { + const next = applyPointCloudData(gpuRef.current, data); + gpuRef.current = next; + setGeometry(next.geometry); + setHasVertexColors(next.color != null); + invalidate(); + }, [data, invalidate]); useEffect(() => { - const geo = new THREE.BufferGeometry(); - geo.setAttribute('position', new THREE.BufferAttribute(data.positions, 3)); - const previous = geometryRef.current; - geometryRef.current = geo; - setGeometry(geo); - if (previous) previous.dispose(); return () => { - // Only dispose in the unmount cleanup; normal updates dispose above so - // this runs a single time when the component actually unmounts. - if (geometryRef.current === geo) { - geometryRef.current = null; - geo.dispose(); + disposePointCloudGpu(gpuRef.current); + gpuRef.current = null; + }; + }, []); + + if (!geometry) return null; + return ( + + + + ); +}; + +/** + * High-frequency PointCloud2 layer. Parsing runs in a dedicated worker; + * transferable ArrayBuffers come back so the main thread only uploads to GPU. + * Intermediate frames are coalesced (latest-only) while a parse is in flight. + */ +const LivePointCloudLayer = ({ + player, + panelId, + topic, + color, + size, +}: { + player: Player; + panelId: string; + topic: string; + color: string; + size: number; +}) => { + const gpuRef = useRef(null); + const [geometry, setGeometry] = useState(null); + const [hasVertexColors, setHasVertexColors] = useState(false); + const { invalidate } = useThree(); + const geometryIdentityRef = useRef(null); + const hasColorsRef = useRef(false); + const workerRef = useRef(null); + const nextIdRef = useRef(1); + const inflightIdRef = useRef(null); + const pendingJobRef = useRef(null); + + useEffect(() => { + const worker = new PointCloudParseWorkerClass(); + workerRef.current = worker; + + const applyParsed = (positions: Float32Array, colors: Float32Array | undefined) => { + const parsed: PointCloudData = colors ? { positions, colors } : { positions }; + const next = applyPointCloudData(gpuRef.current, parsed, { takeOwnership: true }); + gpuRef.current = next; + const colorsChanged = (next.color != null) !== hasColorsRef.current; + const geometryChanged = next.geometry !== geometryIdentityRef.current; + if (geometryChanged || colorsChanged) { + geometryIdentityRef.current = next.geometry; + hasColorsRef.current = next.color != null; + setGeometry(next.geometry); + setHasVertexColors(next.color != null); + } + invalidate(); + }; + + const flushPending = () => { + const pending = pendingJobRef.current; + if (!pending || inflightIdRef.current != null) { + return; } + pendingJobRef.current = null; + inflightIdRef.current = pending.id; + worker.postMessage(pending, [pending.data]); }; - }, [data]); + + worker.onmessage = (event: MessageEvent) => { + const response = event.data; + if (response.id !== inflightIdRef.current) { + // Stale (should not happen with single-inflight); ignore. + flushPending(); + return; + } + inflightIdRef.current = null; + if (response.type === 'parsed') { + const positions = new Float32Array(response.positions); + const colors = response.colors ? new Float32Array(response.colors) : undefined; + applyParsed(positions, colors); + } + flushPending(); + }; + + const consumerId = `${panelId}:pointcloud`; + let cachedFields: PointCloudFieldWire[] | null = null; + player.registerHighFrequencyConsumer(consumerId, { + topic, + lane: 'pointcloud', + mode: 'latest', + onLatestMessage: (msg) => { + const message = msg.message; + if (!message || typeof message !== 'object') { + return; + } + const record = message as Record; + const data = record.data; + if (!(data instanceof Uint8Array)) { + return; + } + if (!Array.isArray(record.fields) || typeof record.point_step !== 'number') { + return; + } + if (typeof record.width !== 'number' || typeof record.height !== 'number') { + return; + } + + if (!cachedFields) { + const fields: PointCloudFieldWire[] = []; + for (const field of record.fields) { + if (!field || typeof field !== 'object') continue; + const f = field as Record; + if (typeof f.name !== 'string' || typeof f.offset !== 'number') continue; + fields.push({ + name: f.name, + offset: f.offset, + datatype: typeof f.datatype === 'number' ? f.datatype : undefined, + }); + } + cachedFields = fields; + } + + // Always copy before transfer. HF lane may hand us a live SAB view + // (`preferSharedView`) or a buffer still referenced by the player tick; + // transferring the original would detach it and break playback. + const payload = data.buffer.slice( + data.byteOffset, + data.byteOffset + data.byteLength, + ) as ArrayBuffer; + const id = nextIdRef.current++; + const job: PointCloudParseRequest = { + type: 'parse', + id, + fields: cachedFields, + pointStep: record.point_step, + width: record.width, + height: record.height, + isBigendian: record.is_bigendian === true, + data: payload, + }; + // Latest-only: replace any queued job (its ArrayBuffer is simply dropped). + pendingJobRef.current = job; + if (inflightIdRef.current == null) { + pendingJobRef.current = null; + inflightIdRef.current = id; + worker.postMessage(job, [payload]); + } + }, + }); + + return () => { + player.unregisterHighFrequencyConsumer(consumerId); + worker.onmessage = null; + worker.terminate(); + workerRef.current = null; + pendingJobRef.current = null; + inflightIdRef.current = null; + disposePointCloudGpu(gpuRef.current); + gpuRef.current = null; + geometryIdentityRef.current = null; + hasColorsRef.current = false; + setGeometry(null); + setHasVertexColors(false); + }; + }, [player, panelId, topic, invalidate]); if (!geometry) return null; return ( - - + + ); }; @@ -1009,7 +1250,6 @@ const Scene = ({ topicSettings: ThreeDTopicSetting[]; onMeshLoadProgressChange?: (progress: MeshLoadProgress | null) => void; }) => { - const [pointCloud, setPointCloud] = useState(null); const [tracks, setTracks] = useState([]); const [markerPrimitives, setMarkerPrimitives] = useState([]); const [skeletonPrimitives, setSkeletonPrimitives] = useState([]); @@ -1182,25 +1422,6 @@ const Scene = ({ return () => player.unregisterSubscriptions(panelId); }, [player, panelId, urdfTopic, jointsTopic, bvhTopic, enabledTopicSettings, tfTopics]); - useEffect(() => { - if (!pcTopic) { - return; - } - const consumerId = `${panelId}:pointcloud`; - player.registerHighFrequencyConsumer(consumerId, { - topic: pcTopic, - lane: 'pointcloud', - mode: 'latest', - onLatestMessage: (msg) => { - const parsed = parsePointCloud2(msg.message); - if (parsed) { - setPointCloud(parsed); - } - }, - }); - return () => player.unregisterHighFrequencyConsumer(consumerId); - }, [player, panelId, pcTopic]); - const tfTopicSet = useMemo(() => new Set(tfTopics), [tfTopics]); const topicSettingByName = useMemo( () => new Map(enabledTopicSettings.map((entry) => [entry.topic, entry])), @@ -1395,7 +1616,15 @@ const Scene = ({ ))} {showAxes && } - {pointCloud && } + {pcTopic && ( + + )} {laserScanCloud && } {occupancyCloud && } {tracks.map((track) => ( @@ -1423,7 +1652,7 @@ const Scene = ({ /> )} - {showPlaceholder && !pointCloud && !urdfText && skeletonPrimitives.length === 0 && ( + {showPlaceholder && !pcTopic && !urdfText && skeletonPrimitives.length === 0 && ( diff --git a/src/features/panels/ThreeD/core/PointCloudParse.worker.ts b/src/features/panels/ThreeD/core/PointCloudParse.worker.ts new file mode 100644 index 0000000..f8a7bb2 --- /dev/null +++ b/src/features/panels/ThreeD/core/PointCloudParse.worker.ts @@ -0,0 +1,65 @@ +import { parsePointCloud2 } from '@/shared/utils/pointCloud'; +import type { + PointCloudParseRequest, + PointCloudParseResponse, + PointCloudParseSuccess, +} from './pointCloudWorkerProtocol'; + +/** `Float32Array.buffer` is typed as ArrayBufferLike; we only transfer plain ABs. */ +function transferableBuffer(view: Float32Array): ArrayBuffer { + const { buffer, byteOffset, byteLength } = view; + if (buffer instanceof ArrayBuffer && byteOffset === 0 && byteLength === buffer.byteLength) { + return buffer; + } + return buffer.slice(byteOffset, byteOffset + byteLength) as ArrayBuffer; +} + +self.onmessage = (event: MessageEvent) => { + const req = event.data; + if (!req || req.type !== 'parse') { + return; + } + + try { + const parsed = parsePointCloud2({ + fields: req.fields, + data: new Uint8Array(req.data), + point_step: req.pointStep, + width: req.width, + height: req.height, + is_bigendian: req.isBigendian, + }); + + if (!parsed) { + const failure: PointCloudParseResponse = { + type: 'error', + id: req.id, + message: 'invalid PointCloud2', + }; + self.postMessage(failure); + return; + } + + const positionsBuffer = transferableBuffer(parsed.positions); + const transfer: Transferable[] = [positionsBuffer]; + const success: PointCloudParseSuccess = { + type: 'parsed', + id: req.id, + pointCount: parsed.positions.length / 3, + positions: positionsBuffer, + }; + if (parsed.colors) { + const colorsBuffer = transferableBuffer(parsed.colors); + transfer.push(colorsBuffer); + success.colors = colorsBuffer; + } + self.postMessage(success, transfer); + } catch (err) { + const failure: PointCloudParseResponse = { + type: 'error', + id: req.id, + message: err instanceof Error ? err.message : String(err), + }; + self.postMessage(failure); + } +}; diff --git a/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts b/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts new file mode 100644 index 0000000..8023b07 --- /dev/null +++ b/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts @@ -0,0 +1,36 @@ +/** Protocol for the PointCloud2 parse worker (main ↔ worker). */ + +export type PointCloudFieldWire = { + name: string; + offset: number; + datatype?: number; +}; + +export type PointCloudParseRequest = { + type: 'parse'; + /** Monotonic id; main thread ignores stale responses. */ + id: number; + fields: PointCloudFieldWire[]; + pointStep: number; + width: number; + height: number; + isBigendian: boolean; + /** Raw PointCloud2 `data` bytes (transferred). */ + data: ArrayBuffer; +}; + +export type PointCloudParseSuccess = { + type: 'parsed'; + id: number; + pointCount: number; + positions: ArrayBuffer; + colors?: ArrayBuffer; +}; + +export type PointCloudParseFailure = { + type: 'error'; + id: number; + message: string; +}; + +export type PointCloudParseResponse = PointCloudParseSuccess | PointCloudParseFailure; diff --git a/src/infra/workers/transports/SabTransport.ts b/src/infra/workers/transports/SabTransport.ts index 9e6beef..f239a1b 100644 --- a/src/infra/workers/transports/SabTransport.ts +++ b/src/infra/workers/transports/SabTransport.ts @@ -4,8 +4,8 @@ import type { IWorkerSerializedSourceWorker } from "../types"; import type { SharedPayloadRingConfig, TransportDiagnostics } from "../transport"; import type { WorkerTransport } from "./BaseWorkerTransport"; -const DEFAULT_RING_BYTES = 256 * 1024 * 1024; -const DEFAULT_SLOT_BYTES = 4 * 1024 * 1024; +const DEFAULT_RING_BYTES = 384 * 1024 * 1024; +const DEFAULT_SLOT_BYTES = 16 * 1024 * 1024; export class SabTransport implements WorkerTransport { private readonly _ringConfig: SharedPayloadRingConfig; diff --git a/src/shared/utils/pointCloud.test.ts b/src/shared/utils/pointCloud.test.ts new file mode 100644 index 0000000..33966ac --- /dev/null +++ b/src/shared/utils/pointCloud.test.ts @@ -0,0 +1,156 @@ +import { describe, expect, it } from 'vitest'; +import { parsePointCloud2, PointFieldDatatype } from './pointCloud'; + +function packPclRgb(r: number, g: number, b: number): number { + const packed = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff); + const buf = new ArrayBuffer(4); + new Uint32Array(buf)[0] = packed; + return new Float32Array(buf)[0]!; +} + +function buildCloud(options: { + fields: Array<{ name: string; offset: number; datatype: number }>; + pointStep: number; + points: Array<(view: DataView, offset: number) => void>; + isBigendian?: boolean; +}) { + const width = options.points.length; + const height = 1; + const data = new Uint8Array(width * options.pointStep); + const view = new DataView(data.buffer); + for (let i = 0; i < options.points.length; i++) { + options.points[i]!(view, i * options.pointStep); + } + return { + fields: options.fields, + data, + point_step: options.pointStep, + width, + height, + is_bigendian: options.isBigendian ?? false, + }; +} + +describe('parsePointCloud2', () => { + it('parses xyz without colors', () => { + const message = buildCloud({ + fields: [ + { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + ], + pointStep: 16, + points: [ + (view, offset) => { + view.setFloat32(offset, 1, true); + view.setFloat32(offset + 4, 2, true); + view.setFloat32(offset + 8, 3, true); + }, + ], + }); + + const parsed = parsePointCloud2(message); + expect(parsed).not.toBeNull(); + expect(Array.from(parsed!.positions)).toEqual([1, 2, 3]); + expect(parsed!.colors).toBeUndefined(); + }); + + it('parses PCL packed rgb float32 field', () => { + const rgbFloat = packPclRgb(255, 128, 0); + const message = buildCloud({ + fields: [ + { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'rgb', offset: 12, datatype: PointFieldDatatype.FLOAT32 }, + ], + pointStep: 16, + points: [ + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 12, rgbFloat, true); + }, + ], + }); + + const parsed = parsePointCloud2(message); + expect(parsed?.colors).toBeDefined(); + expect(parsed!.colors![0]).toBeCloseTo(1); + expect(parsed!.colors![1]).toBeCloseTo(128 / 255); + expect(parsed!.colors![2]).toBeCloseTo(0); + }); + + it('parses separate r/g/b uint8 fields', () => { + const message = buildCloud({ + fields: [ + { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'r', offset: 12, datatype: PointFieldDatatype.UINT8 }, + { name: 'g', offset: 13, datatype: PointFieldDatatype.UINT8 }, + { name: 'b', offset: 14, datatype: PointFieldDatatype.UINT8 }, + ], + pointStep: 16, + points: [ + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 0, true); + view.setUint8(offset + 12, 0); + view.setUint8(offset + 13, 255); + view.setUint8(offset + 14, 127); + }, + ], + }); + + const parsed = parsePointCloud2(message); + expect(parsed?.colors).toBeDefined(); + expect(parsed!.colors![0]).toBeCloseTo(0); + expect(parsed!.colors![1]).toBeCloseTo(1); + expect(parsed!.colors![2]).toBeCloseTo(127 / 255); + }); + + it('parses intensity as per-frame normalized grayscale', () => { + const message = buildCloud({ + fields: [ + { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'intensity', offset: 12, datatype: PointFieldDatatype.FLOAT32 }, + ], + pointStep: 16, + points: [ + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 12, 10, true); + }, + (view, offset) => { + view.setFloat32(offset, 1, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 12, 30, true); + }, + ], + }); + + const parsed = parsePointCloud2(message); + expect(parsed?.colors).toBeDefined(); + // first point = min → 0 + expect(parsed!.colors![0]).toBeCloseTo(0); + expect(parsed!.colors![1]).toBeCloseTo(0); + expect(parsed!.colors![2]).toBeCloseTo(0); + // second point = max → 1 + expect(parsed!.colors![3]).toBeCloseTo(1); + expect(parsed!.colors![4]).toBeCloseTo(1); + expect(parsed!.colors![5]).toBeCloseTo(1); + }); + + it('returns null for incomplete messages', () => { + expect(parsePointCloud2(null)).toBeNull(); + expect(parsePointCloud2({})).toBeNull(); + }); +}); diff --git a/src/shared/utils/pointCloud.ts b/src/shared/utils/pointCloud.ts index 6d3c72b..d5bdc79 100644 --- a/src/shared/utils/pointCloud.ts +++ b/src/shared/utils/pointCloud.ts @@ -9,6 +9,10 @@ interface PointField { datatype?: number; } +/** sensor_msgs/PointField datatype constants. */ +const POINT_FIELD_UINT8 = 2; +const POINT_FIELD_FLOAT32 = 7; + function isPointField(value: unknown): value is PointField { if (!value || typeof value !== "object") return false; const o = value as Record; @@ -19,6 +23,45 @@ function isUint8Array(value: unknown): value is Uint8Array { return value instanceof Uint8Array; } +function fieldByName(fields: PointField[], name: string): PointField | undefined { + return fields.find((f) => f.name === name); +} + +function readPackedRgb( + view: DataView, + offset: number, + littleEndian: boolean, +): [number, number, number] { + const packed = view.getUint32(offset, littleEndian); + // PCL packs float32 bit-pattern as 0xAARRGGBB (rgba) or 0x00RRGGBB (rgb). + const r = ((packed >> 16) & 0xff) / 255; + const g = ((packed >> 8) & 0xff) / 255; + const b = (packed & 0xff) / 255; + return [r, g, b]; +} + +function readNumericField( + view: DataView, + offset: number, + datatype: number | undefined, + littleEndian: boolean, +): number { + if (datatype === POINT_FIELD_UINT8) { + return view.getUint8(offset); + } + // Default to float32 (datatype 7) which covers intensity and xyz. + return view.getFloat32(offset, littleEndian); +} + +/** + * Parse a `sensor_msgs/PointCloud2` message into GPU-friendly typed arrays. + * + * Color priority: + * 1. `rgb` / `rgba` (PCL packed float32) + * 2. separate `r`/`g`/`b` (uint8) + * 3. `intensity` (float32 or uint8), normalized per-frame to grayscale + * 4. none → `colors` stays undefined + */ export function parsePointCloud2(message: unknown): PointCloudData | null { if (!message || typeof message !== "object") return null; const m = message as Record; @@ -27,24 +70,152 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { if (typeof width !== "number" || typeof height !== "number") return null; const count = width * height; - const positions = new Float32Array(count * 3); + if (count <= 0 || point_step <= 0) return null; + if (data.byteLength < count * point_step) return null; const typedFields = fields.filter(isPointField); - const xField = typedFields.find((f) => f.name === "x"); - const yField = typedFields.find((f) => f.name === "y"); - const zField = typedFields.find((f) => f.name === "z"); - + const xField = fieldByName(typedFields, "x"); + const yField = fieldByName(typedFields, "y"); + const zField = fieldByName(typedFields, "z"); if (!xField || !yField || !zField) return null; + const littleEndian = m.is_bigendian !== true; + const positions = extractPositions( + data, + count, + point_step, + xField, + yField, + zField, + littleEndian, + ); + const view = new DataView(data.buffer, data.byteOffset, data.byteLength); - const isLittleEndian = true; // ROS2 is usually little endian + const colors = extractColors(view, typedFields, count, point_step, littleEndian); + return colors ? { positions, colors } : { positions }; +} + +/** + * Fast path: contiguous xyz float32 at offsets 0/4/8 on little-endian clouds + * (Orbbec / most ROS drivers). Uses a Float32Array view instead of DataView. + */ +function extractPositions( + data: Uint8Array, + count: number, + pointStep: number, + xField: PointField, + yField: PointField, + zField: PointField, + littleEndian: boolean, +): Float32Array { + const positions = new Float32Array(count * 3); + const xyzContiguous = + littleEndian && + pointStep % 4 === 0 && + xField.offset === 0 && + yField.offset === 4 && + zField.offset === 8 && + (xField.datatype === undefined || xField.datatype === POINT_FIELD_FLOAT32) && + (yField.datatype === undefined || yField.datatype === POINT_FIELD_FLOAT32) && + (zField.datatype === undefined || zField.datatype === POINT_FIELD_FLOAT32); + + if (xyzContiguous) { + const src = new Float32Array(data.buffer, data.byteOffset, Math.floor(data.byteLength / 4)); + const floatsPerPoint = pointStep / 4; + for (let i = 0; i < count; i++) { + const s = i * floatsPerPoint; + const d = i * 3; + positions[d] = src[s]!; + positions[d + 1] = src[s + 1]!; + positions[d + 2] = src[s + 2]!; + } + return positions; + } + const view = new DataView(data.buffer, data.byteOffset, data.byteLength); for (let i = 0; i < count; i++) { - const offset = i * point_step; - positions[i * 3] = view.getFloat32(offset + xField.offset, isLittleEndian); - positions[i * 3 + 1] = view.getFloat32(offset + yField.offset, isLittleEndian); - positions[i * 3 + 2] = view.getFloat32(offset + zField.offset, isLittleEndian); + const offset = i * pointStep; + const d = i * 3; + positions[d] = view.getFloat32(offset + xField.offset, littleEndian); + positions[d + 1] = view.getFloat32(offset + yField.offset, littleEndian); + positions[d + 2] = view.getFloat32(offset + zField.offset, littleEndian); + } + return positions; +} + +function extractColors( + view: DataView, + fields: PointField[], + count: number, + pointStep: number, + littleEndian: boolean, +): Float32Array | undefined { + const rgbField = fieldByName(fields, "rgb") ?? fieldByName(fields, "rgba"); + if (rgbField) { + const colors = new Float32Array(count * 3); + for (let i = 0; i < count; i++) { + const offset = i * pointStep + rgbField.offset; + const [r, g, b] = readPackedRgb(view, offset, littleEndian); + colors[i * 3] = r; + colors[i * 3 + 1] = g; + colors[i * 3 + 2] = b; + } + return colors; } - return { positions }; + const rField = fieldByName(fields, "r"); + const gField = fieldByName(fields, "g"); + const bField = fieldByName(fields, "b"); + if (rField && gField && bField) { + const colors = new Float32Array(count * 3); + for (let i = 0; i < count; i++) { + const base = i * pointStep; + colors[i * 3] = view.getUint8(base + rField.offset) / 255; + colors[i * 3 + 1] = view.getUint8(base + gField.offset) / 255; + colors[i * 3 + 2] = view.getUint8(base + bField.offset) / 255; + } + return colors; + } + + const intensityField = fieldByName(fields, "intensity"); + if (!intensityField) return undefined; + + let min = Number.POSITIVE_INFINITY; + let max = Number.NEGATIVE_INFINITY; + const raw = new Float32Array(count); + for (let i = 0; i < count; i++) { + const value = readNumericField( + view, + i * pointStep + intensityField.offset, + intensityField.datatype, + littleEndian, + ); + raw[i] = value; + if (value < min) min = value; + if (value > max) max = value; + } + + const range = max - min; + const colors = new Float32Array(count * 3); + if (range <= 0 || !Number.isFinite(range)) { + // Constant intensity → mid-gray so points remain visible. + colors.fill(0.5); + return colors; + } + + // UINT8 intensity is already 0–255; still normalize by observed min/max so + // sparse clouds with a narrow band stay high-contrast. + for (let i = 0; i < count; i++) { + const t = (raw[i]! - min) / range; + colors[i * 3] = t; + colors[i * 3 + 1] = t; + colors[i * 3 + 2] = t; + } + return colors; } + +// Re-export datatype constants for tests. +export const PointFieldDatatype = { + UINT8: POINT_FIELD_UINT8, + FLOAT32: POINT_FIELD_FLOAT32, +} as const; From 579cf187e142cbc648a9543ddcfe8d7e97348516 Mon Sep 17 00:00:00 2001 From: joaner Date: Fri, 10 Jul 2026 18:33:52 +0800 Subject: [PATCH 2/5] fix: restore PointCloud2 view with depth turbo and safe SAB copies Copy SharedArrayBuffer payloads into plain ArrayBuffers before worker transfer, drop non-finite points, map optical frames to ROS Z-up, and autofit the camera so depth clouds match the color image orientation. --- src/features/panels/ThreeD/ThreeDPanel.tsx | 57 +++- src/features/panels/common/zUpSceneLayout.ts | 56 ++++ src/shared/utils/pointCloud.test.ts | 113 ++++++-- src/shared/utils/pointCloud.ts | 289 +++++++++++-------- 4 files changed, 362 insertions(+), 153 deletions(-) diff --git a/src/features/panels/ThreeD/ThreeDPanel.tsx b/src/features/panels/ThreeD/ThreeDPanel.tsx index fdfc118..7c3df52 100644 --- a/src/features/panels/ThreeD/ThreeDPanel.tsx +++ b/src/features/panels/ThreeD/ThreeDPanel.tsx @@ -7,6 +7,7 @@ import { useMessagePipeline } from '@/core/pipeline/useMessagePipeline'; import { messageBus } from '@/core/pipeline/messageBus'; import { scheduleFrame } from '@/shared/utils/rafScheduler'; import type { PointCloudData } from '@/shared/utils/pointCloud'; +import { copyToTransferableArrayBuffer } from '@/shared/utils/pointCloud'; import PointCloudParseWorkerClass from './core/PointCloudParse.worker.ts?worker&inline'; import type { PointCloudFieldWire, @@ -25,6 +26,7 @@ import { CANVAS_GL, DEFAULT_GRID_DIVISIONS, DEFAULT_GRID_SIZE, + framePerspectiveCameraToBox, framePerspectiveCameraToGrid, Z_UP, } from '@/features/panels/common/zUpSceneLayout'; @@ -338,6 +340,14 @@ function disposePointCloudGpu(state: PointCloudGpuState | null): void { state.geometry.dispose(); } +function safeComputeBoundingSphere(geometry: THREE.BufferGeometry): void { + geometry.computeBoundingSphere(); + const sphere = geometry.boundingSphere; + if (!sphere || !Number.isFinite(sphere.radius) || sphere.radius < 0) { + geometry.boundingSphere = new THREE.Sphere(new THREE.Vector3(0, 0, 0), 1); + } +} + function applyPointCloudData( prev: PointCloudGpuState | null, data: PointCloudData, @@ -381,9 +391,9 @@ function applyPointCloudData( color.setUsage(THREE.DynamicDrawUsage); geometry.setAttribute('color', color); } - // Compute once; we disable frustum culling so a stale sphere cannot hide - // a moving cloud after in-place attribute updates. - geometry.computeBoundingSphere(); + // Compute once; frustumCulled is false so a stale sphere cannot hide a + // moving cloud. Guard against NaN from sparse/invalid depth clouds. + safeComputeBoundingSphere(geometry); return { geometry, position, color, pointCount }; } @@ -443,19 +453,44 @@ const LivePointCloudLayer = ({ const gpuRef = useRef(null); const [geometry, setGeometry] = useState(null); const [hasVertexColors, setHasVertexColors] = useState(false); - const { invalidate } = useThree(); + const { invalidate, camera, controls } = useThree(); const geometryIdentityRef = useRef(null); const hasColorsRef = useRef(false); const workerRef = useRef(null); const nextIdRef = useRef(1); const inflightIdRef = useRef(null); const pendingJobRef = useRef(null); + const didAutofitRef = useRef(false); + + useEffect(() => { + didAutofitRef.current = false; + }, [topic]); useEffect(() => { const worker = new PointCloudParseWorkerClass(); workerRef.current = worker; + const autofitOnce = (geo: THREE.BufferGeometry) => { + if (didAutofitRef.current) return; + if (!(camera instanceof THREE.PerspectiveCamera)) return; + safeComputeBoundingSphere(geo); + const sphere = geo.boundingSphere; + if (!sphere || !Number.isFinite(sphere.radius) || sphere.radius <= 0) return; + const box = new THREE.Box3().setFromBufferAttribute( + geo.getAttribute('position') as THREE.BufferAttribute, + ); + if (box.isEmpty()) return; + const target = framePerspectiveCameraToBox(camera, box); + const oc = controls as ThreeOrbitControls | null; + if (oc) { + oc.target.copy(target); + oc.update(); + } + didAutofitRef.current = true; + }; + const applyParsed = (positions: Float32Array, colors: Float32Array | undefined) => { + if (positions.length === 0) return; const parsed: PointCloudData = colors ? { positions, colors } : { positions }; const next = applyPointCloudData(gpuRef.current, parsed, { takeOwnership: true }); gpuRef.current = next; @@ -467,6 +502,7 @@ const LivePointCloudLayer = ({ setGeometry(next.geometry); setHasVertexColors(next.color != null); } + autofitOnce(next.geometry); invalidate(); }; @@ -483,7 +519,6 @@ const LivePointCloudLayer = ({ worker.onmessage = (event: MessageEvent) => { const response = event.data; if (response.id !== inflightIdRef.current) { - // Stale (should not happen with single-inflight); ignore. flushPending(); return; } @@ -534,13 +569,8 @@ const LivePointCloudLayer = ({ cachedFields = fields; } - // Always copy before transfer. HF lane may hand us a live SAB view - // (`preferSharedView`) or a buffer still referenced by the player tick; - // transferring the original would detach it and break playback. - const payload = data.buffer.slice( - data.byteOffset, - data.byteOffset + data.byteLength, - ) as ArrayBuffer; + // SAB views cannot be transferred — always copy into a plain ArrayBuffer. + const payload = copyToTransferableArrayBuffer(data); const id = nextIdRef.current++; const job: PointCloudParseRequest = { type: 'parse', @@ -552,7 +582,6 @@ const LivePointCloudLayer = ({ isBigendian: record.is_bigendian === true, data: payload, }; - // Latest-only: replace any queued job (its ArrayBuffer is simply dropped). pendingJobRef.current = job; if (inflightIdRef.current == null) { pendingJobRef.current = null; @@ -576,7 +605,7 @@ const LivePointCloudLayer = ({ setGeometry(null); setHasVertexColors(false); }; - }, [player, panelId, topic, invalidate]); + }, [player, panelId, topic, invalidate, camera, controls]); if (!geometry) return null; return ( diff --git a/src/features/panels/common/zUpSceneLayout.ts b/src/features/panels/common/zUpSceneLayout.ts index 8813f4e..033a508 100644 --- a/src/features/panels/common/zUpSceneLayout.ts +++ b/src/features/panels/common/zUpSceneLayout.ts @@ -61,3 +61,59 @@ export function framePerspectiveCameraToGrid( camera.far = Math.max(6000, distance * 80); camera.updateProjectionMatrix(); } + +/** + * Frame a Z-up perspective camera to a point-cloud AABB so the view looks + * along ROS +X (forward), matching a depth/color camera "looking ahead". + * Returns the box center for OrbitControls.target. + */ +export function framePerspectiveCameraToBox( + camera: THREE.PerspectiveCamera, + box: THREE.Box3, + fillRatio: number = CAMERA_GRID_FILL_RATIO, +): THREE.Vector3 { + const center = box.getCenter(new THREE.Vector3()); + const size = box.getSize(new THREE.Vector3()); + // Camera sits behind-and-slightly-above the cloud, looking toward +X. + const fromCenterToCamera = new THREE.Vector3(-1, 0, 0.28).normalize(); + const forward = fromCenterToCamera.clone().negate(); + const right = new THREE.Vector3().crossVectors(forward, Z_UP).normalize(); + const up = new THREE.Vector3().crossVectors(right, forward).normalize(); + + const vFov = THREE.MathUtils.degToRad(camera.fov); + const hFov = 2 * Math.atan(Math.tan(vFov / 2) * camera.aspect); + const tanHalfV = Math.tan(vFov / 2); + const tanHalfH = Math.tan(hFov / 2); + + const half = size.clone().multiplyScalar(0.5); + const corners = [ + new THREE.Vector3(-half.x, -half.y, -half.z), + new THREE.Vector3(-half.x, -half.y, half.z), + new THREE.Vector3(-half.x, half.y, -half.z), + new THREE.Vector3(-half.x, half.y, half.z), + new THREE.Vector3(half.x, -half.y, -half.z), + new THREE.Vector3(half.x, -half.y, half.z), + new THREE.Vector3(half.x, half.y, -half.z), + new THREE.Vector3(half.x, half.y, half.z), + ]; + + let distance = 0.5; + for (const corner of corners) { + const towardCamera = corner.dot(fromCenterToCamera); + distance = Math.max( + distance, + towardCamera + Math.abs(corner.dot(right)) / (tanHalfH * fillRatio), + towardCamera + Math.abs(corner.dot(up)) / (tanHalfV * fillRatio), + ); + } + // Keep a little padding so near-plane points are not clipped. + distance = Math.max(distance, size.length() * 0.35 + 0.5); + + camera.up.copy(Z_UP); + camera.position.copy(center).addScaledVector(fromCenterToCamera, distance); + camera.lookAt(center); + camera.near = Math.max(0.01, distance / 1500); + camera.far = Math.max(6000, distance * 80); + camera.updateProjectionMatrix(); + return center; +} diff --git a/src/shared/utils/pointCloud.test.ts b/src/shared/utils/pointCloud.test.ts index 33966ac..7969cb1 100644 --- a/src/shared/utils/pointCloud.test.ts +++ b/src/shared/utils/pointCloud.test.ts @@ -1,5 +1,11 @@ import { describe, expect, it } from 'vitest'; -import { parsePointCloud2, PointFieldDatatype } from './pointCloud'; +import { + copyToTransferableArrayBuffer, + opticalToRos, + parsePointCloud2, + PointFieldDatatype, + sampleTurbo, +} from './pointCloud'; function packPclRgb(r: number, g: number, b: number): number { const packed = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff); @@ -31,14 +37,34 @@ function buildCloud(options: { }; } +const XYZ_FIELDS = [ + { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, + { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, +]; + +describe('opticalToRos', () => { + it('maps optical Z-forward Y-down to ROS X-forward Z-up', () => { + expect(opticalToRos(1, 2, 3)).toEqual([3, -1, -2]); + }); +}); + +describe('copyToTransferableArrayBuffer', () => { + it('returns a plain ArrayBuffer even when source is SharedArrayBuffer-backed', () => { + const sab = new SharedArrayBuffer(8); + const view = new Uint8Array(sab); + view.set([1, 2, 3, 4, 5, 6, 7, 8]); + const copied = copyToTransferableArrayBuffer(view); + expect(copied).toBeInstanceOf(ArrayBuffer); + expect(copied).not.toBeInstanceOf(SharedArrayBuffer); + expect([...new Uint8Array(copied)]).toEqual([1, 2, 3, 4, 5, 6, 7, 8]); + }); +}); + describe('parsePointCloud2', () => { - it('parses xyz without colors', () => { + it('converts optical xyz to ROS and colorizes by depth turbo when no color fields', () => { const message = buildCloud({ - fields: [ - { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, - ], + fields: XYZ_FIELDS, pointStep: 16, points: [ (view, offset) => { @@ -46,22 +72,57 @@ describe('parsePointCloud2', () => { view.setFloat32(offset + 4, 2, true); view.setFloat32(offset + 8, 3, true); }, + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 10, true); + }, ], }); const parsed = parsePointCloud2(message); expect(parsed).not.toBeNull(); - expect(Array.from(parsed!.positions)).toEqual([1, 2, 3]); - expect(parsed!.colors).toBeUndefined(); + // optical (1,2,3) → ROS (3,-1,-2); optical (0,0,10) → ROS (10,0,0) + expect(Array.from(parsed!.positions.subarray(0, 3))).toEqual([3, -1, -2]); + expect(parsed!.positions[3]).toBeCloseTo(10); + expect(parsed!.positions[4]).toBeCloseTo(0); + expect(parsed!.positions[5]).toBeCloseTo(0); + expect(parsed!.colors).toBeDefined(); + expect(parsed!.colors!.length).toBe(6); + // Near (depth 3) vs far (depth 10) should differ under turbo. + expect(parsed!.colors![0]).not.toBeCloseTo(parsed!.colors![3]!, 2); + }); + + it('drops non-finite points', () => { + const message = buildCloud({ + fields: XYZ_FIELDS, + pointStep: 16, + points: [ + (view, offset) => { + view.setFloat32(offset, Number.NaN, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 1, true); + }, + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 2, true); + }, + ], + }); + + const parsed = parsePointCloud2(message); + expect(parsed!.positions.length).toBe(3); + expect(parsed!.positions[0]).toBeCloseTo(2); + expect(parsed!.positions[1]).toBeCloseTo(0); + expect(parsed!.positions[2]).toBeCloseTo(0); }); it('parses PCL packed rgb float32 field', () => { const rgbFloat = packPclRgb(255, 128, 0); const message = buildCloud({ fields: [ - { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + ...XYZ_FIELDS, { name: 'rgb', offset: 12, datatype: PointFieldDatatype.FLOAT32 }, ], pointStep: 16, @@ -69,7 +130,7 @@ describe('parsePointCloud2', () => { (view, offset) => { view.setFloat32(offset, 0, true); view.setFloat32(offset + 4, 0, true); - view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 8, 1, true); view.setFloat32(offset + 12, rgbFloat, true); }, ], @@ -85,9 +146,7 @@ describe('parsePointCloud2', () => { it('parses separate r/g/b uint8 fields', () => { const message = buildCloud({ fields: [ - { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + ...XYZ_FIELDS, { name: 'r', offset: 12, datatype: PointFieldDatatype.UINT8 }, { name: 'g', offset: 13, datatype: PointFieldDatatype.UINT8 }, { name: 'b', offset: 14, datatype: PointFieldDatatype.UINT8 }, @@ -97,7 +156,7 @@ describe('parsePointCloud2', () => { (view, offset) => { view.setFloat32(offset, 0, true); view.setFloat32(offset + 4, 0, true); - view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 8, 1, true); view.setUint8(offset + 12, 0); view.setUint8(offset + 13, 255); view.setUint8(offset + 14, 127); @@ -115,9 +174,7 @@ describe('parsePointCloud2', () => { it('parses intensity as per-frame normalized grayscale', () => { const message = buildCloud({ fields: [ - { name: 'x', offset: 0, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'y', offset: 4, datatype: PointFieldDatatype.FLOAT32 }, - { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, + ...XYZ_FIELDS, { name: 'intensity', offset: 12, datatype: PointFieldDatatype.FLOAT32 }, ], pointStep: 16, @@ -125,13 +182,13 @@ describe('parsePointCloud2', () => { (view, offset) => { view.setFloat32(offset, 0, true); view.setFloat32(offset + 4, 0, true); - view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 8, 1, true); view.setFloat32(offset + 12, 10, true); }, (view, offset) => { view.setFloat32(offset, 1, true); view.setFloat32(offset + 4, 0, true); - view.setFloat32(offset + 8, 0, true); + view.setFloat32(offset + 8, 1, true); view.setFloat32(offset + 12, 30, true); }, ], @@ -139,11 +196,9 @@ describe('parsePointCloud2', () => { const parsed = parsePointCloud2(message); expect(parsed?.colors).toBeDefined(); - // first point = min → 0 expect(parsed!.colors![0]).toBeCloseTo(0); expect(parsed!.colors![1]).toBeCloseTo(0); expect(parsed!.colors![2]).toBeCloseTo(0); - // second point = max → 1 expect(parsed!.colors![3]).toBeCloseTo(1); expect(parsed!.colors![4]).toBeCloseTo(1); expect(parsed!.colors![5]).toBeCloseTo(1); @@ -154,3 +209,13 @@ describe('parsePointCloud2', () => { expect(parsePointCloud2({})).toBeNull(); }); }); + +describe('sampleTurbo', () => { + it('writes distinct colors at 0 and 1', () => { + const a = new Float32Array(3); + const b = new Float32Array(3); + sampleTurbo(0, a, 0); + sampleTurbo(1, b, 0); + expect(a[0]).not.toBeCloseTo(b[0]!, 2); + }); +}); diff --git a/src/shared/utils/pointCloud.ts b/src/shared/utils/pointCloud.ts index d5bdc79..9affc8a 100644 --- a/src/shared/utils/pointCloud.ts +++ b/src/shared/utils/pointCloud.ts @@ -49,18 +49,66 @@ function readNumericField( if (datatype === POINT_FIELD_UINT8) { return view.getUint8(offset); } - // Default to float32 (datatype 7) which covers intensity and xyz. return view.getFloat32(offset, littleEndian); } +/** REP-103: optical (Z forward, Y down) → ROS (X forward, Z up). */ +export function opticalToRos(ox: number, oy: number, oz: number): [number, number, number] { + return [oz, -ox, -oy]; +} + +// ── Turbo (same polynomial as Image panel; compact for point clouds) ───────── + +const kRedVec4 = [0.13572138, 4.6153926, -42.66032258, 132.13108234] as const; +const kGreenVec4 = [0.09140261, 2.19418839, 4.84296658, -14.18503333] as const; +const kBlueVec4 = [0.1066733, 12.64194608, -60.58204836, 110.36276771] as const; +const kRedVec2 = [-152.94239396, 59.28637943] as const; +const kGreenVec2 = [4.27729857, 2.82956604] as const; +const kBlueVec2 = [-89.90310912, 27.34824973] as const; + +function clamp01(x: number): number { + return Math.max(0, Math.min(1, x)); +} + +/** Sample turbo colormap at pct ∈ [0,1] into rgb out[outOffset..+2]. */ +export function sampleTurbo(pct: number, out: Float32Array, outOffset: number): void { + const x = clamp01(pct) * 0.99 + 0.01; + const x2 = x * x; + const x3 = x2 * x; + const x4 = x2 * x2; + const x5 = x3 * x2; + const r = + kRedVec4[0] + + x * kRedVec4[1] + + x2 * kRedVec4[2] + + x3 * kRedVec4[3] + + x4 * kRedVec2[0] + + x5 * kRedVec2[1]; + const g = + kGreenVec4[0] + + x * kGreenVec4[1] + + x2 * kGreenVec4[2] + + x3 * kGreenVec4[3] + + x4 * kGreenVec2[0] + + x5 * kGreenVec2[1]; + const b = + kBlueVec4[0] + + x * kBlueVec4[1] + + x2 * kBlueVec4[2] + + x3 * kBlueVec4[3] + + x4 * kBlueVec2[0] + + x5 * kBlueVec2[1]; + out[outOffset] = clamp01(r); + out[outOffset + 1] = clamp01(g); + out[outOffset + 2] = clamp01(b); +} + /** * Parse a `sensor_msgs/PointCloud2` message into GPU-friendly typed arrays. * - * Color priority: - * 1. `rgb` / `rgba` (PCL packed float32) - * 2. separate `r`/`g`/`b` (uint8) - * 3. `intensity` (float32 or uint8), normalized per-frame to grayscale - * 4. none → `colors` stays undefined + * - Drops non-finite xyz (common when `is_dense=false`). + * - Converts optical → ROS (X forward, Z up) for Z-up scene rendering. + * - Color priority: rgb/rgba → r/g/b → intensity → depth turbo (optical Z). */ export function parsePointCloud2(message: unknown): PointCloudData | null { if (!message || typeof message !== "object") return null; @@ -80,38 +128,31 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { if (!xField || !yField || !zField) return null; const littleEndian = m.is_bigendian !== true; - const positions = extractPositions( - data, - count, - point_step, - xField, - yField, - zField, - littleEndian, - ); - const view = new DataView(data.buffer, data.byteOffset, data.byteLength); - const colors = extractColors(view, typedFields, count, point_step, littleEndian); - return colors ? { positions, colors } : { positions }; -} -/** - * Fast path: contiguous xyz float32 at offsets 0/4/8 on little-endian clouds - * (Orbbec / most ROS drivers). Uses a Float32Array view instead of DataView. - */ -function extractPositions( - data: Uint8Array, - count: number, - pointStep: number, - xField: PointField, - yField: PointField, - zField: PointField, - littleEndian: boolean, -): Float32Array { - const positions = new Float32Array(count * 3); + const rgbField = fieldByName(typedFields, "rgb") ?? fieldByName(typedFields, "rgba"); + const rField = fieldByName(typedFields, "r"); + const gField = fieldByName(typedFields, "g"); + const bField = fieldByName(typedFields, "b"); + const hasRgbChannels = !!(rField && gField && bField); + const intensityField = fieldByName(typedFields, "intensity"); + + const colorMode: "rgb" | "rgba_fields" | "intensity" | "depth" = rgbField + ? "rgb" + : hasRgbChannels + ? "rgba_fields" + : intensityField + ? "intensity" + : "depth"; + + // Temporary full-size buffers; compacted at the end. + const tmpPositions = new Float32Array(count * 3); + const tmpColors = new Float32Array(count * 3); + const tmpDepthOrIntensity = colorMode === "rgb" || colorMode === "rgba_fields" ? null : new Float32Array(count); + const xyzContiguous = littleEndian && - pointStep % 4 === 0 && + point_step % 4 === 0 && xField.offset === 0 && yField.offset === 4 && zField.offset === 8 && @@ -119,102 +160,120 @@ function extractPositions( (yField.datatype === undefined || yField.datatype === POINT_FIELD_FLOAT32) && (zField.datatype === undefined || zField.datatype === POINT_FIELD_FLOAT32); - if (xyzContiguous) { - const src = new Float32Array(data.buffer, data.byteOffset, Math.floor(data.byteLength / 4)); - const floatsPerPoint = pointStep / 4; - for (let i = 0; i < count; i++) { + const srcFloats = xyzContiguous + ? new Float32Array(data.buffer, data.byteOffset, Math.floor(data.byteLength / 4)) + : null; + const floatsPerPoint = point_step / 4; + + let written = 0; + let depthMin = Number.POSITIVE_INFINITY; + let depthMax = Number.NEGATIVE_INFINITY; + + for (let i = 0; i < count; i++) { + let ox: number; + let oy: number; + let oz: number; + if (srcFloats) { const s = i * floatsPerPoint; - const d = i * 3; - positions[d] = src[s]!; - positions[d + 1] = src[s + 1]!; - positions[d + 2] = src[s + 2]!; + ox = srcFloats[s]!; + oy = srcFloats[s + 1]!; + oz = srcFloats[s + 2]!; + } else { + const offset = i * point_step; + ox = view.getFloat32(offset + xField.offset, littleEndian); + oy = view.getFloat32(offset + yField.offset, littleEndian); + oz = view.getFloat32(offset + zField.offset, littleEndian); } - return positions; - } - const view = new DataView(data.buffer, data.byteOffset, data.byteLength); - for (let i = 0; i < count; i++) { - const offset = i * pointStep; - const d = i * 3; - positions[d] = view.getFloat32(offset + xField.offset, littleEndian); - positions[d + 1] = view.getFloat32(offset + yField.offset, littleEndian); - positions[d + 2] = view.getFloat32(offset + zField.offset, littleEndian); - } - return positions; -} + if (!Number.isFinite(ox) || !Number.isFinite(oy) || !Number.isFinite(oz)) { + continue; + } -function extractColors( - view: DataView, - fields: PointField[], - count: number, - pointStep: number, - littleEndian: boolean, -): Float32Array | undefined { - const rgbField = fieldByName(fields, "rgb") ?? fieldByName(fields, "rgba"); - if (rgbField) { - const colors = new Float32Array(count * 3); - for (let i = 0; i < count; i++) { - const offset = i * pointStep + rgbField.offset; - const [r, g, b] = readPackedRgb(view, offset, littleEndian); - colors[i * 3] = r; - colors[i * 3 + 1] = g; - colors[i * 3 + 2] = b; + const [rx, ry, rz] = opticalToRos(ox, oy, oz); + const d = written * 3; + tmpPositions[d] = rx; + tmpPositions[d + 1] = ry; + tmpPositions[d + 2] = rz; + + if (colorMode === "rgb" && rgbField) { + const [cr, cg, cb] = readPackedRgb(view, i * point_step + rgbField.offset, littleEndian); + tmpColors[d] = cr; + tmpColors[d + 1] = cg; + tmpColors[d + 2] = cb; + } else if (colorMode === "rgba_fields" && rField && gField && bField) { + const base = i * point_step; + tmpColors[d] = view.getUint8(base + rField.offset) / 255; + tmpColors[d + 1] = view.getUint8(base + gField.offset) / 255; + tmpColors[d + 2] = view.getUint8(base + bField.offset) / 255; + } else if (tmpDepthOrIntensity) { + const value = + colorMode === "intensity" && intensityField + ? readNumericField( + view, + i * point_step + intensityField.offset, + intensityField.datatype, + littleEndian, + ) + : oz; // optical depth + tmpDepthOrIntensity[written] = value; + if (Number.isFinite(value)) { + if (value < depthMin) depthMin = value; + if (value > depthMax) depthMax = value; + } } - return colors; + + written++; } - const rField = fieldByName(fields, "r"); - const gField = fieldByName(fields, "g"); - const bField = fieldByName(fields, "b"); - if (rField && gField && bField) { - const colors = new Float32Array(count * 3); - for (let i = 0; i < count; i++) { - const base = i * pointStep; - colors[i * 3] = view.getUint8(base + rField.offset) / 255; - colors[i * 3 + 1] = view.getUint8(base + gField.offset) / 255; - colors[i * 3 + 2] = view.getUint8(base + bField.offset) / 255; - } - return colors; + if (written === 0) { + return { positions: new Float32Array(0) }; } - const intensityField = fieldByName(fields, "intensity"); - if (!intensityField) return undefined; + const positions = tmpPositions.slice(0, written * 3); + let colors: Float32Array | undefined; - let min = Number.POSITIVE_INFINITY; - let max = Number.NEGATIVE_INFINITY; - const raw = new Float32Array(count); - for (let i = 0; i < count; i++) { - const value = readNumericField( - view, - i * pointStep + intensityField.offset, - intensityField.datatype, - littleEndian, - ); - raw[i] = value; - if (value < min) min = value; - if (value > max) max = value; + if (colorMode === "rgb" || colorMode === "rgba_fields") { + colors = tmpColors.slice(0, written * 3); + } else if (tmpDepthOrIntensity) { + colors = new Float32Array(written * 3); + const range = depthMax - depthMin; + if (colorMode === "intensity") { + if (range <= 0 || !Number.isFinite(range)) { + colors.fill(0.5); + } else { + for (let i = 0; i < written; i++) { + const t = (tmpDepthOrIntensity[i]! - depthMin) / range; + const o = i * 3; + colors[o] = t; + colors[o + 1] = t; + colors[o + 2] = t; + } + } + } else { + // Depth turbo + if (range <= 0 || !Number.isFinite(range)) { + for (let i = 0; i < written; i++) { + sampleTurbo(0.5, colors, i * 3); + } + } else { + for (let i = 0; i < written; i++) { + const t = (tmpDepthOrIntensity[i]! - depthMin) / range; + sampleTurbo(t, colors, i * 3); + } + } + } } - const range = max - min; - const colors = new Float32Array(count * 3); - if (range <= 0 || !Number.isFinite(range)) { - // Constant intensity → mid-gray so points remain visible. - colors.fill(0.5); - return colors; - } + return colors ? { positions, colors } : { positions }; +} - // UINT8 intensity is already 0–255; still normalize by observed min/max so - // sparse clouds with a narrow band stay high-contrast. - for (let i = 0; i < count; i++) { - const t = (raw[i]! - min) / range; - colors[i * 3] = t; - colors[i * 3 + 1] = t; - colors[i * 3 + 2] = t; - } - return colors; +/** Copy bytes into a plain ArrayBuffer safe to put on a Worker transfer list. */ +export function copyToTransferableArrayBuffer(data: Uint8Array): ArrayBuffer { + const payload = new ArrayBuffer(data.byteLength); + new Uint8Array(payload).set(data); + return payload; } -// Re-export datatype constants for tests. export const PointFieldDatatype = { UINT8: POINT_FIELD_UINT8, FLOAT32: POINT_FIELD_FLOAT32, From b770b89d4f486da9f745fb37f1a615eb2b19bc11 Mon Sep 17 00:00:00 2001 From: joaner Date: Fri, 10 Jul 2026 18:36:20 +0800 Subject: [PATCH 3/5] perf: reuse PointCloud GPU buffers across sparse depth frames MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Size attributes to width*height and draw with setDrawRange so fluctuating valid-point counts no longer rebuild geometry. Apply optical→ROS only for optical/depth topics, drop non-positive optical depth, and keep the parse worker stable via camera/controls refs. --- src/features/panels/ThreeD/ThreeDPanel.tsx | 114 +++++++++++----- .../ThreeD/core/PointCloudParse.worker.ts | 22 +-- .../ThreeD/core/pointCloudWorkerProtocol.ts | 6 + src/shared/utils/pointCloud.test.ts | 65 ++++++--- src/shared/utils/pointCloud.ts | 127 +++++++++++++----- 5 files changed, 241 insertions(+), 93 deletions(-) diff --git a/src/features/panels/ThreeD/ThreeDPanel.tsx b/src/features/panels/ThreeD/ThreeDPanel.tsx index 7c3df52..9d67ef9 100644 --- a/src/features/panels/ThreeD/ThreeDPanel.tsx +++ b/src/features/panels/ThreeD/ThreeDPanel.tsx @@ -332,7 +332,9 @@ type PointCloudGpuState = { geometry: THREE.BufferGeometry; position: THREE.BufferAttribute; color: THREE.BufferAttribute | null; - pointCount: number; + /** Allocated point capacity (may exceed current drawCount). */ + capacity: number; + drawCount: number; }; function disposePointCloudGpu(state: PointCloudGpuState | null): void { @@ -348,25 +350,35 @@ function safeComputeBoundingSphere(geometry: THREE.BufferGeometry): void { } } +function setAttributeDrawCount(attribute: THREE.BufferAttribute, count: number): void { + // Three.js typings mark `count` readonly; runtime still honors updates used with setDrawRange. + (attribute as THREE.BufferAttribute & { count: number }).count = count; +} + function applyPointCloudData( prev: PointCloudGpuState | null, data: PointCloudData, - options?: { takeOwnership?: boolean }, ): PointCloudGpuState { - const pointCount = data.positions.length / 3; - const hasColors = data.colors != undefined && data.colors.length === data.positions.length; - const takeOwnership = options?.takeOwnership === true; - - if (prev && prev.pointCount === pointCount) { - prev.position.array.set(data.positions); + const count = data.count; + const hasColors = data.colors != undefined && data.colors.length >= count * 3; + const desiredCapacity = Math.max(data.maxPoints ?? count, count, 1); + + // Reuse GPU buffers whenever the new cloud fits — valid-point count often + // fluctuates on is_dense=false depth clouds; avoid rebuild every frame. + if (prev && count <= prev.capacity) { + prev.position.array.set(data.positions.subarray(0, count * 3)); + setAttributeDrawCount(prev.position, count); prev.position.needsUpdate = true; if (hasColors) { if (prev.color) { - prev.color.array.set(data.colors!); + prev.color.array.set(data.colors!.subarray(0, count * 3)); + setAttributeDrawCount(prev.color, count); prev.color.needsUpdate = true; } else { - const colorArray = takeOwnership ? data.colors! : data.colors!.slice(); + const colorArray = new Float32Array(prev.capacity * 3); + colorArray.set(data.colors!.subarray(0, count * 3)); const colorAttr = new THREE.BufferAttribute(colorArray, 3); + setAttributeDrawCount(colorAttr, count); colorAttr.setUsage(THREE.DynamicDrawUsage); prev.geometry.setAttribute('color', colorAttr); prev.color = colorAttr; @@ -375,26 +387,41 @@ function applyPointCloudData( prev.geometry.deleteAttribute('color'); prev.color = null; } + prev.geometry.setDrawRange(0, count); + prev.drawCount = count; return prev; } disposePointCloudGpu(prev); + const capacity = desiredCapacity; const geometry = new THREE.BufferGeometry(); - const positionArray = takeOwnership ? data.positions : data.positions.slice(); + + const positionArray = new Float32Array(capacity * 3); + if (count > 0) { + positionArray.set(data.positions.subarray(0, count * 3)); + } const position = new THREE.BufferAttribute(positionArray, 3); + setAttributeDrawCount(position, count); position.setUsage(THREE.DynamicDrawUsage); geometry.setAttribute('position', position); + let color: THREE.BufferAttribute | null = null; - if (hasColors) { - const colorArray = takeOwnership ? data.colors! : data.colors!.slice(); + if (hasColors && count > 0) { + const colorArray = new Float32Array(capacity * 3); + colorArray.set(data.colors!.subarray(0, count * 3)); color = new THREE.BufferAttribute(colorArray, 3); + setAttributeDrawCount(color, count); color.setUsage(THREE.DynamicDrawUsage); geometry.setAttribute('color', color); } - // Compute once; frustumCulled is false so a stale sphere cannot hide a - // moving cloud. Guard against NaN from sparse/invalid depth clouds. - safeComputeBoundingSphere(geometry); - return { geometry, position, color, pointCount }; + + geometry.setDrawRange(0, count); + if (count > 0) { + safeComputeBoundingSphere(geometry); + } else { + geometry.boundingSphere = new THREE.Sphere(new THREE.Vector3(0, 0, 0), 1); + } + return { geometry, position, color, capacity, drawCount: count }; } /** Low-frequency clouds (LaserScan / OccupancyGrid) that arrive via React props. */ @@ -454,6 +481,9 @@ const LivePointCloudLayer = ({ const [geometry, setGeometry] = useState(null); const [hasVertexColors, setHasVertexColors] = useState(false); const { invalidate, camera, controls } = useThree(); + const cameraRef = useRef(camera); + const controlsRef = useRef(controls); + const invalidateRef = useRef(invalidate); const geometryIdentityRef = useRef(null); const hasColorsRef = useRef(false); const workerRef = useRef(null); @@ -462,6 +492,10 @@ const LivePointCloudLayer = ({ const pendingJobRef = useRef(null); const didAutofitRef = useRef(false); + cameraRef.current = camera; + controlsRef.current = controls; + invalidateRef.current = invalidate; + useEffect(() => { didAutofitRef.current = false; }, [topic]); @@ -472,7 +506,8 @@ const LivePointCloudLayer = ({ const autofitOnce = (geo: THREE.BufferGeometry) => { if (didAutofitRef.current) return; - if (!(camera instanceof THREE.PerspectiveCamera)) return; + const cam = cameraRef.current; + if (!(cam instanceof THREE.PerspectiveCamera)) return; safeComputeBoundingSphere(geo); const sphere = geo.boundingSphere; if (!sphere || !Number.isFinite(sphere.radius) || sphere.radius <= 0) return; @@ -480,8 +515,8 @@ const LivePointCloudLayer = ({ geo.getAttribute('position') as THREE.BufferAttribute, ); if (box.isEmpty()) return; - const target = framePerspectiveCameraToBox(camera, box); - const oc = controls as ThreeOrbitControls | null; + const target = framePerspectiveCameraToBox(cam, box); + const oc = controlsRef.current as ThreeOrbitControls | null; if (oc) { oc.target.copy(target); oc.update(); @@ -489,10 +524,20 @@ const LivePointCloudLayer = ({ didAutofitRef.current = true; }; - const applyParsed = (positions: Float32Array, colors: Float32Array | undefined) => { - if (positions.length === 0) return; - const parsed: PointCloudData = colors ? { positions, colors } : { positions }; - const next = applyPointCloudData(gpuRef.current, parsed, { takeOwnership: true }); + const applyParsed = ( + positions: Float32Array, + colors: Float32Array | undefined, + pointCount: number, + maxPoints: number, + ) => { + if (pointCount <= 0) return; + const parsed: PointCloudData = { + positions, + colors, + count: pointCount, + maxPoints, + }; + const next = applyPointCloudData(gpuRef.current, parsed); gpuRef.current = next; const colorsChanged = (next.color != null) !== hasColorsRef.current; const geometryChanged = next.geometry !== geometryIdentityRef.current; @@ -503,7 +548,7 @@ const LivePointCloudLayer = ({ setHasVertexColors(next.color != null); } autofitOnce(next.geometry); - invalidate(); + invalidateRef.current(); }; const flushPending = () => { @@ -526,7 +571,7 @@ const LivePointCloudLayer = ({ if (response.type === 'parsed') { const positions = new Float32Array(response.positions); const colors = response.colors ? new Float32Array(response.colors) : undefined; - applyParsed(positions, colors); + applyParsed(positions, colors, response.pointCount, response.maxPoints); } flushPending(); }; @@ -569,7 +614,12 @@ const LivePointCloudLayer = ({ cachedFields = fields; } - // SAB views cannot be transferred — always copy into a plain ArrayBuffer. + const header = record.header; + const frameId = + header && typeof header === 'object' && typeof (header as Record).frame_id === 'string' + ? ((header as Record).frame_id as string) + : undefined; + const payload = copyToTransferableArrayBuffer(data); const id = nextIdRef.current++; const job: PointCloudParseRequest = { @@ -580,6 +630,8 @@ const LivePointCloudLayer = ({ width: record.width, height: record.height, isBigendian: record.is_bigendian === true, + topic, + frameId, data: payload, }; pendingJobRef.current = job; @@ -605,7 +657,7 @@ const LivePointCloudLayer = ({ setGeometry(null); setHasVertexColors(false); }; - }, [player, panelId, topic, invalidate, camera, controls]); + }, [player, panelId, topic]); if (!geometry) return null; return ( @@ -976,7 +1028,8 @@ function extractLaserScanPoints(message: unknown): PointCloudData | null { positions.push(Math.cos(angle) * range, Math.sin(angle) * range, 0); } if (positions.length === 0) return null; - return { positions: new Float32Array(positions) }; + const positionsArray = new Float32Array(positions); + return { positions: positionsArray, count: positionsArray.length / 3 }; } function extractOccupancyGridPoints(message: unknown): PointCloudData | null { @@ -1001,7 +1054,8 @@ function extractOccupancyGridPoints(message: unknown): PointCloudData | null { } } if (positions.length === 0) return null; - return { positions: new Float32Array(positions) }; + const positionsArray = new Float32Array(positions); + return { positions: positionsArray, count: positionsArray.length / 3 }; } // ── URDF mesh resolution (unchanged) ────────────────────────────── diff --git a/src/features/panels/ThreeD/core/PointCloudParse.worker.ts b/src/features/panels/ThreeD/core/PointCloudParse.worker.ts index f8a7bb2..ce7a0cf 100644 --- a/src/features/panels/ThreeD/core/PointCloudParse.worker.ts +++ b/src/features/panels/ThreeD/core/PointCloudParse.worker.ts @@ -21,14 +21,17 @@ self.onmessage = (event: MessageEvent) => { } try { - const parsed = parsePointCloud2({ - fields: req.fields, - data: new Uint8Array(req.data), - point_step: req.pointStep, - width: req.width, - height: req.height, - is_bigendian: req.isBigendian, - }); + const parsed = parsePointCloud2( + { + fields: req.fields, + data: new Uint8Array(req.data), + point_step: req.pointStep, + width: req.width, + height: req.height, + is_bigendian: req.isBigendian, + }, + { topic: req.topic, frameId: req.frameId }, + ); if (!parsed) { const failure: PointCloudParseResponse = { @@ -45,7 +48,8 @@ self.onmessage = (event: MessageEvent) => { const success: PointCloudParseSuccess = { type: 'parsed', id: req.id, - pointCount: parsed.positions.length / 3, + pointCount: parsed.count, + maxPoints: parsed.maxPoints ?? req.width * req.height, positions: positionsBuffer, }; if (parsed.colors) { diff --git a/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts b/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts index 8023b07..ef3d335 100644 --- a/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts +++ b/src/features/panels/ThreeD/core/pointCloudWorkerProtocol.ts @@ -15,6 +15,10 @@ export type PointCloudParseRequest = { width: number; height: number; isBigendian: boolean; + /** Topic name for optical-frame heuristic. */ + topic?: string; + /** `header.frame_id` when present. */ + frameId?: string; /** Raw PointCloud2 `data` bytes (transferred). */ data: ArrayBuffer; }; @@ -23,6 +27,8 @@ export type PointCloudParseSuccess = { type: 'parsed'; id: number; pointCount: number; + /** width*height — GPU buffer capacity hint. */ + maxPoints: number; positions: ArrayBuffer; colors?: ArrayBuffer; }; diff --git a/src/shared/utils/pointCloud.test.ts b/src/shared/utils/pointCloud.test.ts index 7969cb1..8258156 100644 --- a/src/shared/utils/pointCloud.test.ts +++ b/src/shared/utils/pointCloud.test.ts @@ -5,6 +5,7 @@ import { parsePointCloud2, PointFieldDatatype, sampleTurbo, + shouldTreatAsOpticalFrame, } from './pointCloud'; function packPclRgb(r: number, g: number, b: number): number { @@ -19,6 +20,7 @@ function buildCloud(options: { pointStep: number; points: Array<(view: DataView, offset: number) => void>; isBigendian?: boolean; + frameId?: string; }) { const width = options.points.length; const height = 1; @@ -28,6 +30,7 @@ function buildCloud(options: { options.points[i]!(view, i * options.pointStep); } return { + header: options.frameId ? { frame_id: options.frameId } : undefined, fields: options.fields, data, point_step: options.pointStep, @@ -43,6 +46,14 @@ const XYZ_FIELDS = [ { name: 'z', offset: 8, datatype: PointFieldDatatype.FLOAT32 }, ]; +describe('shouldTreatAsOpticalFrame', () => { + it('detects optical frame_id and depth/points topics', () => { + expect(shouldTreatAsOpticalFrame('camera_depth_optical_frame')).toBe(true); + expect(shouldTreatAsOpticalFrame(undefined, '/orbbec_free/depth/points')).toBe(true); + expect(shouldTreatAsOpticalFrame('map', '/lidar/points')).toBe(false); + }); +}); + describe('opticalToRos', () => { it('maps optical Z-forward Y-down to ROS X-forward Z-up', () => { expect(opticalToRos(1, 2, 3)).toEqual([3, -1, -2]); @@ -62,10 +73,11 @@ describe('copyToTransferableArrayBuffer', () => { }); describe('parsePointCloud2', () => { - it('converts optical xyz to ROS and colorizes by depth turbo when no color fields', () => { + it('converts optical xyz to ROS and colorizes by depth turbo for depth topics', () => { const message = buildCloud({ fields: XYZ_FIELDS, pointStep: 16, + frameId: 'orbbec_depth_optical_frame', points: [ (view, offset) => { view.setFloat32(offset, 1, true); @@ -80,29 +92,50 @@ describe('parsePointCloud2', () => { ], }); - const parsed = parsePointCloud2(message); + const parsed = parsePointCloud2(message, { topic: '/orbbec_free/depth/points' }); expect(parsed).not.toBeNull(); - // optical (1,2,3) → ROS (3,-1,-2); optical (0,0,10) → ROS (10,0,0) + expect(parsed!.count).toBe(2); + expect(parsed!.maxPoints).toBe(2); expect(Array.from(parsed!.positions.subarray(0, 3))).toEqual([3, -1, -2]); expect(parsed!.positions[3]).toBeCloseTo(10); - expect(parsed!.positions[4]).toBeCloseTo(0); - expect(parsed!.positions[5]).toBeCloseTo(0); expect(parsed!.colors).toBeDefined(); - expect(parsed!.colors!.length).toBe(6); - // Near (depth 3) vs far (depth 10) should differ under turbo. expect(parsed!.colors![0]).not.toBeCloseTo(parsed!.colors![3]!, 2); }); - it('drops non-finite points', () => { + it('does not optical-transform map-frame clouds', () => { + const message = buildCloud({ + fields: XYZ_FIELDS, + pointStep: 16, + frameId: 'map', + points: [ + (view, offset) => { + view.setFloat32(offset, 1, true); + view.setFloat32(offset + 4, 2, true); + view.setFloat32(offset + 8, 3, true); + }, + ], + }); + + const parsed = parsePointCloud2(message, { topic: '/map/points', frameId: 'map' }); + expect(Array.from(parsed!.positions)).toEqual([1, 2, 3]); + }); + + it('drops non-finite and non-positive optical depth', () => { const message = buildCloud({ fields: XYZ_FIELDS, pointStep: 16, + frameId: 'camera_optical_frame', points: [ (view, offset) => { view.setFloat32(offset, Number.NaN, true); view.setFloat32(offset + 4, 0, true); view.setFloat32(offset + 8, 1, true); }, + (view, offset) => { + view.setFloat32(offset, 0, true); + view.setFloat32(offset + 4, 0, true); + view.setFloat32(offset + 8, 0, true); // invalid depth + }, (view, offset) => { view.setFloat32(offset, 0, true); view.setFloat32(offset + 4, 0, true); @@ -111,11 +144,9 @@ describe('parsePointCloud2', () => { ], }); - const parsed = parsePointCloud2(message); - expect(parsed!.positions.length).toBe(3); + const parsed = parsePointCloud2(message, { frameId: 'camera_optical_frame' }); + expect(parsed!.count).toBe(1); expect(parsed!.positions[0]).toBeCloseTo(2); - expect(parsed!.positions[1]).toBeCloseTo(0); - expect(parsed!.positions[2]).toBeCloseTo(0); }); it('parses PCL packed rgb float32 field', () => { @@ -136,7 +167,7 @@ describe('parsePointCloud2', () => { ], }); - const parsed = parsePointCloud2(message); + const parsed = parsePointCloud2(message, { frameId: 'map' }); expect(parsed?.colors).toBeDefined(); expect(parsed!.colors![0]).toBeCloseTo(1); expect(parsed!.colors![1]).toBeCloseTo(128 / 255); @@ -164,7 +195,7 @@ describe('parsePointCloud2', () => { ], }); - const parsed = parsePointCloud2(message); + const parsed = parsePointCloud2(message, { frameId: 'map' }); expect(parsed?.colors).toBeDefined(); expect(parsed!.colors![0]).toBeCloseTo(0); expect(parsed!.colors![1]).toBeCloseTo(1); @@ -194,14 +225,10 @@ describe('parsePointCloud2', () => { ], }); - const parsed = parsePointCloud2(message); + const parsed = parsePointCloud2(message, { frameId: 'map' }); expect(parsed?.colors).toBeDefined(); expect(parsed!.colors![0]).toBeCloseTo(0); - expect(parsed!.colors![1]).toBeCloseTo(0); - expect(parsed!.colors![2]).toBeCloseTo(0); expect(parsed!.colors![3]).toBeCloseTo(1); - expect(parsed!.colors![4]).toBeCloseTo(1); - expect(parsed!.colors![5]).toBeCloseTo(1); }); it('returns null for incomplete messages', () => { diff --git a/src/shared/utils/pointCloud.ts b/src/shared/utils/pointCloud.ts index 9affc8a..7e1a1ac 100644 --- a/src/shared/utils/pointCloud.ts +++ b/src/shared/utils/pointCloud.ts @@ -1,8 +1,23 @@ export interface PointCloudData { + /** Densely packed valid points (length === count * 3). */ positions: Float32Array; colors?: Float32Array; + /** Number of valid points in `positions` / `colors`. */ + count: number; + /** + * Preferred GPU buffer capacity (usually width*height). Lets draw-range + * reuse survive per-frame valid-count jitter on sparse depth clouds. + */ + maxPoints?: number; } +export type ParsePointCloud2Options = { + /** Topic name — used with frame_id to decide optical-frame treatment. */ + topic?: string; + /** `header.frame_id` when available. */ + frameId?: string; +}; + interface PointField { name: string; offset: number; @@ -13,6 +28,9 @@ interface PointField { const POINT_FIELD_UINT8 = 2; const POINT_FIELD_FLOAT32 = 7; +/** Depth <= this (meters) is treated as invalid in optical clouds. */ +const OPTICAL_MIN_DEPTH_M = 1e-4; + function isPointField(value: unknown): value is PointField { if (!value || typeof value !== "object") return false; const o = value as Record; @@ -33,7 +51,6 @@ function readPackedRgb( littleEndian: boolean, ): [number, number, number] { const packed = view.getUint32(offset, littleEndian); - // PCL packs float32 bit-pattern as 0xAARRGGBB (rgba) or 0x00RRGGBB (rgb). const r = ((packed >> 16) & 0xff) / 255; const g = ((packed >> 8) & 0xff) / 255; const b = (packed & 0xff) / 255; @@ -57,7 +74,29 @@ export function opticalToRos(ox: number, oy: number, oz: number): [number, numbe return [oz, -ox, -oy]; } -// ── Turbo (same polynomial as Image panel; compact for point clouds) ───────── +/** + * Treat as camera optical frame when frame_id says so (REP-103), or when the + * topic looks like a depth PointCloud2 (`…/depth/…/points`). Map/base_link + * clouds are left unchanged. + */ +export function shouldTreatAsOpticalFrame(frameId?: string, topic?: string): boolean { + if (frameId && /optical/i.test(frameId)) { + return true; + } + if (topic && /(?:^|\/)depth(?:\/|$)/i.test(topic) && /points/i.test(topic)) { + return true; + } + return false; +} + +export function readPointCloudFrameId(message: Record): string | undefined { + const header = message.header; + if (!header || typeof header !== "object") return undefined; + const frameId = (header as Record).frame_id; + return typeof frameId === "string" ? frameId : undefined; +} + +// ── Turbo (same polynomial as Image panel) ─────────────────────────────────── const kRedVec4 = [0.13572138, 4.6153926, -42.66032258, 132.13108234] as const; const kGreenVec4 = [0.09140261, 2.19418839, 4.84296658, -14.18503333] as const; @@ -70,7 +109,6 @@ function clamp01(x: number): number { return Math.max(0, Math.min(1, x)); } -/** Sample turbo colormap at pct ∈ [0,1] into rgb out[outOffset..+2]. */ export function sampleTurbo(pct: number, out: Float32Array, outOffset: number): void { const x = clamp01(pct) * 0.99 + 0.01; const x2 = x * x; @@ -106,11 +144,14 @@ export function sampleTurbo(pct: number, out: Float32Array, outOffset: number): /** * Parse a `sensor_msgs/PointCloud2` message into GPU-friendly typed arrays. * - * - Drops non-finite xyz (common when `is_dense=false`). - * - Converts optical → ROS (X forward, Z up) for Z-up scene rendering. - * - Color priority: rgb/rgba → r/g/b → intensity → depth turbo (optical Z). + * - Drops non-finite xyz; in optical mode also drops depth <= 0. + * - Optical frames (heuristic) are converted to ROS Z-up. + * - Color priority: rgb/rgba → r/g/b → intensity → depth/range turbo. */ -export function parsePointCloud2(message: unknown): PointCloudData | null { +export function parsePointCloud2( + message: unknown, + options: ParsePointCloud2Options = {}, +): PointCloudData | null { if (!message || typeof message !== "object") return null; const m = message as Record; const { fields, data, point_step, width, height } = m; @@ -121,6 +162,9 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { if (count <= 0 || point_step <= 0) return null; if (data.byteLength < count * point_step) return null; + const frameId = options.frameId ?? readPointCloudFrameId(m); + const asOptical = shouldTreatAsOpticalFrame(frameId, options.topic); + const typedFields = fields.filter(isPointField); const xField = fieldByName(typedFields, "x"); const yField = fieldByName(typedFields, "y"); @@ -145,10 +189,9 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { ? "intensity" : "depth"; - // Temporary full-size buffers; compacted at the end. const tmpPositions = new Float32Array(count * 3); const tmpColors = new Float32Array(count * 3); - const tmpDepthOrIntensity = colorMode === "rgb" || colorMode === "rgba_fields" ? null : new Float32Array(count); + const tmpScalar = colorMode === "rgb" || colorMode === "rgba_fields" ? null : new Float32Array(count); const xyzContiguous = littleEndian && @@ -166,8 +209,8 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { const floatsPerPoint = point_step / 4; let written = 0; - let depthMin = Number.POSITIVE_INFINITY; - let depthMax = Number.NEGATIVE_INFINITY; + let scalarMin = Number.POSITIVE_INFINITY; + let scalarMax = Number.NEGATIVE_INFINITY; for (let i = 0; i < count; i++) { let ox: number; @@ -188,12 +231,26 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { if (!Number.isFinite(ox) || !Number.isFinite(oy) || !Number.isFinite(oz)) { continue; } + // Optical depth clouds commonly encode invalid pixels as z<=0. + if (asOptical && oz <= OPTICAL_MIN_DEPTH_M) { + continue; + } + + let px: number; + let py: number; + let pz: number; + if (asOptical) { + [px, py, pz] = opticalToRos(ox, oy, oz); + } else { + px = ox; + py = oy; + pz = oz; + } - const [rx, ry, rz] = opticalToRos(ox, oy, oz); const d = written * 3; - tmpPositions[d] = rx; - tmpPositions[d + 1] = ry; - tmpPositions[d + 2] = rz; + tmpPositions[d] = px; + tmpPositions[d + 1] = py; + tmpPositions[d + 2] = pz; if (colorMode === "rgb" && rgbField) { const [cr, cg, cb] = readPackedRgb(view, i * point_step + rgbField.offset, littleEndian); @@ -205,7 +262,7 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { tmpColors[d] = view.getUint8(base + rField.offset) / 255; tmpColors[d + 1] = view.getUint8(base + gField.offset) / 255; tmpColors[d + 2] = view.getUint8(base + bField.offset) / 255; - } else if (tmpDepthOrIntensity) { + } else if (tmpScalar) { const value = colorMode === "intensity" && intensityField ? readNumericField( @@ -214,11 +271,13 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { intensityField.datatype, littleEndian, ) - : oz; // optical depth - tmpDepthOrIntensity[written] = value; + : asOptical + ? oz + : Math.hypot(ox, oy, oz); + tmpScalar[written] = value; if (Number.isFinite(value)) { - if (value < depthMin) depthMin = value; - if (value > depthMax) depthMax = value; + if (value < scalarMin) scalarMin = value; + if (value > scalarMax) scalarMax = value; } } @@ -226,7 +285,7 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { } if (written === 0) { - return { positions: new Float32Array(0) }; + return { positions: new Float32Array(0), count: 0, maxPoints: count }; } const positions = tmpPositions.slice(0, written * 3); @@ -234,37 +293,35 @@ export function parsePointCloud2(message: unknown): PointCloudData | null { if (colorMode === "rgb" || colorMode === "rgba_fields") { colors = tmpColors.slice(0, written * 3); - } else if (tmpDepthOrIntensity) { + } else if (tmpScalar) { colors = new Float32Array(written * 3); - const range = depthMax - depthMin; + const range = scalarMax - scalarMin; if (colorMode === "intensity") { if (range <= 0 || !Number.isFinite(range)) { colors.fill(0.5); } else { for (let i = 0; i < written; i++) { - const t = (tmpDepthOrIntensity[i]! - depthMin) / range; + const t = (tmpScalar[i]! - scalarMin) / range; const o = i * 3; colors[o] = t; colors[o + 1] = t; colors[o + 2] = t; } } + } else if (range <= 0 || !Number.isFinite(range)) { + for (let i = 0; i < written; i++) { + sampleTurbo(0.5, colors, i * 3); + } } else { - // Depth turbo - if (range <= 0 || !Number.isFinite(range)) { - for (let i = 0; i < written; i++) { - sampleTurbo(0.5, colors, i * 3); - } - } else { - for (let i = 0; i < written; i++) { - const t = (tmpDepthOrIntensity[i]! - depthMin) / range; - sampleTurbo(t, colors, i * 3); - } + for (let i = 0; i < written; i++) { + sampleTurbo((tmpScalar[i]! - scalarMin) / range, colors, i * 3); } } } - return colors ? { positions, colors } : { positions }; + return colors + ? { positions, colors, count: written, maxPoints: count } + : { positions, count: written, maxPoints: count }; } /** Copy bytes into a plain ArrayBuffer safe to put on a Worker transfer list. */ From 7450a8f8c918fb54a27688705048ed5bf211ee96 Mon Sep 17 00:00:00 2001 From: joaner Date: Fri, 10 Jul 2026 18:44:24 +0800 Subject: [PATCH 4/5] fix: keep remote buffer bar truthful after LRU eviction Report all cached playable time ranges instead of only a contiguous prefix from file start, so mid-file cache no longer collapses the UI bar. --- src/infra/workers/mcap.worker.ts | 48 +++------ src/infra/workers/playableTimeRanges.test.ts | 101 +++++++++++++++++++ src/infra/workers/playableTimeRanges.ts | 73 ++++++++++++++ 3 files changed, 186 insertions(+), 36 deletions(-) create mode 100644 src/infra/workers/playableTimeRanges.test.ts create mode 100644 src/infra/workers/playableTimeRanges.ts diff --git a/src/infra/workers/mcap.worker.ts b/src/infra/workers/mcap.worker.ts index d96e39e..817b687 100644 --- a/src/infra/workers/mcap.worker.ts +++ b/src/infra/workers/mcap.worker.ts @@ -27,12 +27,13 @@ import type { Range } from '@/shared/utils/ranges'; import { compactTimeRanges, inferProgressTimeRangeCompaction } from '@/shared/utils/timeRanges'; import { workerPerf } from './workerPerf'; import { DataQualityScanController } from './dataQualityScanController'; -type IndexedChunkCoverage = { - byteRange: Range; - timeRange: TimeRange; - startNs: bigint; - endNs: bigint; -}; +import { + getPlayableTimeRanges, + isByteRangeCovered, + type ChunkCoverage, +} from './playableTimeRanges'; + +type IndexedChunkCoverage = ChunkCoverage; const MIB = 1024 * 1024; const PREFETCH_CACHE_FRACTION = 0.75; @@ -43,10 +44,6 @@ const MAX_CONTIGUOUS_CHUNK_GAP_NS = 750_000_000n; const DEFAULT_PREFETCH_AHEAD_MS = 5_000; const PLAYBACK_CURSOR_BUFFER_AHEAD_MS = 1_500; -function isByteRangeCovered(query: Range, downloaded: readonly Range[]): boolean { - return downloaded.some((range) => range.start <= query.start && range.end >= query.end); -} - class McapWorkerImpl implements IWorkerSerializedSourceWorker { private _source?: McapIndexedIterableSource; private _cachedReadable?: CachedFilelike; @@ -223,7 +220,11 @@ class McapWorkerImpl implements IWorkerSerializedSourceWorker { const loadedBytes = downloadedByteRanges.reduce((sum, range) => sum + (range.end - range.start), 0); const totalBytes = this._totalBytes || (await this._cachedReadable.size()); const percent = totalBytes > 0 ? Math.min(100, (loadedBytes / totalBytes) * 100) : 0; - const coveredChunkRanges = this._getContiguousPlayableRanges(downloadedByteRanges); + const coveredChunkRanges = getPlayableTimeRanges( + this._chunkCoverage, + downloadedByteRanges, + MAX_CONTIGUOUS_CHUNK_GAP_NS, + ); const parsedMessageRanges = compactTimeRanges( coveredChunkRanges, inferProgressTimeRangeCompaction( @@ -356,31 +357,6 @@ class McapWorkerImpl implements IWorkerSerializedSourceWorker { return { byteRange: { start: byteStart, end: byteEnd }, endNs: includedEndNs }; } - private _getContiguousPlayableRanges(downloadedByteRanges: readonly Range[]): TimeRange[] { - const firstChunk = this._chunkCoverage[0]; - if (!firstChunk) { - return []; - } - - const ranges: TimeRange[] = []; - let endNs = firstChunk.startNs; - for (const chunk of this._chunkCoverage) { - if (chunk.startNs > endNs + MAX_CONTIGUOUS_CHUNK_GAP_NS) { - break; - } - if (!isByteRangeCovered(chunk.byteRange, downloadedByteRanges)) { - break; - } - - endNs = chunk.endNs > endNs ? chunk.endNs : endNs; - ranges[0] = { - start: { ...firstChunk.timeRange.start }, - end: fromNano(endNs), - }; - } - return ranges; - } - private _inferPrefetchTargetBytes(): number { const cacheBytes = this._remoteCacheBytes > 0 ? this._remoteCacheBytes : 512 * MIB; const target = Math.floor(cacheBytes * PREFETCH_CACHE_FRACTION); diff --git a/src/infra/workers/playableTimeRanges.test.ts b/src/infra/workers/playableTimeRanges.test.ts new file mode 100644 index 0000000..b515f4f --- /dev/null +++ b/src/infra/workers/playableTimeRanges.test.ts @@ -0,0 +1,101 @@ +import { describe, expect, it } from 'vitest'; +import { getPlayableTimeRanges, type ChunkCoverage } from './playableTimeRanges'; +import { toNano } from '@/shared/utils/time'; + +const GAP_NS = 750_000_000n; + +function chunk( + startSec: number, + endSec: number, + byteStart: number, + byteEnd: number, +): ChunkCoverage { + return { + startNs: BigInt(startSec) * 1_000_000_000n, + endNs: BigInt(endSec) * 1_000_000_000n, + timeRange: { + start: { sec: startSec, nsec: 0 }, + end: { sec: endSec, nsec: 0 }, + }, + byteRange: { start: byteStart, end: byteEnd }, + }; +} + +describe('getPlayableTimeRanges', () => { + it('returns empty when there is no chunk coverage', () => { + expect(getPlayableTimeRanges([], [{ start: 0, end: 100 }], GAP_NS)).toEqual([]); + }); + + it('returns a single prefix when early chunks are continuously covered', () => { + const chunks = [chunk(0, 1, 0, 10), chunk(1, 2, 10, 20), chunk(2, 3, 20, 30)]; + const ranges = getPlayableTimeRanges(chunks, [{ start: 0, end: 20 }], GAP_NS); + expect(ranges).toHaveLength(1); + expect(toNano(ranges[0].start)).toBe(0n); + expect(toNano(ranges[0].end)).toBe(2_000_000_000n); + }); + + it('returns the mid-file segment when early chunks were evicted', () => { + const chunks = [chunk(0, 1, 0, 10), chunk(1, 2, 10, 20), chunk(2, 3, 20, 30)]; + // Only mid/late bytes remain in cache (LRU evicted the start). + const ranges = getPlayableTimeRanges(chunks, [{ start: 10, end: 30 }], GAP_NS); + expect(ranges).toHaveLength(1); + expect(toNano(ranges[0].start)).toBe(1_000_000_000n); + expect(toNano(ranges[0].end)).toBe(3_000_000_000n); + }); + + it('returns multiple segments for disjoint downloaded ranges', () => { + const chunks = [ + chunk(0, 1, 0, 10), + chunk(1, 2, 10, 20), + chunk(2, 3, 20, 30), + chunk(3, 4, 30, 40), + ]; + const ranges = getPlayableTimeRanges( + chunks, + [ + { start: 0, end: 10 }, + { start: 30, end: 40 }, + ], + GAP_NS, + ); + expect(ranges).toHaveLength(2); + expect(toNano(ranges[0].start)).toBe(0n); + expect(toNano(ranges[0].end)).toBe(1_000_000_000n); + expect(toNano(ranges[1].start)).toBe(3_000_000_000n); + expect(toNano(ranges[1].end)).toBe(4_000_000_000n); + }); + + it('splits segments when the time gap exceeds the contiguous threshold', () => { + // 2s gap between chunk 0 end and chunk 1 start (> 750ms). + const chunks = [chunk(0, 1, 0, 10), chunk(3, 4, 10, 20)]; + const ranges = getPlayableTimeRanges(chunks, [{ start: 0, end: 20 }], GAP_NS); + expect(ranges).toHaveLength(2); + expect(toNano(ranges[0].start)).toBe(0n); + expect(toNano(ranges[0].end)).toBe(1_000_000_000n); + expect(toNano(ranges[1].start)).toBe(3_000_000_000n); + expect(toNano(ranges[1].end)).toBe(4_000_000_000n); + }); + + it('merges adjacent covered chunks within the gap threshold', () => { + // 0.5s gap (< 750ms) should still merge. + const chunks = [ + { + ...chunk(0, 1, 0, 10), + endNs: 1_000_000_000n, + }, + { + ...chunk(1, 2, 10, 20), + startNs: 1_500_000_000n, + endNs: 2_000_000_000n, + timeRange: { + start: { sec: 1, nsec: 500_000_000 }, + end: { sec: 2, nsec: 0 }, + }, + }, + ]; + const ranges = getPlayableTimeRanges(chunks, [{ start: 0, end: 20 }], GAP_NS); + expect(ranges).toHaveLength(1); + expect(toNano(ranges[0].start)).toBe(0n); + expect(toNano(ranges[0].end)).toBe(2_000_000_000n); + }); +}); diff --git a/src/infra/workers/playableTimeRanges.ts b/src/infra/workers/playableTimeRanges.ts new file mode 100644 index 0000000..c4110bd --- /dev/null +++ b/src/infra/workers/playableTimeRanges.ts @@ -0,0 +1,73 @@ +import type { TimeRange } from '@/core/types/ros'; +import type { Range } from '@/shared/utils/ranges'; +import { fromNano } from '@/shared/utils/time'; + +export type ChunkCoverage = { + byteRange: Range; + timeRange: TimeRange; + startNs: bigint; + endNs: bigint; +}; + +export function isByteRangeCovered(query: Range, downloaded: readonly Range[]): boolean { + return downloaded.some((range) => range.start <= query.start && range.end >= query.end); +} + +/** + * Map currently-downloaded byte ranges onto chunk time coverage. + * Returns every contiguous playable segment (not only a prefix from file start), + * so LRU eviction of early chunks does not collapse the buffer bar to empty. + */ +export function getPlayableTimeRanges( + chunkCoverage: readonly ChunkCoverage[], + downloadedByteRanges: readonly Range[], + maxContiguousGapNs: bigint, +): TimeRange[] { + if (chunkCoverage.length === 0) { + return []; + } + + const ranges: TimeRange[] = []; + let segmentStart: TimeRange['start'] | undefined; + let segmentEndNs: bigint | undefined; + + const flush = () => { + if (segmentStart == undefined || segmentEndNs == undefined) { + return; + } + ranges.push({ + start: { ...segmentStart }, + end: fromNano(segmentEndNs), + }); + segmentStart = undefined; + segmentEndNs = undefined; + }; + + for (const chunk of chunkCoverage) { + const covered = isByteRangeCovered(chunk.byteRange, downloadedByteRanges); + if (!covered) { + flush(); + continue; + } + + if (segmentStart == undefined || segmentEndNs == undefined) { + segmentStart = chunk.timeRange.start; + segmentEndNs = chunk.endNs; + continue; + } + + if (chunk.startNs > segmentEndNs + maxContiguousGapNs) { + flush(); + segmentStart = chunk.timeRange.start; + segmentEndNs = chunk.endNs; + continue; + } + + if (chunk.endNs > segmentEndNs) { + segmentEndNs = chunk.endNs; + } + } + + flush(); + return ranges; +} From 6aaf9748e2ebb28070ba3e46b1426efab5f97493 Mon Sep 17 00:00:00 2001 From: joaner Date: Fri, 10 Jul 2026 18:46:16 +0800 Subject: [PATCH 5/5] chore: bump version to 1.7.2 --- package-lock.json | 4 ++-- package.json | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/package-lock.json b/package-lock.json index 2565d55..eb5c970 100644 --- a/package-lock.json +++ b/package-lock.json @@ -1,12 +1,12 @@ { "name": "@ioai/rosview", - "version": "1.7.1", + "version": "1.7.2", "lockfileVersion": 3, "requires": true, "packages": { "": { "name": "@ioai/rosview", - "version": "1.7.1", + "version": "1.7.2", "license": "MIT", "devDependencies": { "@eslint/js": "^9.39.4", diff --git a/package.json b/package.json index a24531f..f55c1eb 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "@ioai/rosview", - "version": "1.7.1", + "version": "1.7.2", "description": "High-performance robotics data visualization for MCAP, ROS bag, ROS2 db3, HDF5 and BVH — embeddable React component and standalone SPA", "keywords": [ "ros",