Foxglove extension

dimos/robot/unitree_webrtc/unitree_go2.py

@@ -372,6 +372,8 @@ def _deploy_visualization(self):
         """Deploy and configure visualization modules."""
         self.websocket_vis = self.dimos.deploy(WebsocketVisModule, port=self.websocket_port)
         self.websocket_vis.click_goal.transport = core.LCMTransport("/goal_request", PoseStamped)
+        self.websocket_vis.explore_cmd.transport = core.LCMTransport("/explore_cmd", Bool)
+        self.websocket_vis.stop_explore_cmd.transport = core.LCMTransport("/stop_explore_cmd", Bool)
 
         self.websocket_vis.robot_pose.connect(self.connection.odom)
         self.websocket_vis.path.connect(self.global_planner.path)

dimos/web/command-center-extension/.gitignore

@@ -0,0 +1,3 @@
+*.foxe
+/dist
+/node_modules

dimos/web/command-center-extension/.prettierrc.yaml

@@ -0,0 +1,5 @@
+arrowParens: always
+printWidth: 100
+trailingComma: "all"
+tabWidth: 2
+semi: true

dimos/web/command-center-extension/README.md

@@ -0,0 +1,15 @@
+# command-center-extension
+
+## Usage
+
+Install the Foxglove Studio desktop application.
+
+Install the Node dependencies:
+
+    npm install
+
+Build the package and install it into Foxglove:
+
+    npm run build && npm run local-install
+
+To add the panel, go to Foxglove Studio, click on the "Add panel" icon on the top right and select "command-center [local]".

dimos/web/command-center-extension/eslint.config.js

@@ -0,0 +1,23 @@
+// @ts-check
+
+const foxglove = require("@foxglove/eslint-plugin");
+const globals = require("globals");
+const tseslint = require("typescript-eslint");
+
+module.exports = tseslint.config({
+  files: ["src/**/*.ts", "src/**/*.tsx"],
+  extends: [foxglove.configs.base, foxglove.configs.react, foxglove.configs.typescript],
+  languageOptions: {
+    globals: {
+      ...globals.es2020,
+      ...globals.browser,
+    },
+    parserOptions: {
+      project: "tsconfig.json",
+      tsconfigRootDir: __dirname,
+    },
+  },
+  rules: {
+    "react-hooks/exhaustive-deps": "error",
+  },
+});

dimos/web/command-center-extension/src/App.tsx

@@ -0,0 +1,63 @@
+import * as React from "react";
+
+import Connection from "./Connection";
+import ControlPanel from "./ControlPanel";
+import VisualizerWrapper from "./components/VisualizerWrapper";
+import { AppAction, AppState } from "./types";
+
+function appReducer(state: AppState, action: AppAction): AppState {
+  switch (action.type) {
+    case "SET_COSTMAP":
+      return { ...state, costmap: action.payload };
+    case "SET_ROBOT_POSE":
+      return { ...state, robotPose: action.payload };
+    case "SET_PATH":
+      return { ...state, path: action.payload };
+    case "SET_FULL_STATE":
+      return { ...state, ...action.payload };
+    default:
+      return state;
+  }
+}
+
+const initialState: AppState = {
+  costmap: null,
+  robotPose: null,
+  path: null,
+};
+
+export default function App(): React.ReactElement {
+  const [state, dispatch] = React.useReducer(appReducer, initialState);
+  const connectionRef = React.useRef<Connection | null>(null);
+
+  React.useEffect(() => {
+    connectionRef.current = new Connection(dispatch);
+
+    return () => {
+      if (connectionRef.current) {
+        connectionRef.current.disconnect();
+      }
+    };
+  }, []);
+
+  const handleWorldClick = React.useCallback((worldX: number, worldY: number) => {
+    connectionRef.current?.worldClick(worldX, worldY);
+  }, []);
+
+  const handleStartExplore = React.useCallback(() => {
+    connectionRef.current?.startExplore();
+  }, []);
+
+  const handleStopExplore = React.useCallback(() => {
+    connectionRef.current?.stopExplore();
+  }, []);
+
+  return (
+    <div style={{ position: "relative", width: "100%", height: "100%" }}>
+      <VisualizerWrapper data={state} onWorldClick={handleWorldClick} />
+      <div style={{ position: "absolute", bottom: 0, left: 0 }}>
+        <ControlPanel onStartExplore={handleStartExplore} onStopExplore={handleStopExplore} />
+      </div>
+    </div>
+  );
+}

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

