From c0ca950b24b7e6249f9a279ac7b6f4c6b866f1b5 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 4 Oct 2025 05:09:44 +0300 Subject: [PATCH 1/3] fix websocket vis performance --- dimos/robot/unitree_webrtc/unitree_go2.py | 9 +- .../package-lock.json | 22 ++- .../web/command-center-extension/package.json | 2 + .../src/components/CostmapLayer.tsx | 102 +++++++---- .../src/optimizedCostmap.ts | 118 +++++++++++++ .../web/command-center-extension/src/types.ts | 40 ++--- dimos/web/websocket_vis/optimized_costmap.py | 160 ++++++++++++++++++ .../web/websocket_vis/websocket_vis_module.py | 20 +-- 8 files changed, 404 insertions(+), 69 deletions(-) create mode 100644 dimos/web/command-center-extension/src/optimizedCostmap.ts create mode 100644 dimos/web/websocket_vis/optimized_costmap.py diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 5276586a43..c73da76554 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -406,8 +406,7 @@ def start(self): self._deploy_connection() self._deploy_mapping() self._deploy_navigation() - # self._deploy_visualization() - self._deploy_foxglove_bridge() + self._deploy_visualization() self._deploy_perception() self._deploy_camera() @@ -525,6 +524,12 @@ def _deploy_foxglove_bridge(self): ) self.foxglove_bridge.start() + # TODO: This should be moved. + def _set_goal(goal: LatLon): + self.set_gps_travel_goal_points([goal]) + + unsub = self.websocket_vis.gps_goal.transport.pure_observable().subscribe(_set_goal) + def _deploy_perception(self): """Deploy and configure perception modules.""" # Deploy spatial memory diff --git a/dimos/web/command-center-extension/package-lock.json b/dimos/web/command-center-extension/package-lock.json index 108bfff0be..771bae9aaa 100644 --- a/dimos/web/command-center-extension/package-lock.json +++ b/dimos/web/command-center-extension/package-lock.json @@ -9,7 +9,9 @@ "version": "0.0.0", "license": "UNLICENSED", "dependencies": { + "@types/pako": "^2.0.4", "d3": "^7.9.0", + "pako": "^2.1.0", "react-leaflet": "^4.2.1", "socket.io-client": "^4.8.1" }, @@ -858,6 +860,12 @@ "undici-types": "~7.10.0" } }, + "node_modules/@types/pako": { + "version": "2.0.4", + "resolved": "https://registry.npmjs.org/@types/pako/-/pako-2.0.4.tgz", + "integrity": "sha512-VWDCbrLeVXJM9fihYodcLiIv0ku+AlOa/TQ1SvYOaBuyrSKgEcro95LJyIsJ4vSo6BXIxOKxiJAat04CmST9Fw==", + "license": "MIT" + }, "node_modules/@types/prop-types": { "version": "15.7.15", "resolved": "https://registry.npmjs.org/@types/prop-types/-/prop-types-15.7.15.tgz", @@ -4614,6 +4622,13 @@ "setimmediate": "^1.0.5" } }, + "node_modules/jszip/node_modules/pako": { + "version": "1.0.11", + "resolved": "https://registry.npmjs.org/pako/-/pako-1.0.11.tgz", + "integrity": "sha512-4hLB8Py4zZce5s4yd9XzopqwVv/yGNhV1Bl8NTmCq1763HeK2+EwVTv+leGeL13Dnh2wfbqowVPXCIO0z4taYw==", + "dev": true, + "license": "(MIT AND Zlib)" + }, "node_modules/keyv": { "version": "4.5.4", "resolved": "https://registry.npmjs.org/keyv/-/keyv-4.5.4.tgz", @@ -5131,10 +5146,9 @@ "license": "BlueOak-1.0.0" }, "node_modules/pako": { - "version": "1.0.11", - "resolved": "https://registry.npmjs.org/pako/-/pako-1.0.11.tgz", - "integrity": "sha512-4hLB8Py4zZce5s4yd9XzopqwVv/yGNhV1Bl8NTmCq1763HeK2+EwVTv+leGeL13Dnh2wfbqowVPXCIO0z4taYw==", - "dev": true, + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/pako/-/pako-2.1.0.tgz", + "integrity": "sha512-w+eufiZ1WuJYgPXbV/PO3NCMEc3xqylkKHzp8bxp1uW4qaSNQUkwmLLEc3kKsfz8lpV1F8Ht3U1Cm+9Srog2ug==", "license": "(MIT AND Zlib)" }, "node_modules/parent-module": { diff --git a/dimos/web/command-center-extension/package.json b/dimos/web/command-center-extension/package.json index 000757349e..36eb7854c4 100644 --- a/dimos/web/command-center-extension/package.json +++ b/dimos/web/command-center-extension/package.json @@ -33,7 +33,9 @@ "typescript": "5.9.2" }, "dependencies": { + "@types/pako": "^2.0.4", "d3": "^7.9.0", + "pako": "^2.1.0", "react-leaflet": "^4.2.1", "socket.io-client": "^4.8.1" } diff --git a/dimos/web/command-center-extension/src/components/CostmapLayer.tsx b/dimos/web/command-center-extension/src/components/CostmapLayer.tsx index 0c07d73c85..3881f6f0d5 100644 --- a/dimos/web/command-center-extension/src/components/CostmapLayer.tsx +++ b/dimos/web/command-center-extension/src/components/CostmapLayer.tsx @@ -13,31 +13,22 @@ interface CostmapLayerProps { const CostmapLayer = React.memo(({ costmap, width, height }) => { const canvasRef = React.useRef(null); const { grid, origin, resolution } = costmap; - const rows = grid.shape[0]!; - const cols = grid.shape[1]!; + const rows = Math.max(1, grid.shape[0] || 1); + const cols = Math.max(1, grid.shape[1] || 1); const axisMargin = { left: 60, bottom: 40 }; - const availableWidth = width - axisMargin.left; - const availableHeight = height - axisMargin.bottom; + const availableWidth = Math.max(1, width - axisMargin.left); + const availableHeight = Math.max(1, height - axisMargin.bottom); - const cell = Math.min(availableWidth / cols, availableHeight / rows); - const gridW = cols * cell; - const gridH = rows * cell; + const cell = Math.max(0, Math.min(availableWidth / cols, availableHeight / rows)); + const gridW = Math.max(0, cols * cell); + const gridH = Math.max(0, rows * cell); const offsetX = axisMargin.left + (availableWidth - gridW) / 2; const offsetY = (availableHeight - gridH) / 2; - React.useEffect(() => { - const canvas = canvasRef.current; - if (!canvas) { - return; - } - - canvas.width = cols; - canvas.height = rows; - const ctx = canvas.getContext("2d"); - if (!ctx) { - return; - } + // Pre-compute color lookup table using exact D3 colors (computed once on mount) + const colorLookup = React.useMemo(() => { + const lookup = new Uint8ClampedArray(256 * 3); // RGB values for -1 to 254 (255 total values) const customColorScale = (t: number) => { if (t === 0) { @@ -57,29 +48,82 @@ const CostmapLayer = React.memo(({ costmap, width, height }) }; const colour = d3.scaleSequential(customColorScale).domain([-1, 100]); + + // Pre-compute all 256 possible color values + for (let i = 0; i < 256; i++) { + const value = i === 255 ? -1 : i; + const colorStr = colour(value); + const c = d3.color(colorStr); + + if (c) { + const rgb = c as d3.RGBColor; + lookup[i * 3] = rgb.r; + lookup[i * 3 + 1] = rgb.g; + lookup[i * 3 + 2] = rgb.b; + } else { + lookup[i * 3] = 0; + lookup[i * 3 + 1] = 0; + lookup[i * 3 + 2] = 0; + } + } + + return lookup; + }, []); + + React.useEffect(() => { + const canvas = canvasRef.current; + if (!canvas) { + return; + } + + // Validate grid data length matches dimensions + const expectedLength = rows * cols; + if (grid.data.length !== expectedLength) { + console.warn( + `Grid data length mismatch: expected ${expectedLength}, got ${grid.data.length} (rows=${rows}, cols=${cols})` + ); + } + + canvas.width = cols; + canvas.height = rows; + const ctx = canvas.getContext("2d"); + if (!ctx) { + return; + } + const img = ctx.createImageData(cols, rows); const data = grid.data; + const imgData = img.data; - for (let i = 0; i < data.length; i++) { + for (let i = 0; i < data.length && i < rows * cols; i++) { const row = Math.floor(i / cols); const col = i % cols; const invertedRow = rows - 1 - row; const srcIdx = invertedRow * cols + col; - const value = data[i]!; - const c = d3.color(colour(value)); - if (!c) { + + if (srcIdx < 0 || srcIdx >= data.length) { continue; } + const value = data[i]!; + // Map value to lookup index (handle -1 -> 255 mapping) + const lookupIdx = value === -1 ? 255 : Math.min(254, Math.max(0, value)); + const o = srcIdx * 4; - const rgb = c as d3.RGBColor; - img.data[o] = rgb.r; - img.data[o + 1] = rgb.g; - img.data[o + 2] = rgb.b; - img.data[o + 3] = 255; + if (o < 0 || o + 3 >= imgData.length) { + continue; + } + + // Use pre-computed colors from lookup table + const colorOffset = lookupIdx * 3; + imgData[o] = colorLookup[colorOffset]!; + imgData[o + 1] = colorLookup[colorOffset + 1]!; + imgData[o + 2] = colorLookup[colorOffset + 2]!; + imgData[o + 3] = 255; } + ctx.putImageData(img, 0, 0); - }, [grid.data, cols, rows]); + }, [grid.data, cols, rows, colorLookup]); return ( diff --git a/dimos/web/command-center-extension/src/optimizedCostmap.ts b/dimos/web/command-center-extension/src/optimizedCostmap.ts new file mode 100644 index 0000000000..9e3795c142 --- /dev/null +++ b/dimos/web/command-center-extension/src/optimizedCostmap.ts @@ -0,0 +1,118 @@ +import * as pako from 'pako'; + +export interface EncodedOptimizedGrid { + update_type: "full" | "delta"; + shape: [number, number]; + dtype: string; + compressed: boolean; + compression?: "zlib" | "none"; + data?: string; + chunks?: Array<{ + pos: [number, number]; + size: [number, number]; + data: string; + }>; +} + +export class OptimizedGrid { + private fullGrid: Uint8Array | null = null; + private shape: [number, number] = [0, 0]; + + decode(msg: EncodedOptimizedGrid): Float32Array { + if (msg.update_type === "full") { + return this.decodeFull(msg); + } else { + return this.decodeDelta(msg); + } + } + + private decodeFull(msg: EncodedOptimizedGrid): Float32Array { + if (!msg.data) { + throw new Error("Missing data for full update"); + } + + const binaryString = atob(msg.data); + const compressed = new Uint8Array(binaryString.length); + for (let i = 0; i < binaryString.length; i++) { + compressed[i] = binaryString.charCodeAt(i); + } + + // Decompress if needed + let decompressed: Uint8Array; + if (msg.compressed && msg.compression === "zlib") { + decompressed = pako.inflate(compressed); + } else { + decompressed = compressed; + } + + // Store for delta updates + this.fullGrid = decompressed; + this.shape = msg.shape; + + // Convert uint8 back to float32 costmap values + const float32Data = new Float32Array(decompressed.length); + for (let i = 0; i < decompressed.length; i++) { + // Map 255 back to -1 for unknown cells + const val = decompressed[i]!; + float32Data[i] = val === 255 ? -1 : val; + } + + return float32Data; + } + + private decodeDelta(msg: EncodedOptimizedGrid): Float32Array { + if (!this.fullGrid) { + throw new Error("No full grid available for delta update"); + } + + if (!msg.chunks) { + throw new Error("Missing chunks for delta update"); + } + + // Apply delta updates to the full grid + for (const chunk of msg.chunks) { + const [y, x] = chunk.pos; + const [h, w] = chunk.size; + + // Decode chunk data + const binaryString = atob(chunk.data); + const compressed = new Uint8Array(binaryString.length); + for (let i = 0; i < binaryString.length; i++) { + compressed[i] = binaryString.charCodeAt(i); + } + + let decompressed: Uint8Array; + if (msg.compressed && msg.compression === "zlib") { + decompressed = pako.inflate(compressed); + } else { + decompressed = compressed; + } + + // Update the full grid with chunk data + const width = this.shape[1]; + let chunkIdx = 0; + for (let cy = 0; cy < h; cy++) { + for (let cx = 0; cx < w; cx++) { + const gridIdx = (y + cy) * width + (x + cx); + const val = decompressed[chunkIdx++]; + if (val !== undefined) { + this.fullGrid[gridIdx] = val; + } + } + } + } + + // Convert to float32 + const float32Data = new Float32Array(this.fullGrid.length); + for (let i = 0; i < this.fullGrid.length; i++) { + const val = this.fullGrid[i]!; + float32Data[i] = val === 255 ? -1 : val; + } + + return float32Data; + } + + getShape(): [number, number] { + return this.shape; + } +} diff --git a/dimos/web/command-center-extension/src/types.ts b/dimos/web/command-center-extension/src/types.ts index ce02e37496..5f3a804a9c 100644 --- a/dimos/web/command-center-extension/src/types.ts +++ b/dimos/web/command-center-extension/src/types.ts @@ -1,3 +1,5 @@ +import { EncodedOptimizedGrid, OptimizedGrid } from './optimizedCostmap'; + export type EncodedVector = Encoded<"vector"> & { c: number[]; }; @@ -32,7 +34,7 @@ export class Path { } export type EncodedCostmap = Encoded<"costmap"> & { - grid: EncodedGrid; + grid: EncodedOptimizedGrid; origin: EncodedVector; resolution: number; origin_theta: number; @@ -51,9 +53,22 @@ export class Costmap { this.origin_theta = origin_theta; } + private static decoder: OptimizedGrid | null = null; + static decode(data: EncodedCostmap): Costmap { + // Use a singleton decoder to maintain state for delta updates + if (!Costmap.decoder) { + Costmap.decoder = new OptimizedGrid(); + } + + const float32Data = Costmap.decoder.decode(data.grid); + const shape = data.grid.shape; + + // Create a Grid object from the decoded data + const grid = new Grid(float32Data, shape); + return new Costmap( - Grid.decode(data.grid), + grid, Vector.decode(data.origin), data.resolution, data.origin_theta, @@ -61,32 +76,11 @@ export class Costmap { } } -const DTYPE = { - f32: Float32Array, - f64: Float64Array, - i32: Int32Array, - i8: Int8Array, -}; - -export type EncodedGrid = Encoded<"grid"> & { - shape: [number, number]; - dtype: keyof typeof DTYPE; - compressed: boolean; - data: string; -}; - export class Grid { constructor( public data: Float32Array | Float64Array | Int32Array | Int8Array, public shape: number[], ) {} - - static decode(msg: EncodedGrid): Grid { - const bytes = Uint8Array.from(atob(msg.data), (c) => c.charCodeAt(0)); - const raw = bytes; - const Arr = DTYPE[msg.dtype]; - return new Grid(new Arr(raw.buffer), msg.shape); - } } export type Drawable = Costmap | Vector | Path; diff --git a/dimos/web/websocket_vis/optimized_costmap.py b/dimos/web/websocket_vis/optimized_costmap.py new file mode 100644 index 0000000000..a0013d805e --- /dev/null +++ b/dimos/web/websocket_vis/optimized_costmap.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 + +# Copyright 2025 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2025 Dimensional Inc. + +import base64 +import hashlib +import time +from typing import Dict, Any, Optional, Tuple +import numpy as np +import zlib + + +class OptimizedCostmapEncoder: + """Handles optimized encoding of costmaps with delta compression.""" + + def __init__(self, chunk_size: int = 64): + self.chunk_size = chunk_size + self.last_full_grid: Optional[np.ndarray] = None + self.last_sent_time: float = 0 + self.chunk_hashes: Dict[Tuple[int, int], str] = {} + self.full_update_interval = 20.0 # Send full update every 20 seconds + + def encode_costmap(self, grid: np.ndarray, force_full: bool = False) -> Dict[str, Any]: + """Encode a costmap grid with optimizations. + + Args: + grid: The costmap grid as numpy array + force_full: Force sending a full update + + Returns: + Encoded costmap data + """ + current_time = time.time() + + # Determine if we need a full update + send_full = ( + force_full + or self.last_full_grid is None + or self.last_full_grid.shape != grid.shape + or (current_time - self.last_sent_time) > self.full_update_interval + ) + + if send_full: + return self._encode_full(grid, current_time) + else: + return self._encode_delta(grid, current_time) + + def _encode_full(self, grid: np.ndarray, current_time: float) -> Dict[str, Any]: + height, width = grid.shape + + # Convert to uint8 for better compression (costmap values are -1 to 100) + # Map -1 to 255 for unknown cells + grid_uint8 = grid.astype(np.int16) + grid_uint8[grid_uint8 == -1] = 255 + grid_uint8 = grid_uint8.astype(np.uint8) + + # Compress the data + compressed = zlib.compress(grid_uint8.tobytes(), level=6) + + # Base64 encode + encoded = base64.b64encode(compressed).decode("ascii") + + # Update state + self.last_full_grid = grid.copy() + self.last_sent_time = current_time + self._update_chunk_hashes(grid) + + return { + "update_type": "full", + "shape": [height, width], + "dtype": "u8", # uint8 + "compressed": True, + "compression": "zlib", + "data": encoded, + } + + def _encode_delta(self, grid: np.ndarray, current_time: float) -> Dict[str, Any]: + height, width = grid.shape + changed_chunks = [] + + # Divide grid into chunks and check for changes + for y in range(0, height, self.chunk_size): + for x in range(0, width, self.chunk_size): + # Get chunk bounds + y_end = min(y + self.chunk_size, height) + x_end = min(x + self.chunk_size, width) + + # Extract chunk + chunk = grid[y:y_end, x:x_end] + + # Compute hash of chunk + chunk_hash = hashlib.md5(chunk.tobytes()).hexdigest() + chunk_key = (y, x) + + # Check if chunk has changed + if chunk_key not in self.chunk_hashes or self.chunk_hashes[chunk_key] != chunk_hash: + # Chunk has changed, encode it + chunk_uint8 = chunk.astype(np.int16) + chunk_uint8[chunk_uint8 == -1] = 255 + chunk_uint8 = chunk_uint8.astype(np.uint8) + + # Compress chunk + compressed = zlib.compress(chunk_uint8.tobytes(), level=6) + encoded = base64.b64encode(compressed).decode("ascii") + + changed_chunks.append( + {"pos": [y, x], "size": [y_end - y, x_end - x], "data": encoded} + ) + + # Update hash + self.chunk_hashes[chunk_key] = chunk_hash + + # Update state + self.last_full_grid = grid.copy() + self.last_sent_time = current_time + + # If too many chunks changed, send full update instead + total_chunks = ((height + self.chunk_size - 1) // self.chunk_size) * ( + (width + self.chunk_size - 1) // self.chunk_size + ) + + if len(changed_chunks) > total_chunks * 0.5: + # More than 50% changed, send full update + return self._encode_full(grid, current_time) + + return { + "update_type": "delta", + "shape": [height, width], + "dtype": "u8", + "compressed": True, + "compression": "zlib", + "chunks": changed_chunks, + } + + def _update_chunk_hashes(self, grid: np.ndarray): + """Update all chunk hashes for the grid.""" + self.chunk_hashes.clear() + height, width = grid.shape + + for y in range(0, height, self.chunk_size): + for x in range(0, width, self.chunk_size): + y_end = min(y + self.chunk_size, height) + x_end = min(x + self.chunk_size, width) + chunk = grid[y:y_end, x:x_end] + chunk_hash = hashlib.md5(chunk.tobytes()).hexdigest() + self.chunk_hashes[(y, x)] = chunk_hash diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index f8440d4b20..004853a2d6 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -38,6 +38,7 @@ from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.utils.logging_config import setup_logger from reactivex.disposable import Disposable +from .optimized_costmap import OptimizedCostmapEncoder logger = setup_logger("dimos.web.websocket_vis") @@ -94,6 +95,8 @@ def __init__(self, port: int = 7779, **kwargs): self.vis_state = {} self.state_lock = threading.Lock() + self.costmap_encoder = OptimizedCostmapEncoder(chunk_size=64) + logger.info(f"WebSocket visualization module initialized on port {port}") def _start_broadcast_loop(self) -> None: @@ -183,6 +186,10 @@ async def serve_index(request): async def connect(sid, environ): with self.state_lock: current_state = dict(self.vis_state) + + # Force full costmap update on new connection + self.costmap_encoder.last_full_grid = None + await self.sio.emit("full_state", current_state, room=sid) @self.sio.event @@ -268,20 +275,11 @@ def _on_global_costmap(self, msg: OccupancyGrid): def _process_costmap(self, costmap: OccupancyGrid) -> Dict[str, Any]: """Convert OccupancyGrid to visualization format.""" costmap = costmap.inflate(0.1).gradient(max_distance=1.0) - - # Convert grid data to base64 encoded string - grid_bytes = costmap.grid.astype(np.float32).tobytes() - grid_base64 = base64.b64encode(grid_bytes).decode("ascii") + grid_data = self.costmap_encoder.encode_costmap(costmap.grid) return { "type": "costmap", - "grid": { - "type": "grid", - "shape": [costmap.height, costmap.width], - "dtype": "f32", - "compressed": False, - "data": grid_base64, - }, + "grid": grid_data, "origin": { "type": "vector", "c": [costmap.origin.position.x, costmap.origin.position.y, 0], From 4dea67e37cb603552014fcd5b285b84ddfd7f8d4 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 8 Oct 2025 03:26:31 +0300 Subject: [PATCH 2/3] fix --- dimos/web/command-center-extension/src/optimizedCostmap.ts | 4 +++- dimos/web/websocket_vis/optimized_costmap.py | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/web/command-center-extension/src/optimizedCostmap.ts b/dimos/web/command-center-extension/src/optimizedCostmap.ts index 9e3795c142..2244437eab 100644 --- a/dimos/web/command-center-extension/src/optimizedCostmap.ts +++ b/dimos/web/command-center-extension/src/optimizedCostmap.ts @@ -62,7 +62,9 @@ export class OptimizedGrid { private decodeDelta(msg: EncodedOptimizedGrid): Float32Array { if (!this.fullGrid) { - throw new Error("No full grid available for delta update"); + console.warn("No full grid available for delta update - skipping until full update arrives"); + const size = msg.shape[0] * msg.shape[1]; + return new Float32Array(size).fill(-1); } if (!msg.chunks) { diff --git a/dimos/web/websocket_vis/optimized_costmap.py b/dimos/web/websocket_vis/optimized_costmap.py index a0013d805e..21874128b8 100644 --- a/dimos/web/websocket_vis/optimized_costmap.py +++ b/dimos/web/websocket_vis/optimized_costmap.py @@ -32,7 +32,7 @@ def __init__(self, chunk_size: int = 64): self.last_full_grid: Optional[np.ndarray] = None self.last_sent_time: float = 0 self.chunk_hashes: Dict[Tuple[int, int], str] = {} - self.full_update_interval = 20.0 # Send full update every 20 seconds + self.full_update_interval = 3.0 # Send full update every 3 seconds def encode_costmap(self, grid: np.ndarray, force_full: bool = False) -> Dict[str, Any]: """Encode a costmap grid with optimizations. From 37ff0f1f11599b05a21dc316296d701797361bf9 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 13 Oct 2025 22:17:53 +0300 Subject: [PATCH 3/3] code review change --- dimos/robot/unitree_webrtc/unitree_go2.py | 14 +------------- dimos/web/websocket_vis/optimized_costmap.py | 9 ++++----- 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index c73da76554..058ee28c4b 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -407,6 +407,7 @@ def start(self): self._deploy_mapping() self._deploy_navigation() self._deploy_visualization() + self._deploy_foxglove_bridge() self._deploy_perception() self._deploy_camera() @@ -508,13 +509,6 @@ def _deploy_visualization(self): self.websocket_vis.path.connect(self.global_planner.path) self.websocket_vis.global_costmap.connect(self.mapper.global_costmap) - # TODO: This should be moved. - def _set_goal(goal: LatLon): - self.set_gps_travel_goal_points([goal]) - - unsub = self.websocket_vis.gps_goal.transport.pure_observable().subscribe(_set_goal) - self._disposables.add(unsub) - def _deploy_foxglove_bridge(self): self.foxglove_bridge = FoxgloveBridge( shm_channels=[ @@ -524,12 +518,6 @@ def _deploy_foxglove_bridge(self): ) self.foxglove_bridge.start() - # TODO: This should be moved. - def _set_goal(goal: LatLon): - self.set_gps_travel_goal_points([goal]) - - unsub = self.websocket_vis.gps_goal.transport.pure_observable().subscribe(_set_goal) - def _deploy_perception(self): """Deploy and configure perception modules.""" # Deploy spatial memory diff --git a/dimos/web/websocket_vis/optimized_costmap.py b/dimos/web/websocket_vis/optimized_costmap.py index 21874128b8..30a226c66f 100644 --- a/dimos/web/websocket_vis/optimized_costmap.py +++ b/dimos/web/websocket_vis/optimized_costmap.py @@ -30,7 +30,7 @@ class OptimizedCostmapEncoder: def __init__(self, chunk_size: int = 64): self.chunk_size = chunk_size self.last_full_grid: Optional[np.ndarray] = None - self.last_sent_time: float = 0 + self.last_full_sent_time: float = 0 # Track when last full update was sent self.chunk_hashes: Dict[Tuple[int, int], str] = {} self.full_update_interval = 3.0 # Send full update every 3 seconds @@ -51,7 +51,7 @@ def encode_costmap(self, grid: np.ndarray, force_full: bool = False) -> Dict[str force_full or self.last_full_grid is None or self.last_full_grid.shape != grid.shape - or (current_time - self.last_sent_time) > self.full_update_interval + or (current_time - self.last_full_sent_time) > self.full_update_interval ) if send_full: @@ -76,7 +76,7 @@ def _encode_full(self, grid: np.ndarray, current_time: float) -> Dict[str, Any]: # Update state self.last_full_grid = grid.copy() - self.last_sent_time = current_time + self.last_full_sent_time = current_time self._update_chunk_hashes(grid) return { @@ -124,9 +124,8 @@ def _encode_delta(self, grid: np.ndarray, current_time: float) -> Dict[str, Any] # Update hash self.chunk_hashes[chunk_key] = chunk_hash - # Update state + # Update state - only update the grid, not the timer self.last_full_grid = grid.copy() - self.last_sent_time = current_time # If too many chunks changed, send full update instead total_chunks = ((height + self.chunk_size - 1) // self.chunk_size) * (