Fix websocketvis performance rebase

dimos/robot/unitree_webrtc/unitree_go2.py

@@ -406,7 +406,7 @@ def start(self):
         self._deploy_connection()
         self._deploy_mapping()
         self._deploy_navigation()
-        # self._deploy_visualization()
+        self._deploy_visualization()
         self._deploy_foxglove_bridge()
         self._deploy_perception()
         self._deploy_camera()
@@ -509,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=[

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"
   }

dimos/web/command-center-extension/src/components/CostmapLayer.tsx

@@ -13,31 +13,22 @@ interface CostmapLayerProps {
 const CostmapLayer = React.memo<CostmapLayerProps>(({ costmap, width, height }) => {
   const canvasRef = React.useRef<HTMLCanvasElement>(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<CostmapLayerProps>(({ 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 (
     <g transform={`translate(${offsetX}, ${offsetY})`}>

dimos/web/command-center-extension/src/optimizedCostmap.ts

@@ -0,0 +1,120 @@
+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) {
+      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) {
+      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;
+  }
+}