@@ -0,0 +1,69 @@
+import { io, Socket } from "socket.io-client";
+
+import {
+  AppAction,
+  Costmap,
+  EncodedCostmap,
+  EncodedPath,
+  EncodedVector,
+  FullStateData,
+  Path,
+  Vector,
+} from "./types";
+
+export default class Connection {
+  socket: Socket;
+  dispatch: React.Dispatch<AppAction>;
+
+  constructor(dispatch: React.Dispatch<AppAction>) {
+    this.dispatch = dispatch;
+    this.socket = io("ws://localhost:7779");
+
+    this.socket.on("costmap", (data: EncodedCostmap) => {
+      const costmap = Costmap.decode(data);
+      this.dispatch({ type: "SET_COSTMAP", payload: costmap });
+    });
+
+    this.socket.on("robot_pose", (data: EncodedVector) => {
+      const robotPose = Vector.decode(data);
+      this.dispatch({ type: "SET_ROBOT_POSE", payload: robotPose });
+    });
+
+    this.socket.on("path", (data: EncodedPath) => {
+      const path = Path.decode(data);
+      this.dispatch({ type: "SET_PATH", payload: path });
+    });
+
+    this.socket.on("full_state", (data: FullStateData) => {
+      const state: Partial<{ costmap: Costmap; robotPose: Vector; path: Path }> = {};
+
+      if (data.costmap != undefined) {
+        state.costmap = Costmap.decode(data.costmap);
+      }
+      if (data.robot_pose != undefined) {
+        state.robotPose = Vector.decode(data.robot_pose);
+      }
+      if (data.path != undefined) {
+        state.path = Path.decode(data.path);
+      }
+
+      this.dispatch({ type: "SET_FULL_STATE", payload: state });
+    });
+  }
+
+  worldClick(worldX: number, worldY: number): void {
+    this.socket.emit("click", [worldX, worldY]);
+  }
+
+  startExplore(): void {
+    this.socket.emit("start_explore");
+  }
+
+  stopExplore(): void {
+    this.socket.emit("stop_explore");
+  }
+
+  disconnect(): void {
+    this.socket.disconnect();
+  }
+}

dimos/web/command-center-extension/src/ControlPanel.tsx

@@ -0,0 +1,37 @@
+import * as React from "react";
+
+interface ControlPanelProps {
+  onStartExplore: () => void;
+  onStopExplore: () => void;
+}
+
+export default function ControlPanel({
+  onStartExplore,
+  onStopExplore,
+}: ControlPanelProps): React.ReactElement {
+  const [exploring, setExploring] = React.useState(false);
+
+  return (
+    <div style={{ width: "100%", padding: 5 }}>
+      {exploring ? (
+        <button
+          onClick={() => {
+            onStopExplore();
+            setExploring(false);
+          }}
+        >
+          Stop Exploration
+        </button>
+      ) : (
+        <button
+          onClick={() => {
+            onStartExplore();
+            setExploring(true);
+          }}
+        >
+          Start Exploration
+        </button>
+      )}
+    </div>
+  );
+}

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

@@ -0,0 +1,121 @@
+import * as d3 from "d3";
+import * as React from "react";
+
+import { Costmap } from "../types";
+import GridLayer from "./GridLayer";
+
+interface CostmapLayerProps {
+  costmap: Costmap;
+  width: number;
+  height: number;
+}
+
+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 axisMargin = { left: 60, bottom: 40 };
+  const availableWidth = width - axisMargin.left;
+  const availableHeight = height - axisMargin.bottom;
+
+  const cell = Math.min(availableWidth / cols, availableHeight / rows);
+  const gridW = cols * cell;
+  const gridH = 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;
+    }
+
+    const customColorScale = (t: number) => {
+      if (t === 0) {
+        return "black";
+      }
+      if (t < 0) {
+        return "#2d2136";
+      }
+      if (t > 0.95) {
+        return "#000000";
+      }
+
+      const color = d3.interpolateTurbo(t * 2 - 1);
+      const hsl = d3.hsl(color);
+      hsl.s *= 0.75;
+      return hsl.toString();
+    };
+
+    const colour = d3.scaleSequential(customColorScale).domain([-1, 100]);
+    const img = ctx.createImageData(cols, rows);
+    const data = grid.data;
+
+    for (let i = 0; i < data.length; 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) {
+        continue;
+      }
+
+      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;
+    }
+    ctx.putImageData(img, 0, 0);
+  }, [grid.data, cols, rows]);
+
+  return (
+    <g transform={`translate(${offsetX}, ${offsetY})`}>
+      <foreignObject width={gridW} height={gridH}>
+        <div
+          style={{
+            width: "100%",
+            height: "100%",
+            display: "flex",
+            alignItems: "center",
+            justifyContent: "center",
+          }}
+        >
+          <canvas
+            ref={canvasRef}
+            style={{
+              width: "100%",
+              height: "100%",
+              objectFit: "contain",
+              backgroundColor: "black",
+            }}
+          />
+        </div>
+      </foreignObject>
+      <GridLayer
+        width={gridW}
+        height={gridH}
+        origin={origin}
+        resolution={resolution}
+        rows={rows}
+        cols={cols}
+      />
+    </g>
+  );
+});
+
+CostmapLayer.displayName = "CostmapLayer";
+
+export default CostmapLayer;