diff --git a/src/features/layout/autoLayout/applyDefaultRosDockLayout.test.ts b/src/features/layout/autoLayout/applyDefaultRosDockLayout.test.ts index 6365323..50fa687 100644 --- a/src/features/layout/autoLayout/applyDefaultRosDockLayout.test.ts +++ b/src/features/layout/autoLayout/applyDefaultRosDockLayout.test.ts @@ -115,7 +115,7 @@ describe('buildDefaultRosFoxgloveLayoutData', () => { expect(panelTypes.filter((type) => type === 'Image')).toHaveLength(3); }); - it('builds two image rows for six CompressedVideo streams without 3D', () => { + it('builds two image rows for six CompressedVideo streams and binds one scene mesh topic', () => { const topics: TopicInfo[] = [ { name: '/head/color/image', type: 'foxglove_msgs/msg/CompressedVideo [ros2msg]' }, { name: '/head/depth/image', type: 'foxglove_msgs/msg/CompressedVideo [ros2msg]' }, @@ -123,6 +123,7 @@ describe('buildDefaultRosFoxgloveLayoutData', () => { { name: '/left/depth/image', type: 'foxglove_msgs/msg/CompressedVideo [ros2msg]' }, { name: '/right/color/image', type: 'foxglove_msgs/msg/CompressedVideo [ros2msg]' }, { name: '/right/depth/image', type: 'foxglove_msgs/msg/CompressedVideo [ros2msg]' }, + { name: '/robot0/perception/mano/scene', type: 'foxglove.SceneUpdate' }, ]; const data = buildDefaultRosFoxgloveLayoutData(topics); const ids = collectMosaicPanelIds(data.layout); @@ -143,6 +144,17 @@ describe('buildDefaultRosFoxgloveLayoutData', () => { '/right/color/image', '/right/depth/image', ]); + + const imageConfigs = Object.values(data.configById) as Array<{ + topic?: string; + meshTopic?: string; + meshVisible?: boolean; + }>; + expect(imageConfigs).toHaveLength(6); + expect(imageConfigs.every((config) => config.meshTopic === '/robot0/perception/mano/scene')).toBe( + true, + ); + expect(imageConfigs.every((config) => config.meshVisible === true)).toBe(true); }); it('BVH-only dataset: single 3D panel only; dockview root still wraps to branch for fromJSON', () => { diff --git a/src/features/layout/autoLayout/applyDefaultRosDockLayout.ts b/src/features/layout/autoLayout/applyDefaultRosDockLayout.ts index fadb548..275120c 100644 --- a/src/features/layout/autoLayout/applyDefaultRosDockLayout.ts +++ b/src/features/layout/autoLayout/applyDefaultRosDockLayout.ts @@ -22,6 +22,7 @@ import { heuristicAudioInfoTopics } from '@/features/panels/Audio/core/resolveAu import { getPanelDefinition } from '@/features/panels/registry'; import { isAudioCommonInfoSchema, isJointStateSchema, isRawAudioSchema, isRosImageSchema, normalizeRosSchemaName } from '@/shared/ros/rosMessageTypes'; import { pickDefaultRawMessagesTopic } from '@/features/layout/autoLayout/pickDefaultRawMessagesTopic'; +import { isSceneUpdateSchema } from '@/features/panels/Image/core/sceneMesh'; function imageTabTitle(topic: string): string { const parts = topic.split('/').filter(Boolean); @@ -132,6 +133,7 @@ function appendFallbackRawMessagesPanel( function appendImagePanelsForRow( row: (string | null)[], configById: Record, + meshTopic?: string, ): string[] { const ids: string[] = []; for (const topic of row) { @@ -140,6 +142,7 @@ function appendImagePanelsForRow( ids.push(id); configById[id] = { topic, + ...(meshTopic ? { meshTopic, meshVisible: true } : {}), [FOXGLOVE_PANEL_TITLE_KEY]: imageTabTitle(topic), }; } @@ -237,6 +240,8 @@ export function buildDefaultRosFoxgloveLayoutData( const stackParts: FoxgloveMosaicNode[] = []; const usedImageTopics = new Set(); + const sceneMeshTopics = topics.filter((topic) => isSceneUpdateSchema(topic.type)); + const meshTopic = sceneMeshTopics.length === 1 ? sceneMeshTopics[0]?.name : undefined; const pickedImageTopics = selectImageTopicsForAutoLayout(topics); const hasDepthImageStreams = topics.some( @@ -245,8 +250,8 @@ export function buildDefaultRosFoxgloveLayoutData( if (hasDepthImageStreams) { const { colorRow, depthRow } = planColorDepthCameraRows(topics); - const colorImageIds = appendImagePanelsForRow(colorRow, configById); - const depthImageIds = appendImagePanelsForRow(depthRow, configById); + const colorImageIds = appendImagePanelsForRow(colorRow, configById, meshTopic); + const depthImageIds = appendImagePanelsForRow(depthRow, configById, meshTopic); for (const topic of colorRow) { if (topic) usedImageTopics.add(topic); } @@ -259,7 +264,7 @@ export function buildDefaultRosFoxgloveLayoutData( if (depthMosaic) stackParts.push(depthMosaic); } else if (pickedImageTopics.length > 0) { for (const row of buildImageRows(pickedImageTopics)) { - const imageIds = appendImagePanelsForRow(row, configById); + const imageIds = appendImagePanelsForRow(row, configById, meshTopic); for (const topic of row) { usedImageTopics.add(topic); } diff --git a/src/features/panels/Image/ImagePanel.tsx b/src/features/panels/Image/ImagePanel.tsx index 1a4510e..f1bf902 100644 --- a/src/features/panels/Image/ImagePanel.tsx +++ b/src/features/panels/Image/ImagePanel.tsx @@ -4,7 +4,7 @@ import type { Player } from '@/core/types/player'; import { useMessagePipeline } from '@/core/pipeline/useMessagePipeline'; import type { MessageEvent as RosMessageEvent } from '@/core/types/ros'; import { scheduleFrame } from '@/shared/utils/rafScheduler'; -import { toNano } from '@/shared/utils/time'; +import { addMs, toNano } from '@/shared/utils/time'; import type { RawImageDecodeOptions } from './core/imageColorMode'; import type { ImageRenderMetrics, @@ -19,6 +19,14 @@ import { import { repairH264Seek } from './core/h264SeekRepair'; import { isH264MessageEvent, toWorkerFrame } from './core/messageFrameAdapter'; import { applyDepthTopicPreset } from './core/depthColorDefaults'; +import { parseImageAnnotations } from './core/imageAnnotations'; +import { SceneMeshOverlayRenderer } from './core/SceneMeshOverlayRenderer'; +import { + inferCameraCalibrationTopic, + parseDoubleSphereCalibration, + parseSceneMeshes, +} from './core/sceneMesh'; +import { subscribeSceneMeshTopic } from './core/sceneMeshTopicBroker'; import type { ImageConfig } from './defaults'; import { TopicQuickPicker } from '../framework/TopicQuickPicker'; import { PanelTopicBar } from '../framework/PanelTopicBar'; @@ -54,6 +62,10 @@ export const ImagePanel: React.FC = (props) => { panelId, setConfig, topic, + annotationTopic, + annotationVisible, + meshTopic, + meshVisible, backgroundColor, showStatusText, fitMode, @@ -71,17 +83,46 @@ export const ImagePanel: React.FC = (props) => { } = props; const canvasRef = useRef(null); + const meshCanvasRef = useRef(null); + const meshRendererRef = useRef(null); const viewportRef = useRef(null); const workerRef = useRef(null); const workerDisposeTimerRef = useRef(null); const transferredCanvasRef = useRef(null); const lastPlaybackTimeNsRef = useRef(null); + const seekRepairGenerationRef = useRef(0); const lastUiStatusRef = useRef({ phase: 'idle' }); const h264ModeRef = useRef(false); + const meshRenderOptionsRef = useRef({ + backgroundColor, + flipHorizontal, + flipVertical, + rotationDeg: rotation, + smoothing, + fitMode, + }); const [status, setStatus] = useState({ phase: 'idle' }); const [metrics, setMetrics] = useState(null); const mainConsumerId = `${panelId}:image-main`; const h264ConsumerId = `${panelId}:image-main-h264`; + const annotationConsumerId = `${panelId}:image-annotations`; + const calibrationConsumerId = `${panelId}:image-scene-mesh-calibration`; + const selectedAnnotationTopic = annotationVisible ? annotationTopic.trim() : ''; + const selectedMeshTopic = meshVisible ? meshTopic.trim() : ''; + const selectedCalibrationTopic = inferCameraCalibrationTopic(topic); + + useEffect(() => { + const canvas = meshCanvasRef.current; + if (!canvas) return; + const renderer = new SceneMeshOverlayRenderer(canvas, meshRenderOptionsRef.current); + meshRendererRef.current = renderer; + return () => { + renderer.dispose(); + if (meshRendererRef.current === renderer) { + meshRendererRef.current = null; + } + }; + }, []); // Worker lifecycle: init on mount, dispose on unmount useEffect(() => { @@ -134,6 +175,14 @@ export const ImagePanel: React.FC = (props) => { setMetrics(data.metrics); return; } + if (data.type === 'rendered') { + meshRendererRef.current?.renderImageFrame( + data.timestampNs, + data.width, + data.height, + ); + return; + } if (data.type !== 'status') { return; } @@ -254,6 +303,66 @@ export const ImagePanel: React.FC = (props) => { }; }, [player, mainConsumerId, h264ConsumerId, topic]); + // Keep annotation delivery on the video lane. The worker selects the + // closest publish-time match before drawing over each image frame. + useEffect(() => { + const worker = workerRef.current; + if (!selectedAnnotationTopic || !worker) return; + + player.registerHighFrequencyConsumer(annotationConsumerId, { + topic: selectedAnnotationTopic, + lane: 'video', + mode: 'all', + onMessageBatch: (messages) => { + for (const event of messages) { + const overlay = parseImageAnnotations(event.message); + if (overlay) { + worker.postMessage({ type: 'overlay', overlay } satisfies ImageRenderWorkerRequest); + } + } + }, + }); + + return () => { + player.unregisterHighFrequencyConsumer(annotationConsumerId); + worker.postMessage({ type: 'overlay', overlay: null } satisfies ImageRenderWorkerRequest); + }; + }, [annotationConsumerId, player, selectedAnnotationTopic]); + + useEffect(() => { + const renderer = meshRendererRef.current; + renderer?.clearFrames(); + if (!renderer || !selectedMeshTopic) return; + const unsubscribe = subscribeSceneMeshTopic( + player, + selectedMeshTopic, + (frame) => renderer.addFrame(frame), + ); + return () => { + unsubscribe(); + renderer.clearFrames(); + }; + }, [player, selectedMeshTopic]); + + useEffect(() => { + const renderer = meshRendererRef.current; + renderer?.setCalibration(null); + if (!renderer || !selectedCalibrationTopic) return; + player.registerHighFrequencyConsumer(calibrationConsumerId, { + topic: selectedCalibrationTopic, + lane: 'pointcloud', + mode: 'latest', + onLatestMessage: (event) => { + const calibration = parseDoubleSphereCalibration(event.message); + if (calibration) renderer.setCalibration(calibration); + }, + }); + return () => { + player.unregisterHighFrequencyConsumer(calibrationConsumerId); + renderer.setCalibration(null); + }; + }, [calibrationConsumerId, player, selectedCalibrationTopic]); + // Keep the worker's media deadline current. On rewind, rebuild H.264 state // from the nearest complete random-access point. useEffect(() => { @@ -265,8 +374,15 @@ export const ImagePanel: React.FC = (props) => { } satisfies ImageRenderWorkerRequest); const nowNs = toNano(time); const previousNs = lastPlaybackTimeNsRef.current; + if (previousNs !== nowNs) { + seekRepairGenerationRef.current += 1; + } if (previousNs != null && nowNs + 5_000_000n < previousNs) { + const repairGeneration = seekRepairGenerationRef.current; const worker = workerRef.current; + const renderer = meshRendererRef.current; + renderer?.clearFrames(); + const stillImageTopic = worker && topic && !h264ModeRef.current ? topic : null; if (worker && topic && h264ModeRef.current) { worker.postMessage({ type: 'reset', @@ -274,12 +390,43 @@ export const ImagePanel: React.FC = (props) => { } satisfies ImageRenderWorkerRequest); void repairH264Seek(player, worker, topic, time); } else { - workerRef.current?.postMessage({ type: 'reset' } satisfies ImageRenderWorkerRequest); + worker?.postMessage({ type: 'reset' } satisfies ImageRenderWorkerRequest); + } + const repairTopics = new Set(); + if (stillImageTopic) repairTopics.add(stillImageTopic); + if (renderer && selectedMeshTopic) repairTopics.add(selectedMeshTopic); + if (repairTopics.size > 0 && player.getMessagesInTimeRange) { + void player.getMessagesInTimeRange({ + start: addMs(time, -2000), + end: time, + topics: [...repairTopics], + }).then((messages) => { + if ( + seekRepairGenerationRef.current !== repairGeneration || + (renderer && meshRendererRef.current !== renderer) + ) return; + let latestImage: RosMessageEvent | undefined; + for (const event of messages) { + if ( + stillImageTopic && + event.topic === stillImageTopic && + toNano(event.receiveTime) <= nowNs && + (!latestImage || toNano(event.receiveTime) > toNano(latestImage.receiveTime)) + ) { + latestImage = event; + } + if (renderer && event.topic === selectedMeshTopic) { + const frame = parseSceneMeshes(event.message, toNano(event.publishTime)); + if (frame) renderer.addFrame(frame); + } + } + if (worker && latestImage) postImageFrame(worker, latestImage); + }); } } lastPlaybackTimeNsRef.current = nowNs; }); - }, [isPlaying, player, topic]); + }, [isPlaying, player, selectedMeshTopic, topic]); // Send color/depth decode options when they change — triggers immediate redraw in worker useEffect(() => { @@ -303,10 +450,6 @@ export const ImagePanel: React.FC = (props) => { // Send render options (flip/rotation/smoothing/fitMode) — triggers immediate redraw useEffect(() => { - const worker = workerRef.current; - if (!worker) { - return; - } const options: ImageRenderOptions = { backgroundColor, flipHorizontal, @@ -315,7 +458,12 @@ export const ImagePanel: React.FC = (props) => { smoothing, fitMode, }; - worker.postMessage({ type: 'renderOptions', options } satisfies ImageRenderWorkerRequest); + meshRenderOptionsRef.current = options; + meshRendererRef.current?.setOptions(options); + workerRef.current?.postMessage({ + type: 'renderOptions', + options, + } satisfies ImageRenderWorkerRequest); }, [backgroundColor, flipHorizontal, flipVertical, rotation, smoothing, fitMode]); const statusText = getStatusText(status); @@ -352,6 +500,11 @@ export const ImagePanel: React.FC = (props) => { className="w-full h-full block" data-testid="image-panel-canvas" /> + {showStatusText && statusText && (
{statusText} diff --git a/src/features/panels/Image/ImagePanelSettings.tsx b/src/features/panels/Image/ImagePanelSettings.tsx index 782acfd..e51db44 100644 --- a/src/features/panels/Image/ImagePanelSettings.tsx +++ b/src/features/panels/Image/ImagePanelSettings.tsx @@ -16,6 +16,8 @@ import { useTopicSeq } from '@/core/pipeline/useMessageBus'; import { isRawImageMessage, isRawImageTopicSchema, isCompressedImageMessage, depthEncodingFromFormat, IMAGE_PANEL_TOPIC_INCLUDES } from './core/imageTypes'; import { applyDepthTopicPreset, defaultDepthMaxValue, defaultDepthMinValue } from './core/depthColorDefaults'; import type { ImageConfig } from './defaults'; +import { isImageAnnotationsSchema } from './core/imageAnnotations'; +import { isSceneUpdateSchema } from './core/sceneMesh'; const DEPTH_ENCODINGS = new Set(['mono16', '16uc1', '32fc1']); @@ -131,6 +133,52 @@ export function ImagePanelSettings({ placeholder={formatMessage({ id: 'panels.image.settings.field.topic.placeholder' })} /> + + setConfig({ ...config, annotationTopic })} + topics={topics} + topicTypeMatches={isImageAnnotationsSchema} + placeholder={formatMessage({ + id: 'panels.image.settings.field.annotationTopic.placeholder', + })} + /> + + + setConfig({ ...config, annotationVisible })} + /> + + + setConfig({ ...config, meshTopic })} + topics={topics} + topicTypeMatches={isSceneUpdateSchema} + placeholder={formatMessage({ + id: 'panels.image.settings.field.meshTopic.placeholder', + })} + /> + + + setConfig({ ...config, meshVisible })} + /> + diff --git a/src/features/panels/Image/core/ImageRender.worker.ts b/src/features/panels/Image/core/ImageRender.worker.ts index 2b5d8b0..ceda562 100644 --- a/src/features/panels/Image/core/ImageRender.worker.ts +++ b/src/features/panels/Image/core/ImageRender.worker.ts @@ -18,6 +18,7 @@ import { decodedFrameLatenessMs, initialH264PressureState, isH264HardLimitExceeded, + isRetrogradeMediaFrame, shouldDropDecodedH264Frame, updateDecodeDurationEwma, updateH264Pressure, @@ -36,6 +37,11 @@ import { normalizeCompressedMime, type ImageSurfaceStatus, } from './imageTypes'; +import { + drawImageAnnotations, + selectSynchronizedImageAnnotations, + type ImageAnnotationsFrame, +} from './imageAnnotations'; import type { ImageRenderOptions, ImageRenderMetrics, @@ -277,6 +283,7 @@ type CachedFrame = isBigEndian: boolean; data: Uint8Array; receiveTime: Time; + publishTime: Time; } | { kind: 'bitmap'; @@ -285,6 +292,7 @@ type CachedFrame = encoding: string; bitmap: ImageBitmap; receiveTime: Time; + publishTime: Time; }; // ---------- Main runtime ---------- @@ -297,6 +305,7 @@ class ImageRenderWorkerRuntime { #renderOptions: ImageRenderOptions = { ...DEFAULT_RENDER_OPTIONS }; #viewport: ImageViewport = { ...DEFAULT_VIEWPORT }; #rawDecodeOptions: Partial = {}; + #overlays: ImageAnnotationsFrame[] = []; #pendingFrame: ImageWorkerFrameEnvelope | null = null; #pendingH264Frames: ImageWorkerFrameEnvelope[] = []; #isProcessing = false; @@ -329,6 +338,7 @@ class ImageRenderWorkerRuntime { #lastH264ResyncAt = -Infinity; #playbackTimeNs: bigint | null = null; #lastDecodedH264TimeNs: bigint | null = null; + #lastDrawnMediaTimeNs: bigint | null = null; #isPlaying = false; #lastMetricsAt = -Infinity; #epoch = 0; @@ -398,6 +408,20 @@ class ImageRenderWorkerRuntime { } return; + case 'overlay': + if (!message.overlay) { + this.#overlays = []; + } else { + const existing = this.#overlays.findIndex( + (overlay) => overlay.timestampNs === message.overlay?.timestampNs, + ); + if (existing >= 0) this.#overlays[existing] = message.overlay; + else this.#overlays.push(message.overlay); + if (this.#overlays.length > 120) this.#overlays.shift(); + } + this.#redrawCachedFrameForOverlay(); + return; + case 'reset': this.#epoch += 1; this.#pendingFrame = null; @@ -407,6 +431,7 @@ class ImageRenderWorkerRuntime { this.#resetH264RuntimeState(); this.#decoder.reset(); this.#disposeAuxiliaryDecodeState(); + this.#overlays = []; if (!message.preserveFrame) { this.#disposeCachedBitmap(); this.#cachedFrame = null; @@ -585,6 +610,7 @@ class ImageRenderWorkerRuntime { const decoded = await decodeCompressedDepth(bytes, frame.format); this.#renderRawFrame({ receiveTime: frame.receiveTime, + publishTime: frame.publishTime, encoding: decoded.encoding, width: decoded.width, height: decoded.height, @@ -628,8 +654,8 @@ class ImageRenderWorkerRuntime { } closeCanvasImageSourceIfNeeded(sourceToClose); sourceToClose = null; - this.#storeBitmap(bitmap, width, height, frame.format, frame.receiveTime); - this.#drawBitmap(bitmap, width, height); + this.#storeBitmap(bitmap, width, height, frame.format, frame.receiveTime, frame.publishTime); + this.#drawBitmap(bitmap, width, height, frame.publishTime); this.#emitStatus({ phase: 'ready', width, @@ -648,6 +674,7 @@ class ImageRenderWorkerRuntime { const bytes = ensureOwnedBytes(frame.data); this.#renderRawFrame({ receiveTime: frame.receiveTime, + publishTime: frame.publishTime, encoding: frame.encoding, width: frame.width, height: frame.height, @@ -749,7 +776,10 @@ class ImageRenderWorkerRuntime { const width = videoFrame.displayWidth || videoFrame.codedWidth; const height = videoFrame.displayHeight || videoFrame.codedHeight; - this.#drawCanvasImageSource(videoFrame, width, height); + if (!this.#drawCanvasImageSource(videoFrame, width, height, sourceFrame.publishTime)) { + this.#droppedH264Frames += 1; + return; + } this.#lastH264RenderAt = now; this.#renderedH264Frames += 1; this.#emitStatus({ @@ -769,6 +799,7 @@ class ImageRenderWorkerRuntime { height, sourceFrame.kind === 'compressed' ? sourceFrame.format : 'h264', sourceFrame.receiveTime, + sourceFrame.publishTime, ); this.#lastH264BitmapAt = now; } catch { @@ -858,6 +889,7 @@ class ImageRenderWorkerRuntime { this.#h264ResyncCount = 0; this.#lastH264ResyncAt = -Infinity; this.#lastDecodedH264TimeNs = null; + this.#lastDrawnMediaTimeNs = null; this.#lastMetricsAt = -Infinity; } @@ -888,6 +920,7 @@ class ImageRenderWorkerRuntime { #renderRawFrame(frame: { receiveTime: Time; + publishTime: Time; encoding: string; width: number; height: number; @@ -928,9 +961,10 @@ class ImageRenderWorkerRuntime { isBigEndian: frame.isBigEndian, data: frame.data, receiveTime: frame.receiveTime, + publishTime: frame.publishTime, }; - this.#drawRawImageData(frame.width, frame.height); + this.#drawRawImageData(frame.width, frame.height, frame.publishTime); this.#emitStatus({ phase: 'ready', width: frame.width, @@ -968,7 +1002,7 @@ class ImageRenderWorkerRuntime { rgba, this.#rawDecodeOptions, ); - this.#drawRawImageData(cached.width, cached.height); + this.#drawRawImageData(cached.width, cached.height, cached.publishTime); this.#emitStatus({ phase: 'ready', width: cached.width, @@ -981,6 +1015,22 @@ class ImageRenderWorkerRuntime { } } + #redrawCachedFrameForOverlay(): void { + const cached = this.#cachedFrame; + if (cached?.kind === 'bitmap' && getCompressedKind(cached.encoding) === 'h264') { + // H.264 retains only a periodic bitmap for option and viewport redraws. + // Redraw it only when it is the frame still visible on the canvas. + if ( + this.#lastDrawnMediaTimeNs !== null && + timeToKey(cached.publishTime) === this.#lastDrawnMediaTimeNs + ) { + this.#redrawCachedFrame(); + } + return; + } + this.#redrawCachedFrame(); + } + /** Redraw the cached frame with current renderOptions / viewport. */ #redrawCachedFrame(): void { const cached = this.#cachedFrame; @@ -989,7 +1039,7 @@ class ImageRenderWorkerRuntime { return; } if (cached.kind === 'raw') { - this.#drawRawImageData(cached.width, cached.height); + this.#drawRawImageData(cached.width, cached.height, cached.publishTime); this.#emitStatus({ phase: 'ready', width: cached.width, @@ -998,7 +1048,7 @@ class ImageRenderWorkerRuntime { receiveTime: cached.receiveTime, }); } else { - this.#drawBitmap(cached.bitmap, cached.width, cached.height); + this.#drawBitmap(cached.bitmap, cached.width, cached.height, cached.publishTime); this.#emitStatus({ phase: 'ready', width: cached.width, @@ -1009,9 +1059,16 @@ class ImageRenderWorkerRuntime { } } - #storeBitmap(bitmap: ImageBitmap, width: number, height: number, encoding: string, receiveTime: Time): void { + #storeBitmap( + bitmap: ImageBitmap, + width: number, + height: number, + encoding: string, + receiveTime: Time, + publishTime: Time, + ): void { this.#disposeCachedBitmap(); - this.#cachedFrame = { kind: 'bitmap', width, height, encoding, bitmap, receiveTime }; + this.#cachedFrame = { kind: 'bitmap', width, height, encoding, bitmap, receiveTime, publishTime }; } #disposeCachedBitmap(): void { @@ -1061,22 +1118,31 @@ class ImageRenderWorkerRuntime { workerScope.postMessage(event); } - #drawRawImageData(width: number, height: number): void { + #drawRawImageData(width: number, height: number, publishTime: Time): void { ensureBufferCanvas(this.#bufferCanvas, width, height); this.#bufferCtx!.putImageData(this.#rawImageData!, 0, 0); - this.#drawCanvasImageSource(this.#bufferCanvas, width, height); + this.#drawCanvasImageSource(this.#bufferCanvas, width, height, publishTime); } - #drawBitmap(bitmap: ImageBitmap, width: number, height: number): void { - this.#drawCanvasImageSource(bitmap, width, height); + #drawBitmap(bitmap: ImageBitmap, width: number, height: number, publishTime: Time): void { + this.#drawCanvasImageSource(bitmap, width, height, publishTime); } - #drawCanvasImageSource(source: CanvasImageSource, sourceWidth: number, sourceHeight: number): void { + #drawCanvasImageSource( + source: CanvasImageSource, + sourceWidth: number, + sourceHeight: number, + publishTime: Time, + ): boolean { + const imageTimestampNs = timeToKey(publishTime); + if (isRetrogradeMediaFrame(this.#isPlaying, this.#lastDrawnMediaTimeNs, imageTimestampNs)) { + return false; + } this.#applyViewport(); const ctx = this.#ctx; const canvas = this.#canvas; if (!ctx || !canvas) { - return; + return false; } const viewportWidth = this.#viewport.cssWidth || canvas.width / Math.max(1, this.#viewport.devicePixelRatio) || 1; const viewportHeight = this.#viewport.cssHeight || canvas.height / Math.max(1, this.#viewport.devicePixelRatio) || 1; @@ -1102,7 +1168,21 @@ class ImageRenderWorkerRuntime { ctx.rotate((rotDeg * Math.PI) / 180); ctx.scale(this.#renderOptions.flipHorizontal ? -1 : 1, this.#renderOptions.flipVertical ? -1 : 1); ctx.drawImage(source, -drawWidth / 2, -drawHeight / 2, drawWidth, drawHeight); + const overlay = selectSynchronizedImageAnnotations(this.#overlays, imageTimestampNs); + if (overlay) { + ctx.translate(-drawWidth / 2, -drawHeight / 2); + ctx.scale(scale, scale); + drawImageAnnotations(ctx, overlay); + } ctx.restore(); + this.#lastDrawnMediaTimeNs = imageTimestampNs; + workerScope.postMessage({ + type: 'rendered', + timestampNs: imageTimestampNs, + width: sourceWidth, + height: sourceHeight, + } satisfies ImageRenderWorkerEvent); + return true; } #applyViewport(): void { diff --git a/src/features/panels/Image/core/SceneMeshOverlayRenderer.ts b/src/features/panels/Image/core/SceneMeshOverlayRenderer.ts new file mode 100644 index 0000000..0a9a5b4 --- /dev/null +++ b/src/features/panels/Image/core/SceneMeshOverlayRenderer.ts @@ -0,0 +1,393 @@ +import * as THREE from 'three'; +import type { ImageRenderOptions } from './imageWorkerProtocol'; +import { + SCENE_MESH_SYNC_TOLERANCE_NS, + selectSynchronizedSceneMeshes, + type DoubleSphereCalibration, + type SceneMeshPrimitive, + type SceneMeshFrame, +} from './sceneMesh'; + +const MAX_CACHED_FRAMES = 120; + +const VERTEX_SHADER = /* glsl */ ` + precision highp float; + + uniform mat4 uCameraFromReference; + uniform vec4 uIntrinsics; + uniform vec2 uXiAlpha; + uniform vec2 uSourceSize; + uniform vec2 uViewportSize; + uniform vec2 uFlip; + uniform vec2 uRotation; + uniform vec2 uPixelOffset; + uniform float uImageScale; + uniform vec2 uDepthRange; + + varying vec3 vNormalCamera; + varying vec3 vViewDirection; + + void main() { + vec3 cameraPoint = (uCameraFromReference * vec4(position, 1.0)).xyz; + float distanceOne = length(cameraPoint); + float zXi = uXiAlpha.x * distanceOne + cameraPoint.z; + float distanceTwo = length(vec3(cameraPoint.xy, zXi)); + float denominator = uXiAlpha.y * distanceTwo + (1.0 - uXiAlpha.y) * zXi; + if (denominator <= 1e-7) { + gl_Position = vec4(2.0, 2.0, 1.0, 1.0); + return; + } + vec2 sourcePixel = vec2( + uIntrinsics.x * cameraPoint.x / denominator + uIntrinsics.z, + uIntrinsics.y * cameraPoint.y / denominator + uIntrinsics.w + ); + vec2 imagePosition = (sourcePixel - uSourceSize * 0.5) * uImageScale * uFlip; + imagePosition = vec2( + uRotation.x * imagePosition.x - uRotation.y * imagePosition.y, + uRotation.y * imagePosition.x + uRotation.x * imagePosition.y + ); + vec2 viewportPixel = uViewportSize * 0.5 + imagePosition + uPixelOffset; + vec2 ndc = vec2( + viewportPixel.x / uViewportSize.x * 2.0 - 1.0, + 1.0 - viewportPixel.y / uViewportSize.y * 2.0 + ); + float depth = clamp( + (distanceOne - uDepthRange.x) / (uDepthRange.y - uDepthRange.x), + 0.0, + 1.0 + ) * 2.0 - 1.0; + gl_Position = vec4(ndc, depth, 1.0); + vNormalCamera = normalize(mat3(uCameraFromReference) * normal); + vViewDirection = normalize(-cameraPoint); + } +`; + +const MATERIAL_FRAGMENT_SHADER = /* glsl */ ` + precision highp float; + + uniform vec4 uColor; + uniform vec3 uLightDirection; + + varying vec3 vNormalCamera; + varying vec3 vViewDirection; + + void main() { + vec3 normalDirection = normalize(vNormalCamera); + float diffuse = abs(dot(normalDirection, normalize(uLightDirection))); + float rim = pow(1.0 - abs(dot(normalDirection, normalize(vViewDirection))), 2.0); + vec3 shaded = uColor.rgb * (0.36 + 0.64 * diffuse) + vec3(0.10) * rim; + gl_FragColor = vec4(shaded, uColor.a); + } +`; + +const SHADOW_FRAGMENT_SHADER = /* glsl */ ` + precision highp float; + uniform vec4 uColor; + void main() { + gl_FragColor = uColor; + } +`; + +interface MaterialUniforms extends Record { + uCameraFromReference: THREE.IUniform; + uIntrinsics: THREE.IUniform; + uXiAlpha: THREE.IUniform; + uSourceSize: THREE.IUniform; + uViewportSize: THREE.IUniform; + uFlip: THREE.IUniform; + uRotation: THREE.IUniform; + uPixelOffset: THREE.IUniform; + uImageScale: THREE.IUniform; + uDepthRange: THREE.IUniform; + uColor: THREE.IUniform; + uLightDirection: THREE.IUniform; +} + +interface MeshBundle { + id: string; + geometry: THREE.BufferGeometry; + material: THREE.ShaderMaterial; + shadowMaterial: THREE.ShaderMaterial; + mesh: THREE.Mesh; + shadow: THREE.Mesh; +} + +interface LastImageFrame { + timestampNs: bigint; + width: number; + height: number; +} + +export class SceneMeshOverlayRenderer { + readonly #canvas: HTMLCanvasElement; + readonly #renderer: THREE.WebGLRenderer; + readonly #scene = new THREE.Scene(); + readonly #camera = new THREE.Camera(); + readonly #cameraFromReference = new THREE.Matrix4(); + readonly #referenceFromCameraTranslation = new THREE.Vector3(); + readonly #referenceFromCameraQuaternion = new THREE.Quaternion(); + readonly #unitScale = new THREE.Vector3(1, 1, 1); + readonly #resizeObserver: ResizeObserver; + #frames: SceneMeshFrame[] = []; + #calibration: DoubleSphereCalibration | null = null; + #options: ImageRenderOptions; + #bundles: MeshBundle[] = []; + #activeTimestampNs: bigint | null = null; + #lastImageFrame: LastImageFrame | null = null; + + constructor(canvas: HTMLCanvasElement, options: ImageRenderOptions) { + this.#canvas = canvas; + this.#options = options; + this.#renderer = new THREE.WebGLRenderer({ + canvas, + alpha: true, + antialias: true, + premultipliedAlpha: true, + powerPreference: 'high-performance', + }); + this.#renderer.setClearColor(0x000000, 0); + this.#renderer.outputColorSpace = THREE.SRGBColorSpace; + this.#renderer.autoClear = true; + this.#resizeObserver = new ResizeObserver(() => { + this.#resize(); + this.#draw(); + }); + this.#resizeObserver.observe(canvas); + this.#resize(); + } + + addFrame(frame: SceneMeshFrame): void { + const oldestFrame = this.#frames[0]; + if (oldestFrame && frame.timestampNs < oldestFrame.timestampNs) { + this.#frames = []; + this.#activeTimestampNs = null; + this.#disposeBundles(); + } + const existing = this.#frames.findIndex((candidate) => candidate.timestampNs === frame.timestampNs); + if (existing >= 0) { + this.#frames[existing] = frame; + } else { + this.#frames.push(frame); + this.#frames.sort((left, right) => left.timestampNs < right.timestampNs ? -1 : 1); + if (this.#frames.length > MAX_CACHED_FRAMES) { + this.#frames.splice(0, this.#frames.length - MAX_CACHED_FRAMES); + } + } + const imageTimestampNs = this.#lastImageFrame?.timestampNs; + if (imageTimestampNs !== undefined) { + const delta = frame.timestampNs >= imageTimestampNs + ? frame.timestampNs - imageTimestampNs + : imageTimestampNs - frame.timestampNs; + if (delta <= SCENE_MESH_SYNC_TOLERANCE_NS) this.#draw(); + } + } + + clearFrames(): void { + this.#frames = []; + this.#activeTimestampNs = null; + this.#disposeBundles(); + this.#clear(); + } + + setCalibration(calibration: DoubleSphereCalibration | null): void { + this.#calibration = calibration; + if (calibration) { + this.#referenceFromCameraTranslation.set(...calibration.referenceFromCameraTranslation); + this.#referenceFromCameraQuaternion.set(...calibration.referenceFromCameraQuaternion); + this.#cameraFromReference.compose( + this.#referenceFromCameraTranslation, + this.#referenceFromCameraQuaternion, + this.#unitScale, + ).invert(); + } + this.#canvas.dataset.sceneMeshCalibrated = calibration ? 'true' : 'false'; + this.#draw(); + } + + setOptions(options: ImageRenderOptions): void { + this.#options = options; + this.#draw(); + } + + renderImageFrame(timestampNs: bigint, width: number, height: number): void { + this.#lastImageFrame = { timestampNs, width, height }; + this.#draw(); + } + + dispose(): void { + this.#resizeObserver.disconnect(); + this.#disposeBundles(); + this.#renderer.dispose(); + this.#canvas.removeAttribute('data-scene-mesh-frame-ns'); + this.#canvas.removeAttribute('data-scene-mesh-count'); + this.#canvas.removeAttribute('data-scene-mesh-calibrated'); + } + + #draw(): void { + const calibration = this.#calibration; + const image = this.#lastImageFrame; + if (!calibration || !image || this.#canvas.clientWidth <= 0 || this.#canvas.clientHeight <= 0) { + this.#clear(); + return; + } + const frame = selectSynchronizedSceneMeshes(this.#frames, image.timestampNs); + if (!frame) { + this.#activeTimestampNs = null; + this.#disposeBundles(); + this.#clear(); + return; + } + if (this.#activeTimestampNs !== frame.timestampNs) { + this.#applyMeshes(frame.meshes); + this.#activeTimestampNs = frame.timestampNs; + } + const viewportWidth = Math.max(1, this.#canvas.clientWidth); + const viewportHeight = Math.max(1, this.#canvas.clientHeight); + const rotationRad = ((this.#options.rotationDeg % 360) * Math.PI) / 180; + const rotationCos = Math.cos(rotationRad); + const rotationSin = Math.sin(rotationRad); + const absCos = Math.abs(rotationCos); + const absSin = Math.abs(rotationSin); + const logicalWidth = image.width * absCos + image.height * absSin; + const logicalHeight = image.width * absSin + image.height * absCos; + const imageScale = this.#options.fitMode === 'contain' + ? Math.min(viewportWidth / logicalWidth, viewportHeight / logicalHeight) + : Math.max(viewportWidth / logicalWidth, viewportHeight / logicalHeight); + const widthRatio = image.width / calibration.width; + const heightRatio = image.height / calibration.height; + const [fx, fy, cx, cy, xi, alpha] = calibration.intrinsics; + for (const bundle of this.#bundles) { + for (const material of [bundle.material, bundle.shadowMaterial]) { + const uniforms = material.uniforms as unknown as MaterialUniforms; + uniforms.uCameraFromReference.value.copy(this.#cameraFromReference); + uniforms.uIntrinsics.value.set( + fx * widthRatio, + fy * heightRatio, + cx * widthRatio, + cy * heightRatio, + ); + uniforms.uXiAlpha.value.set(xi, alpha); + uniforms.uSourceSize.value.set(image.width, image.height); + uniforms.uViewportSize.value.set(viewportWidth, viewportHeight); + uniforms.uFlip.value.set( + this.#options.flipHorizontal ? -1 : 1, + this.#options.flipVertical ? -1 : 1, + ); + uniforms.uRotation.value.set(rotationCos, rotationSin); + uniforms.uImageScale.value = imageScale; + } + } + this.#renderer.render(this.#scene, this.#camera); + this.#canvas.dataset.sceneMeshFrameNs = frame.timestampNs.toString(); + this.#canvas.dataset.sceneMeshCount = String(frame.meshes.length); + } + + #applyMeshes(meshes: readonly SceneMeshPrimitive[]): void { + if (!this.#updateMeshes(meshes)) this.#replaceMeshes(meshes); + } + + #updateMeshes(meshes: readonly SceneMeshPrimitive[]): boolean { + if (meshes.length !== this.#bundles.length) return false; + for (let index = 0; index < meshes.length; index += 1) { + const primitive = meshes[index]; + const bundle = this.#bundles[index]; + if (!primitive || !bundle || bundle.id !== primitive.id) return false; + const position = bundle.geometry.getAttribute('position'); + const indices = bundle.geometry.getIndex(); + if ( + !(position instanceof THREE.BufferAttribute) || + !indices || + position.array.length !== primitive.points.length || + indices.array.length !== primitive.indices.length + ) return false; + } + for (let index = 0; index < meshes.length; index += 1) { + const primitive = meshes[index]; + const bundle = this.#bundles[index]; + const position = bundle.geometry.getAttribute('position') as THREE.BufferAttribute; + const indices = bundle.geometry.getIndex()!; + (position.array as Float32Array).set(primitive.points); + (indices.array as Uint32Array).set(primitive.indices); + position.needsUpdate = true; + indices.needsUpdate = true; + bundle.geometry.computeVertexNormals(); + const uniforms = bundle.material.uniforms as unknown as MaterialUniforms; + uniforms.uColor.value.set(...primitive.color); + } + return true; + } + + #replaceMeshes(meshes: readonly SceneMeshPrimitive[]): void { + this.#disposeBundles(); + this.#bundles = meshes.map((primitive, index) => { + const geometry = new THREE.BufferGeometry(); + geometry.setAttribute('position', new THREE.BufferAttribute(primitive.points, 3)); + geometry.setIndex(new THREE.BufferAttribute(primitive.indices, 1)); + geometry.computeVertexNormals(); + geometry.computeBoundingSphere(); + const material = this.#createMaterial(primitive.color, false); + const shadowMaterial = this.#createMaterial([0.01, 0.015, 0.025, 0.30], true); + const mesh = new THREE.Mesh(geometry, material); + const shadow = new THREE.Mesh(geometry, shadowMaterial); + shadow.frustumCulled = false; + mesh.frustumCulled = false; + shadow.renderOrder = index; + mesh.renderOrder = 10 + index; + this.#scene.add(shadow, mesh); + return { id: primitive.id, geometry, material, shadowMaterial, mesh, shadow }; + }); + } + + #createMaterial( + color: readonly [number, number, number, number], + shadow: boolean, + ): THREE.ShaderMaterial { + const uniforms: MaterialUniforms = { + uCameraFromReference: { value: new THREE.Matrix4() }, + uIntrinsics: { value: new THREE.Vector4() }, + uXiAlpha: { value: new THREE.Vector2() }, + uSourceSize: { value: new THREE.Vector2(1, 1) }, + uViewportSize: { value: new THREE.Vector2(1, 1) }, + uFlip: { value: new THREE.Vector2(1, 1) }, + uRotation: { value: new THREE.Vector2(1, 0) }, + uPixelOffset: { value: shadow ? new THREE.Vector2(3, 4) : new THREE.Vector2(0, 0) }, + uImageScale: { value: 1 }, + uDepthRange: { value: new THREE.Vector2(0.02, 5) }, + uColor: { value: new THREE.Vector4(...color) }, + uLightDirection: { value: new THREE.Vector3(-0.35, -0.55, 0.76).normalize() }, + }; + return new THREE.ShaderMaterial({ + uniforms, + vertexShader: VERTEX_SHADER, + fragmentShader: shadow ? SHADOW_FRAGMENT_SHADER : MATERIAL_FRAGMENT_SHADER, + transparent: true, + depthTest: !shadow, + depthWrite: !shadow, + side: THREE.DoubleSide, + blending: THREE.NormalBlending, + }); + } + + #disposeBundles(): void { + for (const bundle of this.#bundles) { + this.#scene.remove(bundle.mesh, bundle.shadow); + bundle.material.dispose(); + bundle.shadowMaterial.dispose(); + bundle.geometry.dispose(); + } + this.#bundles = []; + } + + #resize(): void { + const width = Math.max(1, this.#canvas.clientWidth); + const height = Math.max(1, this.#canvas.clientHeight); + this.#renderer.setPixelRatio(window.devicePixelRatio || 1); + this.#renderer.setSize(width, height, false); + } + + #clear(): void { + this.#renderer.clear(true, true, true); + this.#canvas.removeAttribute('data-scene-mesh-frame-ns'); + this.#canvas.dataset.sceneMeshCount = '0'; + } +} diff --git a/src/features/panels/Image/core/h264Backpressure.test.ts b/src/features/panels/Image/core/h264Backpressure.test.ts index 6c81da1..fbe58a5 100644 --- a/src/features/panels/Image/core/h264Backpressure.test.ts +++ b/src/features/panels/Image/core/h264Backpressure.test.ts @@ -5,6 +5,7 @@ import { decodedFrameLatenessMs, initialH264PressureState, isH264HardLimitExceeded, + isRetrogradeMediaFrame, shouldDropDecodedH264Frame, updateDecodeDurationEwma, updateH264Pressure, @@ -98,4 +99,12 @@ describe('H.264 adaptive backpressure', () => { expect(shouldDropDecodedH264Frame(playback, 850_000_000n)).toBe(true); expect(shouldDropDecodedH264Frame(null, 0n)).toBe(false); }); + + it('rejects backward frame paints only during playback', () => { + expect(isRetrogradeMediaFrame(true, 1_000n, 999n)).toBe(true); + expect(isRetrogradeMediaFrame(true, 1_000n, 1_000n)).toBe(false); + expect(isRetrogradeMediaFrame(true, 1_000n, 1_001n)).toBe(false); + expect(isRetrogradeMediaFrame(false, 1_000n, 999n)).toBe(false); + expect(isRetrogradeMediaFrame(true, null, 999n)).toBe(false); + }); }); diff --git a/src/features/panels/Image/core/h264Backpressure.ts b/src/features/panels/Image/core/h264Backpressure.ts index 3cdce2a..2ba6505 100644 --- a/src/features/panels/Image/core/h264Backpressure.ts +++ b/src/features/panels/Image/core/h264Backpressure.ts @@ -123,3 +123,11 @@ export function shouldDropDecodedH264Frame( ): boolean { return decodedFrameLatenessMs(playbackTimeNs, frameTimeNs) > deadlineMs; } + +export function isRetrogradeMediaFrame( + isPlaying: boolean, + lastDrawnTimeNs: bigint | null, + frameTimeNs: bigint, +): boolean { + return isPlaying && lastDrawnTimeNs !== null && frameTimeNs < lastDrawnTimeNs; +} diff --git a/src/features/panels/Image/core/imageAnnotations.test.ts b/src/features/panels/Image/core/imageAnnotations.test.ts new file mode 100644 index 0000000..ea9a36e --- /dev/null +++ b/src/features/panels/Image/core/imageAnnotations.test.ts @@ -0,0 +1,98 @@ +import { describe, expect, it } from 'vitest'; +import { + drawImageAnnotations, + isImageAnnotationsSchema, + parseImageAnnotations, + selectSynchronizedImageAnnotations, +} from './imageAnnotations'; + +describe('foxglove.ImageAnnotations parsing', () => { + it('normalizes numeric and named point annotation enums', () => { + const parsed = parseImageAnnotations({ + points: [ + { + timestamp: { seconds: '7', nanos: 11 }, + type: 'POINTS', + points: [{ x: 12.5, y: 25.25 }], + outlineColor: { r: 1, g: 1, b: 1, a: 1 }, + fillColor: { r: 0.1, g: 0.2, b: 0.3, a: 1 }, + thickness: 7, + }, + { + timestamp: { seconds: '7', nanos: 11 }, + type: 4, + points: [{ x: 12.5, y: 25.25 }, { x: 15, y: 30 }], + outline_color: { r: 1, g: 0.38, b: 0.1, a: 1 }, + fill_color: { r: 1, g: 0.38, b: 0.1, a: 0 }, + thickness: 4, + }, + ], + }); + + expect(parsed?.timestampNs).toBe(7_000_000_011n); + expect(parsed?.points.map((point) => point.kind)).toEqual(['points', 'line-list']); + expect(parsed?.points[0]?.fillColor).toEqual({ r: 0.1, g: 0.2, b: 0.3, a: 1 }); + }); + + it('uses the root timestamp when annotations are empty', () => { + expect(parseImageAnnotations({ timestamp: { sec: 9, nsec: 3 }, points: [] })).toEqual({ + timestampNs: 9_000_000_003n, + points: [], + }); + }); +}); + +describe('foxglove.ImageAnnotations schema matching', () => { + it('recognizes ROS and Foxglove schema spellings', () => { + expect(isImageAnnotationsSchema('foxglove.ImageAnnotations')).toBe(true); + expect(isImageAnnotationsSchema('foxglove_msgs/msg/ImageAnnotations')).toBe(true); + expect(isImageAnnotationsSchema('sensor_msgs/msg/Image')).toBe(false); + }); +}); + +describe('annotation rendering and synchronization', () => { + it('draws point circles and line-list segments', () => { + const overlay = parseImageAnnotations({ + points: [ + { + timestamp: { sec: 9, nsec: 0 }, + type: 1, + points: [{ x: 100, y: 120 }], + outline_color: { r: 0.12, g: 0.64, b: 1, a: 1 }, + fill_color: { r: 0.12, g: 0.64, b: 1, a: 1 }, + thickness: 7, + }, + { + timestamp: { sec: 9, nsec: 0 }, + type: 'LINE_LIST', + points: [{ x: 100, y: 120 }, { x: 130, y: 150 }], + outline_color: { r: 0.12, g: 0.64, b: 1, a: 1 }, + fill_color: { r: 0.12, g: 0.64, b: 1, a: 0 }, + thickness: 4, + }, + ], + }); + const calls: string[] = []; + const context = { + beginPath: () => calls.push('begin'), + arc: (x: number, y: number, radius: number) => calls.push(`arc:${x},${y},${radius}`), + fill: () => calls.push('fill'), + stroke: () => calls.push('stroke'), + moveTo: (x: number, y: number) => calls.push(`move:${x},${y}`), + lineTo: (x: number, y: number) => calls.push(`line:${x},${y}`), + closePath: () => calls.push('close'), + } as unknown as CanvasRenderingContext2D; + + drawImageAnnotations(context, overlay!); + expect(calls).toContain('arc:100,120,3.5'); + expect(calls).toContain('move:100,120'); + expect(calls).toContain('line:130,150'); + }); + + it('selects the nearest annotation within the default eight-millisecond window', () => { + const early = { timestampNs: 1_000_000_000n, points: [] }; + const late = { timestampNs: 1_033_000_000n, points: [] }; + expect(selectSynchronizedImageAnnotations([early, late], 1_034_000_000n)).toBe(late); + expect(selectSynchronizedImageAnnotations([early, late], 1_050_000_000n)).toBeNull(); + }); +}); diff --git a/src/features/panels/Image/core/imageAnnotations.ts b/src/features/panels/Image/core/imageAnnotations.ts new file mode 100644 index 0000000..cf723bd --- /dev/null +++ b/src/features/panels/Image/core/imageAnnotations.ts @@ -0,0 +1,223 @@ +const ANNOTATION_SYNC_TOLERANCE_NS = 8_000_000n; + +interface AnnotationColor { + r: number; + g: number; + b: number; + a: number; +} + +interface AnnotationPoint { + x: number; + y: number; +} + +type PointsAnnotationKind = 'points' | 'line-loop' | 'line-strip' | 'line-list'; + +interface PointsAnnotation { + kind: PointsAnnotationKind; + points: AnnotationPoint[]; + outlineColor: AnnotationColor; + outlineColors: AnnotationColor[]; + fillColor: AnnotationColor; + thickness: number; +} + +export interface ImageAnnotationsFrame { + timestampNs: bigint; + points: PointsAnnotation[]; +} +const POINTS_KIND_BY_ENUM: Readonly> = { + '1': 'points', + POINTS: 'points', + '2': 'line-loop', + LINE_LOOP: 'line-loop', + '3': 'line-strip', + LINE_STRIP: 'line-strip', + '4': 'line-list', + LINE_LIST: 'line-list', +}; + +export function isImageAnnotationsSchema(schemaName: string): boolean { + const normalized = schemaName.toLowerCase().replaceAll('_', ''); + return normalized.endsWith('imageannotations'); +} + + +export function parseImageAnnotations(message: unknown): ImageAnnotationsFrame | null { + if (!isRecord(message) || !Array.isArray(message.points)) return null; + + const points = message.points.map(parsePointsAnnotation).filter(isPresent); + const timestampNs = + readTimestampNs(message.timestamp) ?? firstAnnotationTimestampNs(message.points); + if (timestampNs === undefined) return null; + + return { timestampNs, points }; +} + +export function selectSynchronizedImageAnnotations( + overlays: readonly ImageAnnotationsFrame[], + imageTimestampNs: bigint, + toleranceNs = ANNOTATION_SYNC_TOLERANCE_NS, +): ImageAnnotationsFrame | null { + let best: ImageAnnotationsFrame | null = null; + let bestDelta = toleranceNs + 1n; + for (const overlay of overlays) { + const delta = overlay.timestampNs >= imageTimestampNs + ? overlay.timestampNs - imageTimestampNs + : imageTimestampNs - overlay.timestampNs; + if (delta < bestDelta) { + best = overlay; + bestDelta = delta; + } + } + return bestDelta <= toleranceNs ? best : null; +} + +export function drawImageAnnotations( + context: CanvasRenderingContext2D | OffscreenCanvasRenderingContext2D, + overlay: ImageAnnotationsFrame, +): void { + context.lineCap = 'round'; + context.lineJoin = 'round'; + for (const annotation of overlay.points) { + drawPointsAnnotation(context, annotation); + } +} + +function drawPointsAnnotation( + context: CanvasRenderingContext2D | OffscreenCanvasRenderingContext2D, + annotation: PointsAnnotation, +): void { + const { points } = annotation; + if (points.length === 0) return; + + context.lineWidth = Math.max(1, annotation.thickness); + context.strokeStyle = colorToCss(annotation.outlineColor); + context.fillStyle = colorToCss(annotation.fillColor); + + switch (annotation.kind) { + case 'points': + for (let index = 0; index < points.length; index += 1) { + const point = points[index]; + context.strokeStyle = colorToCss( + annotation.outlineColors[index] ?? annotation.outlineColor, + ); + context.beginPath(); + context.arc(point.x, point.y, Math.max(1, annotation.thickness / 2), 0, Math.PI * 2); + context.fill(); + context.stroke(); + } + return; + case 'line-loop': + case 'line-strip': + context.beginPath(); + context.moveTo(points[0].x, points[0].y); + for (const point of points.slice(1)) context.lineTo(point.x, point.y); + if (annotation.kind === 'line-loop') { + context.closePath(); + if (annotation.fillColor.a > 0) context.fill(); + } + context.stroke(); + return; + case 'line-list': + for (let index = 0; index + 1 < points.length; index += 2) { + context.beginPath(); + context.moveTo(points[index].x, points[index].y); + context.lineTo(points[index + 1].x, points[index + 1].y); + context.stroke(); + } + } +} + +function parsePointsAnnotation(value: unknown): PointsAnnotation | null { + if (!isRecord(value) || !Array.isArray(value.points)) return null; + const kind = POINTS_KIND_BY_ENUM[String(value.type)]; + if (!kind) return null; + const points = value.points.map(parsePoint).filter(isPresent); + const outlineColor = parseColor(value.outline_color ?? value.outlineColor) ?? { + r: 1, + g: 1, + b: 1, + a: 1, + }; + const fillColor = parseColor(value.fill_color ?? value.fillColor) ?? outlineColor; + const rawOutlineColors = value.outline_colors ?? value.outlineColors; + const outlineColors = Array.isArray(rawOutlineColors) + ? rawOutlineColors.map(parseColor).filter(isPresent) + : []; + return { + kind, + points, + outlineColor, + outlineColors, + fillColor, + thickness: Math.max(1, readNumber(value, ['thickness']) ?? 1), + }; +} + +function readTimestampNs(value: unknown): bigint | undefined { + if (!isRecord(value)) return undefined; + const sec = readBigInt(value.sec ?? value.seconds); + const nsec = readBigInt(value.nsec ?? value.nanosec ?? value.nanos); + return sec !== undefined && nsec !== undefined ? sec * 1_000_000_000n + nsec : undefined; +} + +function firstAnnotationTimestampNs(points: readonly unknown[]): bigint | undefined { + for (const point of points) { + if (!isRecord(point)) continue; + const timestampNs = readTimestampNs(point.timestamp); + if (timestampNs !== undefined) return timestampNs; + } + return undefined; +} + +function parsePoint(value: unknown): AnnotationPoint | null { + if (!isRecord(value) || !isFiniteNumber(value.x) || !isFiniteNumber(value.y)) return null; + return { x: value.x, y: value.y }; +} + +function parseColor(value: unknown): AnnotationColor | null { + if ( + !isRecord(value) || + !isFiniteNumber(value.r) || + !isFiniteNumber(value.g) || + !isFiniteNumber(value.b) || + !isFiniteNumber(value.a) + ) return null; + return { r: value.r, g: value.g, b: value.b, a: value.a }; +} + +function colorToCss(color: AnnotationColor): string { + const red = Math.round(Math.max(0, Math.min(1, color.r)) * 255); + const green = Math.round(Math.max(0, Math.min(1, color.g)) * 255); + const blue = Math.round(Math.max(0, Math.min(1, color.b)) * 255); + const alpha = Math.max(0, Math.min(1, color.a)); + return `rgba(${red}, ${green}, ${blue}, ${alpha})`; +} + +function readNumber(record: Record, keys: readonly string[]): number | undefined { + for (const key of keys) { + if (isFiniteNumber(record[key])) return record[key]; + } + return undefined; +} + +function readBigInt(value: unknown): bigint | undefined { + if (typeof value === 'bigint') return value; + if (typeof value === 'number' && Number.isSafeInteger(value)) return BigInt(value); + if (typeof value === 'string' && /^\d+$/.test(value)) return BigInt(value); + return undefined; +} + +function isRecord(value: unknown): value is Record { + return typeof value === 'object' && value !== null; +} + +function isFiniteNumber(value: unknown): value is number { + return typeof value === 'number' && Number.isFinite(value); +} + +function isPresent(value: T | null): value is T { + return value !== null; +} diff --git a/src/features/panels/Image/core/imageWorkerProtocol.ts b/src/features/panels/Image/core/imageWorkerProtocol.ts index 2db2ebe..2507f1a 100644 --- a/src/features/panels/Image/core/imageWorkerProtocol.ts +++ b/src/features/panels/Image/core/imageWorkerProtocol.ts @@ -2,6 +2,7 @@ import type { Time } from '@/core/types/ros'; import type { RawImageDecodeOptions } from './imageColorMode'; import type { ImageSurfaceStatus } from './imageTypes'; import type { H264PressureMode } from './h264Backpressure'; +import type { ImageAnnotationsFrame } from './imageAnnotations'; export interface ImageRenderOptions { /** CSS color string (e.g. `#ff0000`) used to fill letterbox/pillarbox and idle canvas. */ @@ -23,6 +24,7 @@ export type ImageWorkerFrameEnvelope = | { kind: 'compressed'; receiveTime: Time; + publishTime: Time; format: string; data: Uint8Array; } @@ -30,6 +32,7 @@ export type ImageWorkerFrameEnvelope = kind: 'raw'; receiveTime: Time; encoding: string; + publishTime: Time; width: number; height: number; step?: number; @@ -63,6 +66,10 @@ export type ImageRenderWorkerRequest = type: 'frame'; frame: ImageWorkerFrameEnvelope; } + | { + type: 'overlay'; + overlay: ImageAnnotationsFrame | null; + } | { type: 'reset'; preserveFrame?: boolean; @@ -92,4 +99,10 @@ export type ImageRenderWorkerEvent = | { type: 'metrics'; metrics: ImageRenderMetrics; + } + | { + type: 'rendered'; + timestampNs: bigint; + width: number; + height: number; }; diff --git a/src/features/panels/Image/core/messageFrameAdapter.test.ts b/src/features/panels/Image/core/messageFrameAdapter.test.ts index b9c4fce..ee503a4 100644 --- a/src/features/panels/Image/core/messageFrameAdapter.test.ts +++ b/src/features/panels/Image/core/messageFrameAdapter.test.ts @@ -3,12 +3,13 @@ import type { MessageEvent as RosMessageEvent } from '@/core/types/ros'; import { isH264MessageEvent, toWorkerFrame } from './messageFrameAdapter'; const receiveTime = { sec: 10, nsec: 0 }; +const publishTime = { sec: 9, nsec: 250 }; function makeCompressedImageEvent(data: Uint8Array, format = 'h264'): RosMessageEvent { return { topic: '/camera/compressed', receiveTime, - publishTime: receiveTime, + publishTime, message: { format, data }, schemaName: 'sensor_msgs/msg/CompressedImage', }; @@ -18,7 +19,7 @@ function makeCompressedVideoEvent(data: Uint8Array, format = 'h264'): RosMessage return { topic: '/camera/video', receiveTime, - publishTime: receiveTime, + publishTime, message: { timestamp: receiveTime, frame_id: 'camera_optical', @@ -41,10 +42,12 @@ describe('messageFrameAdapter', () => { kind: 'compressed', receiveTime, format: 'h264', + publishTime, }); expect(fromVideo!.frame).toMatchObject({ kind: 'compressed', receiveTime, + publishTime, format: 'h264', }); expect(Array.from(fromImage!.frame.data)).toEqual(Array.from(fromVideo!.frame.data)); diff --git a/src/features/panels/Image/core/messageFrameAdapter.ts b/src/features/panels/Image/core/messageFrameAdapter.ts index e0e0cdc..d265d5a 100644 --- a/src/features/panels/Image/core/messageFrameAdapter.ts +++ b/src/features/panels/Image/core/messageFrameAdapter.ts @@ -31,6 +31,7 @@ export function toWorkerFrame( frame: { kind: 'compressed', receiveTime: messageEvent.receiveTime, + publishTime: messageEvent.publishTime, format: getCompressedFrameFormat(message), data: payload.data, }, @@ -46,6 +47,7 @@ export function toWorkerFrame( frame: { kind: 'raw', receiveTime: messageEvent.receiveTime, + publishTime: messageEvent.publishTime, encoding: message.encoding, width: message.width, height: message.height, diff --git a/src/features/panels/Image/core/sceneMesh.test.ts b/src/features/panels/Image/core/sceneMesh.test.ts new file mode 100644 index 0000000..b760cc9 --- /dev/null +++ b/src/features/panels/Image/core/sceneMesh.test.ts @@ -0,0 +1,62 @@ +import { describe, expect, it } from 'vitest'; +import { + inferCameraCalibrationTopic, + parseDoubleSphereCalibration, + parseSceneMeshes, + selectSynchronizedSceneMeshes, +} from './sceneMesh'; + +describe('scene mesh image overlay messages', () => { + it('parses indexed triangle meshes from foxglove.SceneUpdate', () => { + const frame = parseSceneMeshes({ + entities: [{ + id: 'safety-zone', + frameId: 'warehouse_map', + timestamp: { sec: 12n, nsec: 34 }, + triangles: [{ + points: [ + { x: 0, y: 0, z: 1 }, + { x: 1, y: 0, z: 1 }, + { x: 0, y: 1, z: 1 }, + ], + indices: [0, 1, 2], + color: { r: 0.1, g: 0.2, b: 0.3, a: 0.8 }, + }], + }], + }); + + expect(frame?.timestampNs).toBe(12_000_000_034n); + expect(frame?.meshes).toHaveLength(1); + expect(frame?.meshes[0]?.frameId).toBe('warehouse_map'); + expect(Array.from(frame?.meshes[0]?.indices ?? [])).toEqual([0, 1, 2]); + }); + + it.each(['T_r_c', 'T_b_c'])( + 'parses and normalizes Double Sphere camera calibration with %s', + (transformField) => { + const calibration = parseDoubleSphereCalibration({ + width: 1280, + height: 720, + distortion_model: 'DS', + D: [640, 641, 639, 359, -0.12, 0.55], + [transformField]: [1, 2, 3, 0, 0, 0, 2], + }); + + expect(calibration?.intrinsics).toEqual([640, 641, 639, 359, -0.12, 0.55]); + expect(calibration?.referenceFromCameraTranslation).toEqual([1, 2, 3]); + expect(calibration?.referenceFromCameraQuaternion).toEqual([0, 0, 0, 1]); + }, + ); + + it('infers the paired camera calibration topic', () => { + expect(inferCameraCalibrationTopic('/warehouse/front_camera/image/compressed')).toBe( + '/warehouse/front_camera/camera_info', + ); + }); + + it('selects only a mesh frame within the image sync tolerance', () => { + const frame = { timestampNs: 1_000_000_000n, meshes: [] }; + expect(selectSynchronizedSceneMeshes([frame], 1_007_000_000n)).toBe(frame); + expect(selectSynchronizedSceneMeshes([frame], 1_009_000_000n)).toBeNull(); + }); +}); diff --git a/src/features/panels/Image/core/sceneMesh.ts b/src/features/panels/Image/core/sceneMesh.ts new file mode 100644 index 0000000..199764b --- /dev/null +++ b/src/features/panels/Image/core/sceneMesh.ts @@ -0,0 +1,217 @@ +export const SCENE_MESH_SYNC_TOLERANCE_NS = 8_000_000n; +const parsedSceneMeshCache = new WeakMap(); + + +export interface SceneMeshPrimitive { + id: string; + frameId: string; + points: Float32Array; + indices: Uint32Array; + color: [number, number, number, number]; +} + +export interface SceneMeshFrame { + timestampNs: bigint; + meshes: SceneMeshPrimitive[]; +} + +export interface DoubleSphereCalibration { + width: number; + height: number; + intrinsics: [number, number, number, number, number, number]; + referenceFromCameraTranslation: [number, number, number]; + referenceFromCameraQuaternion: [number, number, number, number]; +} + +export function isSceneUpdateSchema(schemaName: string): boolean { + const normalized = schemaName.toLowerCase().replaceAll('_', ''); + return normalized.endsWith('sceneupdate'); +} + +export function inferCameraCalibrationTopic(imageTopic: string): string | null { + const match = imageTopic.match(/^(.*?)(?:\/image)?\/(?:compressed|image(?:_raw)?)$/i); + return match ? `${match[1]}/camera_info` : null; +} + +export function parseSceneMeshes( + message: unknown, + fallbackTimestampNs?: bigint, +): SceneMeshFrame | null { + if (!isRecord(message) || !Array.isArray(message.entities)) return null; + const cached = parsedSceneMeshCache.get(message); + if (cached) return cached; + const meshes: SceneMeshPrimitive[] = []; + let timestampNs: bigint | undefined; + for (const entityValue of message.entities) { + if (!isRecord(entityValue)) continue; + const trianglesValue = entityValue.triangles; + if (!Array.isArray(trianglesValue)) continue; + const id = readString(entityValue, ['id']) ?? `mesh-${meshes.length}`; + const frameId = readString(entityValue, ['frame_id', 'frameId']) ?? ''; + timestampNs ??= readTimestampNs(entityValue.timestamp); + for (let index = 0; index < trianglesValue.length; index += 1) { + const mesh = parseTrianglePrimitive( + trianglesValue[index], + trianglesValue.length === 1 ? id : `${id}-${index}`, + frameId, + ); + if (mesh) meshes.push(mesh); + } + } + if (meshes.length === 0) return null; + timestampNs ??= fallbackTimestampNs; + if (timestampNs === undefined) return null; + const frame = { timestampNs, meshes }; + parsedSceneMeshCache.set(message, frame); + return frame; +} + +export function parseDoubleSphereCalibration(message: unknown): DoubleSphereCalibration | null { + if (!isRecord(message)) return null; + const width = readNumber(message, ['width']); + const height = readNumber(message, ['height']); + const model = readString(message, ['distortion_model', 'distortionModel']); + const distortion = readNumberArray(message.D ?? message.d); + const transform = readNumberArray( + message.T_r_c ?? + message.t_r_c ?? + message.TRC ?? + message.tRC ?? + message.T_b_c ?? + message.t_b_c ?? + message.TBC ?? + message.tBC, + ); + if ( + width === undefined || + height === undefined || + width <= 0 || + height <= 0 || + model?.toLowerCase() !== 'ds' || + distortion?.length !== 6 || + transform?.length !== 7 + ) return null; + const quaternionNorm = Math.hypot(transform[3], transform[4], transform[5], transform[6]); + if (!Number.isFinite(quaternionNorm) || quaternionNorm < 1e-9) return null; + return { + width, + height, + intrinsics: distortion as DoubleSphereCalibration['intrinsics'], + referenceFromCameraTranslation: [transform[0], transform[1], transform[2]], + referenceFromCameraQuaternion: [ + transform[3] / quaternionNorm, + transform[4] / quaternionNorm, + transform[5] / quaternionNorm, + transform[6] / quaternionNorm, + ], + }; +} + +export function selectSynchronizedSceneMeshes( + frames: readonly SceneMeshFrame[], + imageTimestampNs: bigint, + toleranceNs = SCENE_MESH_SYNC_TOLERANCE_NS, +): SceneMeshFrame | null { + let best: SceneMeshFrame | null = null; + let bestDelta = toleranceNs + 1n; + for (const frame of frames) { + const delta = frame.timestampNs >= imageTimestampNs + ? frame.timestampNs - imageTimestampNs + : imageTimestampNs - frame.timestampNs; + if (delta < bestDelta) { + best = frame; + bestDelta = delta; + } + } + return bestDelta <= toleranceNs ? best : null; +} + +function parseTrianglePrimitive( + value: unknown, + id: string, + frameId: string, +): SceneMeshPrimitive | null { + if (!isRecord(value) || !Array.isArray(value.points)) return null; + const pointValues = value.points as unknown[]; + const indicesValue = readNumberArray(value.indices); + if (!indicesValue || indicesValue.length === 0 || indicesValue.length % 3 !== 0) return null; + const points = new Float32Array(pointValues.length * 3); + for (let index = 0; index < pointValues.length; index += 1) { + const point = pointValues[index]; + if (!isRecord(point)) return null; + const x = readNumber(point, ['x']); + const y = readNumber(point, ['y']); + const z = readNumber(point, ['z']); + if (x === undefined || y === undefined || z === undefined) return null; + points[index * 3] = x; + points[index * 3 + 1] = y; + points[index * 3 + 2] = z; + } + const indices = new Uint32Array(indicesValue.length); + for (let index = 0; index < indicesValue.length; index += 1) { + const vertex = indicesValue[index]; + if (!Number.isInteger(vertex) || vertex < 0 || vertex >= pointValues.length) return null; + indices[index] = vertex; + } + const color = parseColor(value.color) ?? [0.3, 0.72, 1, 0.78]; + return { id, frameId, points, indices, color }; +} + +function parseColor(value: unknown): [number, number, number, number] | null { + if (!isRecord(value)) return null; + const r = readNumber(value, ['r']); + const g = readNumber(value, ['g']); + const b = readNumber(value, ['b']); + const a = readNumber(value, ['a']); + return r === undefined || g === undefined || b === undefined || a === undefined + ? null + : [clamp01(r), clamp01(g), clamp01(b), clamp01(a)]; +} + +function readTimestampNs(value: unknown): bigint | undefined { + if (!isRecord(value)) return undefined; + const sec = readBigInt(value.sec ?? value.seconds); + const nsec = readBigInt(value.nsec ?? value.nanosec ?? value.nanos); + return sec !== undefined && nsec !== undefined ? sec * 1_000_000_000n + nsec : undefined; +} + +function readNumberArray(value: unknown): number[] | null { + if (!Array.isArray(value) && !ArrayBuffer.isView(value)) return null; + const values = Array.from(value as ArrayLike); + return values.every(isFiniteNumber) ? values : null; +} + +function readString(record: Record, keys: readonly string[]): string | undefined { + for (const key of keys) { + const value = record[key]; + if (typeof value === 'string') return value; + } + return undefined; +} + +function readNumber(record: Record, keys: readonly string[]): number | undefined { + for (const key of keys) { + const value = record[key]; + if (isFiniteNumber(value)) return value; + } + return undefined; +} + +function readBigInt(value: unknown): bigint | undefined { + if (typeof value === 'bigint') return value; + if (typeof value === 'number' && Number.isSafeInteger(value)) return BigInt(value); + if (typeof value === 'string' && /^\d+$/.test(value)) return BigInt(value); + return undefined; +} + +function clamp01(value: number): number { + return Math.max(0, Math.min(1, value)); +} + +function isRecord(value: unknown): value is Record { + return typeof value === 'object' && value !== null; +} + +function isFiniteNumber(value: unknown): value is number { + return typeof value === 'number' && Number.isFinite(value); +} diff --git a/src/features/panels/Image/core/sceneMeshTopicBroker.test.ts b/src/features/panels/Image/core/sceneMeshTopicBroker.test.ts new file mode 100644 index 0000000..02567e0 --- /dev/null +++ b/src/features/panels/Image/core/sceneMeshTopicBroker.test.ts @@ -0,0 +1,59 @@ +import { describe, expect, it, vi } from 'vitest'; +import type { HighFrequencyConsumer, Player } from '@/core/types/player'; +import type { MessageEvent } from '@/core/types/ros'; +import { subscribeSceneMeshTopic } from './sceneMeshTopicBroker'; + +function sceneEvent(timestampSec: number): MessageEvent { + return { + topic: '/warehouse/safety_zones', + schemaName: 'foxglove.SceneUpdate', + receiveTime: { sec: timestampSec, nsec: 0 }, + publishTime: { sec: timestampSec, nsec: 0 }, + message: { + entities: [{ + id: 'loading-bay', + timestamp: { sec: timestampSec, nsec: 0 }, + triangles: [{ + points: [ + { x: 0, y: 0, z: 1 }, + { x: 1, y: 0, z: 1 }, + { x: 0, y: 1, z: 1 }, + ], + indices: [0, 1, 2], + }], + }], + }, + sizeInBytes: 1, + }; +} + +describe('subscribeSceneMeshTopic', () => { + it('decodes each all-frame batch once and shares it across panel listeners', () => { + let consumer: HighFrequencyConsumer | undefined; + const register = vi.fn((_id: string, next: HighFrequencyConsumer) => { + consumer = next; + }); + const unregister = vi.fn(); + const player = { + registerHighFrequencyConsumer: register, + unregisterHighFrequencyConsumer: unregister, + } as unknown as Player; + const first = vi.fn(); + const second = vi.fn(); + + const unsubscribeFirst = subscribeSceneMeshTopic(player, '/warehouse/safety_zones', first); + const unsubscribeSecond = subscribeSceneMeshTopic(player, '/warehouse/safety_zones', second); + consumer?.onMessageBatch?.([sceneEvent(1), sceneEvent(2)]); + + expect(register).toHaveBeenCalledTimes(1); + expect(consumer?.mode).toBe('all'); + expect(first).toHaveBeenCalledTimes(2); + expect(second).toHaveBeenCalledTimes(2); + expect(first.mock.calls[0]?.[0]).toBe(second.mock.calls[0]?.[0]); + + unsubscribeFirst(); + expect(unregister).not.toHaveBeenCalled(); + unsubscribeSecond(); + expect(unregister).toHaveBeenCalledTimes(1); + }); +}); diff --git a/src/features/panels/Image/core/sceneMeshTopicBroker.ts b/src/features/panels/Image/core/sceneMeshTopicBroker.ts new file mode 100644 index 0000000..66fd4f5 --- /dev/null +++ b/src/features/panels/Image/core/sceneMeshTopicBroker.ts @@ -0,0 +1,59 @@ +import type { Player } from '@/core/types/player'; +import { toNano } from '@/shared/utils/time'; +import { parseSceneMeshes, type SceneMeshFrame } from './sceneMesh'; + +type SceneMeshListener = (frame: SceneMeshFrame) => void; + +interface SceneMeshBroker { + consumerId: string; + listeners: Set; +} + +const brokersByPlayer = new WeakMap>(); +let nextBrokerId = 1; + +export function subscribeSceneMeshTopic( + player: Player, + topic: string, + listener: SceneMeshListener, +): () => void { + let brokers = brokersByPlayer.get(player); + if (!brokers) { + brokers = new Map(); + brokersByPlayer.set(player, brokers); + } + let broker = brokers.get(topic); + if (!broker) { + broker = { + consumerId: `scene-mesh-broker-${nextBrokerId}`, + listeners: new Set(), + }; + nextBrokerId += 1; + brokers.set(topic, broker); + const activeBroker = broker; + player.registerHighFrequencyConsumer(activeBroker.consumerId, { + topic, + lane: 'pointcloud', + mode: 'all', + onMessageBatch: (messages) => { + for (const event of messages) { + const frame = parseSceneMeshes(event.message, toNano(event.publishTime)); + if (!frame) continue; + for (const activeListener of activeBroker.listeners) activeListener(frame); + } + }, + }); + } + broker.listeners.add(listener); + + return () => { + const activeBrokers = brokersByPlayer.get(player); + const activeBroker = activeBrokers?.get(topic); + if (!activeBroker) return; + activeBroker.listeners.delete(listener); + if (activeBroker.listeners.size > 0) return; + player.unregisterHighFrequencyConsumer(activeBroker.consumerId); + activeBrokers?.delete(topic); + if (activeBrokers?.size === 0) brokersByPlayer.delete(player); + }; +} diff --git a/src/features/panels/Image/defaults.ts b/src/features/panels/Image/defaults.ts index f6444ef..235edae 100644 --- a/src/features/panels/Image/defaults.ts +++ b/src/features/panels/Image/defaults.ts @@ -2,6 +2,14 @@ import type { ImageColorMode } from './core/imageColorMode'; export interface ImageConfig { topic: string; + /** Optional foxglove.ImageAnnotations topic drawn over the image. */ + annotationTopic: string; + /** Whether the configured ImageAnnotations layer is visible. */ + annotationVisible: boolean; + /** Optional foxglove.SceneUpdate topic containing triangle meshes. */ + meshTopic: string; + /** Whether the configured SceneUpdate mesh layer is visible. */ + meshVisible: boolean; // Display backgroundColor: string; showStatusText: boolean; @@ -24,6 +32,10 @@ export interface ImageConfig { export const defaultImageConfig = (): ImageConfig => ({ topic: '', + annotationTopic: '', + annotationVisible: true, + meshTopic: '', + meshVisible: true, backgroundColor: '#000000', showStatusText: true, fitMode: 'contain', diff --git a/src/features/panels/Image/definition.tsx b/src/features/panels/Image/definition.tsx index 140abcc..310a207 100644 --- a/src/features/panels/Image/definition.tsx +++ b/src/features/panels/Image/definition.tsx @@ -26,7 +26,7 @@ export const imagePanelDefinition: PanelDefinition = { ], }, createDefaultConfig: defaultImageConfig, - configSchema: { version: 5, parse: parseImageConfig }, + configSchema: { version: 6, parse: parseImageConfig }, render: ({ player, panelId, config, setConfig }) => ( diff --git a/src/features/panels/Image/foxgloveAdapter.test.ts b/src/features/panels/Image/foxgloveAdapter.test.ts new file mode 100644 index 0000000..bcd3a74 --- /dev/null +++ b/src/features/panels/Image/foxgloveAdapter.test.ts @@ -0,0 +1,21 @@ +import { describe, expect, it } from 'vitest'; +import { imageFoxgloveAdapter } from './foxgloveAdapter'; + +describe('imageFoxgloveAdapter', () => { + it('round-trips annotation and scene mesh topics', () => { + const decoded = imageFoxgloveAdapter.fromConfig({ + topic: '/warehouse/front_camera/image', + annotationTopic: '/warehouse/front_camera/annotations', + meshTopic: '/warehouse/safety_zones', + }); + + const exported = imageFoxgloveAdapter.toConfig({ + config: decoded.config, + extras: decoded.extras, + title: decoded.title, + }); + + expect(exported.annotationTopic).toBe('/warehouse/front_camera/annotations'); + expect(exported.meshTopic).toBe('/warehouse/safety_zones'); + }); +}); diff --git a/src/features/panels/Image/foxgloveAdapter.ts b/src/features/panels/Image/foxgloveAdapter.ts index 0ede7a4..ffd6ddf 100644 --- a/src/features/panels/Image/foxgloveAdapter.ts +++ b/src/features/panels/Image/foxgloveAdapter.ts @@ -21,6 +21,10 @@ import { parseImageConfig } from './schema'; const KNOWN_KEYS = [ 'topic', 'topicPath', + 'annotationTopic', + 'annotationVisible', + 'meshTopic', + 'meshVisible', 'backgroundColor', 'showStatusText', 'fitMode', @@ -62,6 +66,10 @@ function toConfigForFoxgloveType( ? { topicPath: c.topic } : { topic: c.topic }), backgroundColor: c.backgroundColor, + annotationTopic: c.annotationTopic, + annotationVisible: c.annotationVisible, + meshTopic: c.meshTopic, + meshVisible: c.meshVisible, showStatusText: c.showStatusText, fitMode: c.fitMode, smoothing: c.smoothing, diff --git a/src/features/panels/Image/schema.test.ts b/src/features/panels/Image/schema.test.ts index d4fa0d2..dad32df 100644 --- a/src/features/panels/Image/schema.test.ts +++ b/src/features/panels/Image/schema.test.ts @@ -19,6 +19,24 @@ describe('parseImageConfig', () => { expect(config.backgroundColor).toBe(defaultImageConfig().backgroundColor); }); + it('parses an explicit foxglove.ImageAnnotations topic', () => { + expect(parseImageConfig({ + annotationTopic: '/warehouse/front_camera/annotations', + }).annotationTopic).toBe('/warehouse/front_camera/annotations'); + }); + + it('parses an explicit foxglove.SceneUpdate mesh topic', () => { + expect(parseImageConfig({ meshTopic: '/warehouse/safety_zones' }).meshTopic).toBe( + '/warehouse/safety_zones', + ); + }); + + it('parses annotation and scene mesh visibility independently', () => { + const config = parseImageConfig({ annotationVisible: false, meshVisible: false }); + expect(config.annotationVisible).toBe(false); + expect(config.meshVisible).toBe(false); + }); + it('does not expose removed overlay/annotation fields from legacy input', () => { const config = parseImageConfig({ overlays: [{ topic: '/x', opacity: 1, blendMode: 'alpha', enabled: true }], diff --git a/src/features/panels/Image/schema.ts b/src/features/panels/Image/schema.ts index 4c0a872..5eafe3c 100644 --- a/src/features/panels/Image/schema.ts +++ b/src/features/panels/Image/schema.ts @@ -55,6 +55,12 @@ export function parseImageConfig(input: unknown): ImageConfig { return { topic: typeof input.topic === 'string' ? input.topic : base.topic, + annotationTopic: + typeof input.annotationTopic === 'string' ? input.annotationTopic : base.annotationTopic, + annotationVisible: + typeof input.annotationVisible === 'boolean' ? input.annotationVisible : base.annotationVisible, + meshTopic: typeof input.meshTopic === 'string' ? input.meshTopic : base.meshTopic, + meshVisible: typeof input.meshVisible === 'boolean' ? input.meshVisible : base.meshVisible, backgroundColor: typeof input.backgroundColor === 'string' ? input.backgroundColor : base.backgroundColor, showStatusText: diff --git a/src/shared/intl/messages/en/panels.json b/src/shared/intl/messages/en/panels.json index 0cc8a07..aa36415 100644 --- a/src/shared/intl/messages/en/panels.json +++ b/src/shared/intl/messages/en/panels.json @@ -275,6 +275,14 @@ "panels.image.settings.field.topic.label": "Topic", "panels.image.settings.field.topic.help": "ROS image topic (compressed or raw).", "panels.image.settings.field.topic.placeholder": "/camera/.../image_raw", + "panels.image.settings.field.annotationTopic.label": "Annotations", + "panels.image.settings.field.annotationTopic.help": "foxglove.ImageAnnotations topic drawn over the image.", + "panels.image.settings.field.annotationTopic.placeholder": "Select annotations topic", + "panels.image.settings.field.annotationVisible": "Show annotations", + "panels.image.settings.field.meshTopic.label": "Scene mesh", + "panels.image.settings.field.meshTopic.help": "foxglove.SceneUpdate triangle meshes projected using this camera's Double Sphere calibration.", + "panels.image.settings.field.meshTopic.placeholder": "Select scene mesh topic", + "panels.image.settings.field.meshVisible": "Show scene mesh", "panels.image.settings.section.display": "Display", "panels.image.settings.field.showStatusText": "Show status text", "panels.image.settings.field.backgroundColor": "Background color", diff --git a/src/shared/intl/messages/ja/panels.json b/src/shared/intl/messages/ja/panels.json index 048592d..d881888 100644 --- a/src/shared/intl/messages/ja/panels.json +++ b/src/shared/intl/messages/ja/panels.json @@ -275,6 +275,14 @@ "panels.image.settings.field.topic.label": "トピック", "panels.image.settings.field.topic.help": "ROS image topic (compressed or raw).", "panels.image.settings.field.topic.placeholder": "/camera/.../image_raw", + "panels.image.settings.field.annotationTopic.label": "アノテーション", + "panels.image.settings.field.annotationTopic.help": "画像に重ねる foxglove.ImageAnnotations トピック。", + "panels.image.settings.field.annotationTopic.placeholder": "アノテーショントピックを選択", + "panels.image.settings.field.annotationVisible": "アノテーションを表示", + "panels.image.settings.field.meshTopic.label": "シーンメッシュ", + "panels.image.settings.field.meshTopic.help": "このカメラの Double Sphere キャリブレーションで投影する foxglove.SceneUpdate の三角形メッシュ。", + "panels.image.settings.field.meshTopic.placeholder": "シーンメッシュトピックを選択", + "panels.image.settings.field.meshVisible": "シーンメッシュを表示", "panels.image.settings.section.display": "表示", "panels.image.settings.field.showStatusText": "ステータス文字を表示", "panels.image.settings.field.backgroundColor": "背景色", diff --git a/src/shared/intl/messages/zh/panels.json b/src/shared/intl/messages/zh/panels.json index fc2825a..07ad0f9 100644 --- a/src/shared/intl/messages/zh/panels.json +++ b/src/shared/intl/messages/zh/panels.json @@ -275,6 +275,14 @@ "panels.image.settings.field.topic.label": "话题", "panels.image.settings.field.topic.help": "ROS image topic (compressed or raw).", "panels.image.settings.field.topic.placeholder": "/camera/.../image_raw", + "panels.image.settings.field.annotationTopic.label": "标注", + "panels.image.settings.field.annotationTopic.help": "叠加在图像上的 foxglove.ImageAnnotations 话题。", + "panels.image.settings.field.annotationTopic.placeholder": "选择标注话题", + "panels.image.settings.field.annotationVisible": "显示标注", + "panels.image.settings.field.meshTopic.label": "场景网格", + "panels.image.settings.field.meshTopic.help": "使用当前相机 Double Sphere 标定投影 foxglove.SceneUpdate 三角形网格。", + "panels.image.settings.field.meshTopic.placeholder": "选择场景网格话题", + "panels.image.settings.field.meshVisible": "显示场景网格", "panels.image.settings.section.display": "显示", "panels.image.settings.field.showStatusText": "显示状态文字", "panels.image.settings.field.backgroundColor": "背景色",