diff --git a/.github/workflows/build-viewer.yml b/.github/workflows/build-viewer.yml new file mode 100644 index 000000000000..f31d00aa98e8 --- /dev/null +++ b/.github/workflows/build-viewer.yml @@ -0,0 +1,313 @@ +# Build & publish dimos-viewer wheels. +# +# Closely follows Rerun's reusable_build_and_upload_wheels.yml pattern: +# - manylinux_2_28 Docker container for Linux builds +# - Same system deps as Rerun's ci_docker Dockerfile +# - Same Rust toolchain (from rust-toolchain file) +# - Same shell defaults and env conventions +# - GitHub Artifacts instead of GCS for wheel storage +# - Trusted publisher (OIDC) for PyPI instead of API token +# +# Triggers: +# PR → main build + test +# push → main build + test +# tag v* build + test + publish to PyPI + GitHub Release + +name: Build & Publish dimos-viewer + +on: + push: + branches: [main] + tags: ['v*'] + pull_request: + branches: [main] + workflow_dispatch: + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: write + id-token: write + +defaults: + run: + shell: bash --noprofile --norc -euo pipefail {0} + +env: + PYTHON_VERSION: "3.10" + PACKAGE_DIR: dimos + +# --------------------------------------------------------------------------- +jobs: + + # ------------------------------------------------------------------- + # 1. Rust checks & tests (native Ubuntu, no container needed) + # ------------------------------------------------------------------- + check: + name: Cargo check & test + runs-on: ubuntu-22.04 + timeout-minutes: 60 + steps: + - uses: actions/checkout@v4 + + - name: Install Rust + run: | + rustup install + rustup show active-toolchain + + - name: Install system dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + libgtk-3-dev libxcb-render0-dev libxcb-shape0-dev \ + libxcb-xfixes0-dev libxkbcommon-dev libvulkan-dev \ + mesa-vulkan-drivers libxkbcommon-x11-0 + + - uses: actions/cache@v4 + with: + path: | + ~/.cargo/registry/index/ + ~/.cargo/registry/cache/ + ~/.cargo/git/db/ + target/ + key: ${{ runner.os }}-cargo-check-${{ hashFiles('**/Cargo.lock') }} + + - run: cargo check -p dimos-viewer + - run: cargo test -p dimos-viewer + + # ------------------------------------------------------------------- + # 2. Build wheels per platform + # Linux: manylinux_2_28 container with Rerun's exact deps + # macOS: native runner + # ------------------------------------------------------------------- + build-wheel: + name: Build wheel (${{ matrix.platform }}-py${{ matrix.python-version }}) + needs: [check] + timeout-minutes: 60 + strategy: + fail-fast: false + matrix: + include: + - platform: linux-x64 + python-version: "3.10" + python-tag: cp310-cp310 + os: ubuntu-22.04 + target: x86_64-unknown-linux-gnu + container: quay.io/pypa/manylinux_2_28_x86_64 + compatibility: manylinux_2_28 + - platform: linux-x64 + python-version: "3.11" + python-tag: cp311-cp311 + os: ubuntu-22.04 + target: x86_64-unknown-linux-gnu + container: quay.io/pypa/manylinux_2_28_x86_64 + compatibility: manylinux_2_28 + - platform: linux-x64 + python-version: "3.12" + python-tag: cp312-cp312 + os: ubuntu-22.04 + target: x86_64-unknown-linux-gnu + container: quay.io/pypa/manylinux_2_28_x86_64 + compatibility: manylinux_2_28 + - platform: macos-arm64 + python-version: "3.10" + python-tag: cp310-cp310 + os: macos-14 + target: aarch64-apple-darwin + container: "" + compatibility: "" + - platform: macos-arm64 + python-version: "3.11" + python-tag: cp311-cp311 + os: macos-14 + target: aarch64-apple-darwin + container: "" + compatibility: "" + - platform: macos-arm64 + python-version: "3.12" + python-tag: cp312-cp312 + os: macos-14 + target: aarch64-apple-darwin + container: "" + compatibility: "" + + runs-on: ${{ matrix.os }} + container: ${{ matrix.container || null }} + + steps: + - uses: actions/checkout@v4 + + # --- Linux (inside manylinux_2_28 container) --- + # Replicates Rerun's ci_docker/Dockerfile setup inline + - name: Install system dependencies (manylinux) + if: matrix.container != '' + run: | + dnf install -y \ + make automake gcc gcc-c++ kernel-devel \ + cmake \ + curl \ + git \ + atk at-spi2-atk \ + fontconfig-devel \ + freetype-devel \ + glib2-devel \ + gtk3-devel \ + openssl-devel \ + xcb-util-renderutil-devel \ + xcb-util-devel \ + xcb-util-wm-devel \ + libxkbcommon-devel \ + python3-pip + dnf clean all + + - name: Install Rust (manylinux) + if: matrix.container != '' + env: + RUSTUP_HOME: /usr/local/rustup + CARGO_HOME: /usr/local/cargo + run: | + curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y + echo "/usr/local/cargo/bin" >> "$GITHUB_PATH" + + - name: Set up Rust toolchain (manylinux) + if: matrix.container != '' + run: | + rustup install + rustup show active-toolchain + + - name: Install maturin (manylinux) + if: matrix.container != '' + run: | + /opt/python/${{ matrix.python-tag }}/bin/python3 -m pip install --upgrade pip + /opt/python/${{ matrix.python-tag }}/bin/python3 -m pip install maturin>=1.8.1 + echo "/opt/python/${{ matrix.python-tag }}/bin" >> "$GITHUB_PATH" + + - name: Build wheel (manylinux) + if: matrix.container != '' + run: | + maturin build \ + --release \ + --manifest-path "${{ env.PACKAGE_DIR }}/Cargo.toml" \ + --target "${{ matrix.target }}" \ + --compatibility "${{ matrix.compatibility }}" \ + --out dist/ + + # --- macOS (native runner) --- + - name: Install Rust (macOS) + if: matrix.container == '' + run: | + rustup install + rustup show active-toolchain + + - name: Set up Python (macOS) + if: matrix.container == '' + uses: actions/setup-python@v5 + with: + python-version: "${{ matrix.python-version }}" + + - name: Install maturin (macOS) + if: matrix.container == '' + run: | + python3 -m pip install maturin>=1.8.1 + + - name: Build wheel (macOS) + if: matrix.container == '' + run: | + maturin build \ + --release \ + --manifest-path "${{ env.PACKAGE_DIR }}/Cargo.toml" \ + --target "${{ matrix.target }}" \ + --out dist/ + + # --- Common --- + - name: Upload wheel artifact + uses: actions/upload-artifact@v4 + with: + name: wheel-${{ matrix.platform }}-py${{ matrix.python-version }} + path: dist/*.whl + + # ------------------------------------------------------------------- + # 3. Test installed wheels + # ------------------------------------------------------------------- + test-wheel: + name: Test wheel (py${{ matrix.python-version }}) + needs: [build-wheel] + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + python-version: ["3.10", "3.11", "3.12"] + + steps: + - uses: actions/checkout@v4 + + - uses: actions/setup-python@v5 + with: + python-version: "${{ matrix.python-version }}" + + - name: Download linux-x64 wheel + uses: actions/download-artifact@v4 + with: + name: wheel-linux-x64-py${{ matrix.python-version }} + path: dist/ + + - name: Install wheel + run: pip install dist/*.whl + + - name: Verify binary + run: | + which dimos-viewer + dimos-viewer --help + + # ------------------------------------------------------------------- + # 4. Publish to PyPI (tag v* only) + # Uses trusted publisher (OIDC) — no API token needed. + # ------------------------------------------------------------------- + publish-pypi: + name: Publish to PyPI + needs: [test-wheel] + if: startsWith(github.ref, 'refs/tags/v') + runs-on: ubuntu-22.04 + permissions: + id-token: write + + steps: + - name: Download all wheels + uses: actions/download-artifact@v4 + with: + pattern: wheel-* + merge-multiple: true + path: dist/ + + - run: ls -la dist/ + + - name: Publish to PyPI (trusted publisher) + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages-dir: dist/ + skip-existing: true + + # ------------------------------------------------------------------- + # 5. GitHub Release (tag v* only) + # ------------------------------------------------------------------- + github-release: + name: GitHub Release + needs: [publish-pypi] + if: startsWith(github.ref, 'refs/tags/v') + runs-on: ubuntu-22.04 + + steps: + - name: Download all wheels + uses: actions/download-artifact@v4 + with: + pattern: wheel-* + merge-multiple: true + path: dist/ + + - name: Create GitHub Release + uses: softprops/action-gh-release@v2 + with: + files: dist/*.whl + generate_release_notes: true diff --git a/Cargo.lock b/Cargo.lock index 76569ebadddf..0748530c4368 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3108,6 +3108,19 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-viewer" +version = "0.30.0-alpha.1+dev" +dependencies = [ + "bincode", + "clap", + "mimalloc", + "parking_lot", + "rerun", + "serde", + "tokio", +] + [[package]] name = "directories" version = "6.0.0" diff --git a/Cargo.toml b/Cargo.toml index af7187f27572..69cdfbb65b13 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -7,6 +7,7 @@ members = [ "crates/utils/*", "crates/viewer/*", "docs/snippets", + "dimos", "examples/rust/*", "rerun_py", "run_wasm", diff --git a/dimos/Cargo.toml b/dimos/Cargo.toml new file mode 100644 index 000000000000..3f9f6f383d04 --- /dev/null +++ b/dimos/Cargo.toml @@ -0,0 +1,42 @@ +[package] +name = "dimos-viewer" +version = "0.30.0-alpha.3" +edition = "2024" +rust-version = "1.92" +license = "MIT OR Apache-2.0" +publish = false +description = "DimOS Interactive Viewer — custom Rerun viewer with LCM click-to-navigate" + +[[bin]] +name = "dimos-viewer" +path = "src/viewer.rs" + +[lib] +name = "dimos_viewer" +path = "src/lib.rs" + +[features] +default = [] +analytics = ["rerun/analytics"] + +[dependencies] +rerun = { path = "../crates/top/rerun", default-features = false, features = [ + "native_viewer", + "run", + "server", +] } + +clap = { workspace = true, features = ["derive"] } +bincode.workspace = true +mimalloc.workspace = true +parking_lot.workspace = true +serde = { workspace = true, features = ["derive"] } +tokio = { workspace = true, features = [ + "io-util", + "macros", + "net", + "rt-multi-thread", + "signal", + "sync", + "time", +] } diff --git a/dimos/README.md b/dimos/README.md new file mode 100644 index 000000000000..035e569a5224 --- /dev/null +++ b/dimos/README.md @@ -0,0 +1,41 @@ + + +Advanced example showing how to control an external application from the Rerun viewer, by extending the viewer UI. + + + Custom Viewer Callback example screenshot + + + + + + +> [!NOTE] +> [#2337](https://github.com/rerun-io/rerun/issues/2337): In order to spawn a web viewer with these customizations applied, you have to build the web viewer of the version yourself. This is currently not supported outside of the Rerun repository. + +## Overview + +This example is divided into two parts: + +- **Viewer** ([`src/viewer.rs`](src/viewer.rs)): Wraps the Rerun viewer inside an [`eframe`](https://github.com/emilk/egui/tree/master/crates/eframe) app. +- **App** ([`src/app.rs`](src/app.rs)): The application that uses the Rerun SDK. + +In the `app`, an additional gRPC server is opened to allow the `viewer` to send messages to the `app`. +Similar to the [`extend_viewer_ui`](../extend_viewer_ui/) example, the `viewer` is wrapped in an `eframe` app, which allows us to handle the extra communication logic and define our own control UI using [`egui`](https://github.com/emilk/egui). + +The communication between the `viewer` and the `app` is implemented in the [`comms`](src/comms/) module. It defines a simple protocol to send messages between the `viewer` and the `app` using [`bincode`](https://github.com/bincode-org/bincode). +The protocol supports basic commands that the `viewer` can send to the `app`, such as logging a [`Boxes3D`](https://www.rerun.io/docs/reference/types/archetypes/boxes3d) or [`Point3D`](https://www.rerun.io/docs/reference/types/archetypes/points3d) to an entity, or changing the radius of a set of points that is being logged. + +## Usage + +First start the Rerun SDK app with `cargo run -p custom_callback --bin custom_callback_app`, +and then start the extended viewer with `cargo run -p custom_callback --bin custom_callback_viewer`. + +## Relationship with Viewer callbacks + +The [`re_viewer`] crate also exposes some baseline Viewer events through the [`StartupOptions.on_event`](https://docs.rs/re_viewer/latest/re_viewer/struct.StartupOptions.html#structfield.on_event) field, +which can exist alongside your own events from widgets added by extending the UI. diff --git a/dimos/pyproject.toml b/dimos/pyproject.toml new file mode 100644 index 000000000000..e3832a0b5798 --- /dev/null +++ b/dimos/pyproject.toml @@ -0,0 +1,37 @@ +[build-system] +requires = ["maturin>=1.8.1"] +build-backend = "maturin" + +[project] +name = "dimos-viewer" +version = "0.30.0a2" +description = "Interactive Rerun viewer for DimOS with click-to-navigate support" +readme = "README.md" +requires-python = ">=3.10" +license = {text = "MIT OR Apache-2.0"} +authors = [{name = "Dimensional Inc.", email = "engineering@dimensional.com"}] +classifiers = [ + "Development Status :: 4 - Beta", + "Intended Audience :: Developers", + "Topic :: Scientific/Engineering", + "Topic :: Scientific/Engineering :: Visualization", + "Programming Language :: Rust", + "License :: OSI Approved :: MIT License", + "License :: OSI Approved :: Apache Software License", + "Operating System :: POSIX :: Linux", + "Operating System :: MacOS", +] + +[project.urls] +homepage = "https://github.com/dimensionalOS/dimos-viewer" +repository = "https://github.com/dimensionalOS/dimos-viewer" +issues = "https://github.com/dimensionalOS/dimos-viewer/issues" + +[tool.maturin] +# Build the Rust binary and package it for distribution. +# "bin" mode: maturin compiles the [[bin]] target and installs it as a console script. +bindings = "bin" +manifest-path = "Cargo.toml" +locked = false +# Strip binaries to reduce wheel size +strip = true diff --git a/dimos/src/interaction/handle.rs b/dimos/src/interaction/handle.rs new file mode 100644 index 000000000000..0f71a6f11fd6 --- /dev/null +++ b/dimos/src/interaction/handle.rs @@ -0,0 +1,76 @@ +use tokio::sync::mpsc; +use super::protocol::ViewerEvent; + +/// Handle for sending interaction events from the viewer to the application. +/// +/// Cheap to clone and thread-safe. +#[derive(Clone)] +pub struct InteractionHandle { + tx: mpsc::UnboundedSender, +} + +impl InteractionHandle { + /// Create a new handle from a channel sender. + pub fn new(tx: mpsc::UnboundedSender) -> Self { + Self { tx } + } + + /// Send a click event to the application. + pub fn send_click( + &self, + position: [f32; 3], + entity_path: Option, + view_id: String, + is_2d: bool, + ) { + let event = ViewerEvent::Click { + position, + entity_path, + view_id, + timestamp_ms: std::time::SystemTime::now() + .duration_since(std::time::UNIX_EPOCH) + .unwrap_or_default() + .as_millis() as u64, + is_2d, + }; + + if let Err(e) = self.tx.send(event) { + eprintln!("Failed to send click event: {}", e); + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_handle_send_click() { + let (tx, mut rx) = mpsc::unbounded_channel(); + let handle = InteractionHandle::new(tx); + + handle.send_click( + [1.0, 2.0, 3.0], + Some("world/robot".to_string()), + "view_123".to_string(), + false, + ); + + let event = rx.try_recv().unwrap(); + match event { + ViewerEvent::Click { position, entity_path, view_id, is_2d, .. } => { + assert_eq!(position, [1.0, 2.0, 3.0]); + assert_eq!(entity_path, Some("world/robot".to_string())); + assert_eq!(view_id, "view_123"); + assert!(!is_2d); + } + } + } + + #[test] + fn test_handle_is_cloneable() { + let (tx, _rx) = mpsc::unbounded_channel(); + let handle1 = InteractionHandle::new(tx); + let _handle2 = handle1.clone(); + } +} diff --git a/dimos/src/interaction/keyboard.rs b/dimos/src/interaction/keyboard.rs new file mode 100644 index 000000000000..b6cdcd809c37 --- /dev/null +++ b/dimos/src/interaction/keyboard.rs @@ -0,0 +1,510 @@ +//! Keyboard handler for WASD movement controls that publish Twist messages. +//! +//! Converts keyboard input to robot velocity commands following teleop conventions: +//! - WASD/arrows for linear/angular motion +//! - QE for strafing +//! - Space for emergency stop +//! - Shift for speed multiplier + +use std::io; +use super::lcm::{LcmPublisher, twist_command}; +use rerun::external::{egui, re_log}; + +/// LCM channel for twist commands (matches DimOS convention) +const CMD_VEL_CHANNEL: &str = "/cmd_vel#geometry_msgs.Twist"; + +/// Base speeds for keyboard control +const BASE_LINEAR_SPEED: f64 = 0.5; // m/s +const BASE_ANGULAR_SPEED: f64 = 0.8; // rad/s +const FAST_MULTIPLIER: f64 = 2.0; // Shift modifier + +/// Overlay styling +const OVERLAY_MARGIN: f32 = 12.0; +const OVERLAY_PADDING: f32 = 10.0; +const OVERLAY_ROUNDING: f32 = 8.0; +const OVERLAY_BG: egui::Color32 = egui::Color32::from_rgba_premultiplied(20, 20, 30, 220); +const KEY_SIZE: f32 = 32.0; +const KEY_GAP: f32 = 3.0; +const KEY_ACTIVE_BG: egui::Color32 = egui::Color32::from_rgb(60, 180, 75); +const KEY_INACTIVE_BG: egui::Color32 = egui::Color32::from_rgba_premultiplied(60, 60, 80, 180); +const KEY_TEXT_COLOR: egui::Color32 = egui::Color32::WHITE; +const LABEL_COLOR: egui::Color32 = egui::Color32::from_rgb(180, 180, 200); +const ESTOP_ACTIVE_BG: egui::Color32 = egui::Color32::from_rgb(220, 50, 50); + +/// Tracks which movement keys are currently held down. +#[derive(Debug, Clone, Default)] +struct KeyState { + forward: bool, // W or Up + backward: bool, // S or Down + left: bool, // A or Left + right: bool, // D or Right + strafe_l: bool, // Q + strafe_r: bool, // E + fast: bool, // Shift held +} + +impl KeyState { + fn new() -> Self { + Default::default() + } + + /// Returns true if any movement key is currently active + fn any_active(&self) -> bool { + self.forward || self.backward || self.left || self.right || self.strafe_l || self.strafe_r + } + + /// Reset all key states (used for emergency stop) + fn reset(&mut self) { + self.forward = false; + self.backward = false; + self.left = false; + self.right = false; + self.strafe_l = false; + self.strafe_r = false; + self.fast = false; + } +} + +/// Handles keyboard input and publishes Twist via LCM. +pub struct KeyboardHandler { + publisher: LcmPublisher, + state: KeyState, + was_active: bool, + estop_flash: bool, // true briefly after space pressed +} + +impl KeyboardHandler { + /// Create a new keyboard handler with LCM publisher on CMD_VEL_CHANNEL. + pub fn new() -> Result { + let publisher = LcmPublisher::new(CMD_VEL_CHANNEL.to_string())?; + Ok(Self { + publisher, + state: KeyState::new(), + was_active: false, + estop_flash: false, + }) + } + + /// Process keyboard input from egui and publish Twist if keys are held. + /// Called once per frame from DimosApp.ui(). + /// + /// Returns true if any movement key is active (for UI overlay). + pub fn process(&mut self, ctx: &egui::Context) -> bool { + self.estop_flash = false; + + // Check if any text widget has focus - if so, skip keyboard capture + let text_has_focus = ctx.memory(|m| m.focused().is_some()); + if text_has_focus { + if self.was_active { + if let Err(e) = self.publish_stop() { + re_log::warn!("Failed to send stop command on focus change: {e:?}"); + } + self.was_active = false; + } + return false; + } + + // Update key state from egui input + self.update_key_state(ctx); + + // Check for emergency stop (Space key pressed - one-shot action) + if ctx.input(|i| i.key_pressed(egui::Key::Space)) { + self.state.reset(); + if let Err(e) = self.publish_stop() { + re_log::warn!("Failed to send emergency stop: {e:?}"); + } + self.was_active = false; + self.estop_flash = true; + return true; // return true so overlay shows the e-stop flash + } + + // Publish twist command if keys are active, or stop if just released + if self.state.any_active() { + if let Err(e) = self.publish_twist() { + re_log::warn!("Failed to publish twist command: {e:?}"); + } + self.was_active = true; + } else if self.was_active { + if let Err(e) = self.publish_stop() { + re_log::warn!("Failed to send stop on key release: {e:?}"); + } + self.was_active = false; + } + + self.state.any_active() + } + + /// Draw keyboard overlay HUD. Always shown (dim when idle, bright when active). + pub fn draw_overlay(&self, ctx: &egui::Context) { + egui::Area::new("keyboard_hud".into()) + .fixed_pos(egui::pos2(OVERLAY_MARGIN, OVERLAY_MARGIN)) + .order(egui::Order::Foreground) + .interactable(false) + .show(ctx, |ui| { + egui::Frame::new() + .fill(OVERLAY_BG) + .corner_radius(egui::CornerRadius::same(OVERLAY_ROUNDING as u8)) + .inner_margin(egui::Margin::same(OVERLAY_PADDING as i8)) + .show(ui, |ui| { + self.draw_hud_content(ui); + }); + }); + } + + fn draw_hud_content(&self, ui: &mut egui::Ui) { + let active = self.state.any_active() || self.estop_flash; + + // Title + let title_color = if active { + egui::Color32::WHITE + } else { + egui::Color32::from_rgb(120, 120, 140) + }; + ui.label(egui::RichText::new("🎮 Keyboard Teleop").color(title_color).size(13.0)); + ui.add_space(4.0); + + // Key grid: [Q] [W] [E] + // [A] [S] [D] + // [ SPACE ] + let row1 = [ + ("Q", self.state.strafe_l), + ("W", self.state.forward), + ("E", self.state.strafe_r), + ]; + let row2 = [ + ("A", self.state.left), + ("S", self.state.backward), + ("D", self.state.right), + ]; + + // Row 1 + ui.horizontal(|ui| { + ui.spacing_mut().item_spacing.x = KEY_GAP; + for (label, pressed) in &row1 { + self.draw_key(ui, label, *pressed); + } + }); + + // Row 2 + ui.horizontal(|ui| { + ui.spacing_mut().item_spacing.x = KEY_GAP; + for (label, pressed) in &row2 { + self.draw_key(ui, label, *pressed); + } + }); + + // Space bar (e-stop) + let space_width = KEY_SIZE * 3.0 + KEY_GAP * 2.0; + let space_rect = ui.allocate_exact_size( + egui::vec2(space_width, KEY_SIZE * 0.7), + egui::Sense::hover(), + ).0; + let space_bg = if self.estop_flash { + ESTOP_ACTIVE_BG + } else { + KEY_INACTIVE_BG + }; + ui.painter().rect_filled(space_rect, egui::CornerRadius::same(4), space_bg); + ui.painter().text( + space_rect.center(), + egui::Align2::CENTER_CENTER, + "STOP", + egui::FontId::proportional(11.0), + KEY_TEXT_COLOR, + ); + + ui.add_space(4.0); + + // Speed indicator + let speed_label = if self.state.fast { "⇧ FAST" } else { "⇧ shift=fast" }; + let speed_color = if self.state.fast { + egui::Color32::from_rgb(255, 200, 50) + } else { + LABEL_COLOR + }; + ui.label(egui::RichText::new(speed_label).color(speed_color).size(10.0)); + } + + fn draw_key(&self, ui: &mut egui::Ui, label: &str, pressed: bool) { + let (rect, _) = ui.allocate_exact_size( + egui::vec2(KEY_SIZE, KEY_SIZE), + egui::Sense::hover(), + ); + let bg = if pressed { KEY_ACTIVE_BG } else { KEY_INACTIVE_BG }; + ui.painter().rect_filled(rect, egui::CornerRadius::same(4), bg); + ui.painter().text( + rect.center(), + egui::Align2::CENTER_CENTER, + label, + egui::FontId::monospace(14.0), + KEY_TEXT_COLOR, + ); + } + + /// Read current key state from egui input, update self.state. + fn update_key_state(&mut self, ctx: &egui::Context) { + ctx.input(|i| { + self.state.forward = i.key_down(egui::Key::W) || i.key_down(egui::Key::ArrowUp); + self.state.backward = i.key_down(egui::Key::S) || i.key_down(egui::Key::ArrowDown); + self.state.left = i.key_down(egui::Key::A) || i.key_down(egui::Key::ArrowLeft); + self.state.right = i.key_down(egui::Key::D) || i.key_down(egui::Key::ArrowRight); + self.state.strafe_l = i.key_down(egui::Key::Q); + self.state.strafe_r = i.key_down(egui::Key::E); + self.state.fast = i.modifiers.shift; + }); + } + + /// Convert current KeyState to Twist and publish via LCM. + fn publish_twist(&mut self) -> io::Result<()> { + let (lin_x, lin_y, lin_z, ang_x, ang_y, ang_z) = self.compute_twist(); + + let cmd = twist_command( + [lin_x, lin_y, lin_z], + [ang_x, ang_y, ang_z], + ); + + self.publisher.publish_twist(&cmd)?; + + re_log::trace!( + "Published twist: lin=({:.2},{:.2},{:.2}) ang=({:.2},{:.2},{:.2})", + lin_x, lin_y, lin_z, ang_x, ang_y, ang_z + ); + + Ok(()) + } + + /// Publish all-zero twist (stop command) + fn publish_stop(&mut self) -> io::Result<()> { + let cmd = twist_command([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]); + self.publisher.publish_twist(&cmd)?; + re_log::debug!("Published stop command"); + Ok(()) + } + + /// Map KeyState to linear/angular velocities. + fn compute_twist(&self) -> (f64, f64, f64, f64, f64, f64) { + let mut linear_x = 0.0; + let mut linear_y = 0.0; + let mut angular_z = 0.0; + + if self.state.forward { + linear_x += BASE_LINEAR_SPEED; + } + if self.state.backward { + linear_x -= BASE_LINEAR_SPEED; + } + if self.state.strafe_l { + linear_y += BASE_LINEAR_SPEED; + } + if self.state.strafe_r { + linear_y -= BASE_LINEAR_SPEED; + } + if self.state.left { + angular_z += BASE_ANGULAR_SPEED; + } + if self.state.right { + angular_z -= BASE_ANGULAR_SPEED; + } + if self.state.fast { + linear_x *= FAST_MULTIPLIER; + linear_y *= FAST_MULTIPLIER; + angular_z *= FAST_MULTIPLIER; + } + + (linear_x, linear_y, 0.0, 0.0, 0.0, angular_z) + } +} + +impl std::fmt::Debug for KeyboardHandler { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("KeyboardHandler") + .field("state", &self.state) + .field("was_active", &self.was_active) + .finish() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_key_state_any_active() { + let mut state = KeyState::new(); + assert!(!state.any_active()); + + state.forward = true; + assert!(state.any_active()); + + state.reset(); + assert!(!state.any_active()); + + state.strafe_l = true; + assert!(state.any_active()); + } + + #[test] + fn test_wasd_to_twist_mapping() { + let mut state = KeyState::new(); + state.forward = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, BASE_LINEAR_SPEED); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, 0.0); + } + + #[test] + fn test_turn_left_right_mapping() { + let mut state = KeyState::new(); + state.left = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, BASE_ANGULAR_SPEED); + + let mut state = KeyState::new(); + state.right = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, -BASE_ANGULAR_SPEED); + } + + #[test] + fn test_strafe_mapping() { + let mut state = KeyState::new(); + state.strafe_l = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, BASE_LINEAR_SPEED); + assert_eq!(ang_z, 0.0); + + let mut state = KeyState::new(); + state.strafe_r = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, -BASE_LINEAR_SPEED); + assert_eq!(ang_z, 0.0); + } + + #[test] + fn test_shift_doubles_speed() { + let mut state = KeyState::new(); + state.forward = true; + state.fast = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, BASE_LINEAR_SPEED * FAST_MULTIPLIER); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, 0.0); + } + + #[test] + fn test_simultaneous_keys() { + let mut state = KeyState::new(); + state.forward = true; + state.left = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, BASE_LINEAR_SPEED); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, BASE_ANGULAR_SPEED); + } + + #[test] + fn test_key_reset() { + let mut state = KeyState::new(); + state.forward = true; + state.left = true; + state.fast = true; + assert!(state.any_active()); + state.reset(); + assert!(!state.forward); + assert!(!state.left); + assert!(!state.fast); + assert!(!state.any_active()); + } + + #[test] + fn test_keyboard_handler_creation() { + let handler = KeyboardHandler::new(); + assert!(handler.is_ok()); + let handler = handler.unwrap(); + assert!(!handler.was_active); + assert!(!handler.state.any_active()); + } + + #[test] + fn test_opposite_keys_cancel() { + let mut state = KeyState::new(); + state.forward = true; + state.backward = true; + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state, + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, _, _, _, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, 0.0); + assert_eq!(ang_z, 0.0); + } + + #[test] + fn test_compute_twist_all_zeros() { + let handler = KeyboardHandler { + publisher: LcmPublisher::new("/test".to_string()).unwrap(), + state: KeyState::new(), + was_active: false, + estop_flash: false, + }; + let (lin_x, lin_y, lin_z, ang_x, ang_y, ang_z) = handler.compute_twist(); + assert_eq!(lin_x, 0.0); + assert_eq!(lin_y, 0.0); + assert_eq!(lin_z, 0.0); + assert_eq!(ang_x, 0.0); + assert_eq!(ang_y, 0.0); + assert_eq!(ang_z, 0.0); + } +} diff --git a/dimos/src/interaction/lcm.rs b/dimos/src/interaction/lcm.rs new file mode 100644 index 000000000000..6b6b4146a8f5 --- /dev/null +++ b/dimos/src/interaction/lcm.rs @@ -0,0 +1,464 @@ +//! LCM (Lightweight Communications and Marshalling) publisher for click events and twist commands. +//! +//! Publishes `geometry_msgs/PointStamped` and `geometry_msgs/Twist` messages over UDP multicast, +//! following the same convention as RViz's `/clicked_point` and `/cmd_vel` topics. +//! +//! ## LCM Wire Protocol (short message) +//! ```text +//! [4B magic "LC02"] [4B seqno] [channel\0] [LCM-encoded payload] +//! ``` +//! +//! ## PointStamped Binary Layout +//! ```text +//! [8B fingerprint hash] [Header (no hash)] [Point (no hash)] +//! +//! Header: +//! [4B seq: i32] [4B stamp.sec: i32] [4B stamp.nsec: i32] +//! [4B frame_id_len: i32 (including null)] [frame_id bytes] [null] +//! +//! Point: +//! [8B x: f64] [8B y: f64] [8B z: f64] +//! ``` +//! +//! ## Twist Binary Layout +//! ```text +//! [8B fingerprint hash] [Twist (no hash)] +//! +//! Twist: +//! Vector3 linear: [8B x: f64] [8B y: f64] [8B z: f64] +//! Vector3 angular: [8B x: f64] [8B y: f64] [8B z: f64] +//! ``` + +use std::net::UdpSocket; +use std::sync::atomic::{AtomicU32, Ordering}; +use std::time::SystemTime; + +/// LCM multicast address and port (default LCM configuration). +const LCM_MULTICAST_ADDR: &str = "239.255.76.67:7667"; + +/// LCM short message magic number: "LC02" in ASCII. +const LCM_MAGIC_SHORT: u32 = 0x4c433032; + +/// Pre-computed fingerprint hash for `geometry_msgs/PointStamped`. +/// +/// Computed from the recursive hash chain: +/// - Time: base=0xde1d24a3a8ecb648 -> rot -> 0xbc3a494751d96c91 +/// - Header: base=0xdbb33f5b4c19b8ea + Time -> rot -> 0x2fdb11453be64af7 +/// - Point: base=0x573f2fdd2f76508f -> rot -> 0xae7e5fba5eeca11e +/// - PointStamped: base=0xf012413a2c8028c2 + Header + Point -> rot -> 0x9cd764738ea629af +const POINT_STAMPED_HASH: u64 = 0x9cd764738ea629af; + +/// Pre-computed fingerprint hash for `geometry_msgs/Twist`. +/// +/// Computed from the recursive hash chain: +/// - Vector3: base=0x573f2fdd2f76508f -> rot -> 0xae7e5fba5eeca11e +/// - Twist: base=0x3a4144772922add7 + Vector3 + Vector3 -> rot -> 0x2e7c07d7cdf7e027 +const TWIST_HASH: u64 = 0x2e7c07d7cdf7e027; + +/// A click event with world-space coordinates and entity info. +#[derive(Debug, Clone)] +pub struct ClickEvent { + pub x: f64, + pub y: f64, + pub z: f64, + /// Rerun entity path (stored in frame_id per our convention). + pub entity_path: String, + /// Unix timestamp in seconds. + pub timestamp_sec: i32, + /// Nanosecond remainder. + pub timestamp_nsec: i32, +} + +/// A velocity command (maps to geometry_msgs/Twist). +#[derive(Debug, Clone)] +pub struct TwistCommand { + pub linear_x: f64, // forward/backward + pub linear_y: f64, // strafe left/right + pub linear_z: f64, // up/down (unused for ground robots) + pub angular_x: f64, // roll (unused) + pub angular_y: f64, // pitch (unused) + pub angular_z: f64, // yaw left/right +} + +/// Encodes a `PointStamped` LCM message (with fingerprint hash prefix). +/// +/// Binary layout: +/// - 8 bytes: fingerprint hash (big-endian i64) +/// - Header (no hash): seq(i32) + stamp.sec(i32) + stamp.nsec(i32) + frame_id(len-prefixed string) +/// - Point (no hash): x(f64) + y(f64) + z(f64) +pub fn encode_point_stamped(event: &ClickEvent) -> Vec { + let frame_id_bytes = event.entity_path.as_bytes(); + // LCM string encoding: i32 length (including null terminator) + bytes + null + let string_len = (frame_id_bytes.len() + 1) as i32; + + // Calculate total size: + // 8 (hash) + 4 (seq) + 4 (sec) + 4 (nsec) + 4 (string_len) + frame_id_bytes + 1 (null) + 24 (3 doubles) + let total_size = 8 + 4 + 4 + 4 + 4 + frame_id_bytes.len() + 1 + 24; + let mut buf = Vec::with_capacity(total_size); + + // Fingerprint hash (big-endian) + buf.extend_from_slice(&POINT_STAMPED_HASH.to_be_bytes()); + + // Header._encodeNoHash: + // seq (i32, big-endian) -- always 0 for click events + buf.extend_from_slice(&0i32.to_be_bytes()); + // stamp.sec (i32) + buf.extend_from_slice(&event.timestamp_sec.to_be_bytes()); + // stamp.nsec (i32) + buf.extend_from_slice(&event.timestamp_nsec.to_be_bytes()); + // frame_id: string = i32 length (incl null) + bytes + null + buf.extend_from_slice(&string_len.to_be_bytes()); + buf.extend_from_slice(frame_id_bytes); + buf.push(0); // null terminator + + // Point._encodeNoHash: + buf.extend_from_slice(&event.x.to_be_bytes()); + buf.extend_from_slice(&event.y.to_be_bytes()); + buf.extend_from_slice(&event.z.to_be_bytes()); + + buf +} + +/// Encodes a `Twist` LCM message (with fingerprint hash prefix). +/// +/// Binary layout: +/// - 8 bytes: fingerprint hash (big-endian u64) +/// - Twist (no hash): linear(Vector3: x,y,z f64) + angular(Vector3: x,y,z f64) +pub fn encode_twist(cmd: &TwistCommand) -> Vec { + // 8 (hash) + 48 (6 doubles) = 56 bytes + let mut buf = Vec::with_capacity(56); + + // Fingerprint hash (big-endian) + buf.extend_from_slice(&TWIST_HASH.to_be_bytes()); + + // Twist._encodeNoHash: + // Vector3 linear: + buf.extend_from_slice(&cmd.linear_x.to_be_bytes()); + buf.extend_from_slice(&cmd.linear_y.to_be_bytes()); + buf.extend_from_slice(&cmd.linear_z.to_be_bytes()); + // Vector3 angular: + buf.extend_from_slice(&cmd.angular_x.to_be_bytes()); + buf.extend_from_slice(&cmd.angular_y.to_be_bytes()); + buf.extend_from_slice(&cmd.angular_z.to_be_bytes()); + + buf +} + +/// Builds a complete LCM UDP packet (short message format). +/// +/// Format: `[4B magic] [4B seqno] [channel\0] [payload]` +pub fn build_lcm_packet(channel: &str, payload: &[u8], seq: u32) -> Vec { + let channel_bytes = channel.as_bytes(); + let total = 4 + 4 + channel_bytes.len() + 1 + payload.len(); + let mut pkt = Vec::with_capacity(total); + + pkt.extend_from_slice(&LCM_MAGIC_SHORT.to_be_bytes()); + pkt.extend_from_slice(&seq.to_be_bytes()); + pkt.extend_from_slice(channel_bytes); + pkt.push(0); // null terminator + pkt.extend_from_slice(payload); + + pkt +} + +/// LCM publisher that sends PointStamped and Twist messages via UDP multicast. +pub struct LcmPublisher { + socket: UdpSocket, + seq: AtomicU32, + channel: String, +} + +impl LcmPublisher { + /// Create a new LCM publisher. + /// + /// `channel` is the LCM channel name, e.g. + /// `"/clicked_point#geometry_msgs.PointStamped"` or + /// `"/cmd_vel#geometry_msgs.Twist"`. + pub fn new(channel: String) -> std::io::Result { + let socket = UdpSocket::bind("0.0.0.0:0")?; + // TTL=0 means local machine only; TTL=1 for same subnet + socket.set_multicast_ttl_v4(0)?; + Ok(Self { + socket, + seq: AtomicU32::new(0), + channel, + }) + } + + /// Publish a click event as a PointStamped LCM message. + pub fn publish(&self, event: &ClickEvent) -> std::io::Result { + let payload = encode_point_stamped(event); + let seq = self.seq.fetch_add(1, Ordering::Relaxed); + let packet = build_lcm_packet(&self.channel, &payload, seq); + self.socket.send_to(&packet, LCM_MULTICAST_ADDR) + } + + /// Publish a twist command as a Twist LCM message. + pub fn publish_twist(&self, cmd: &TwistCommand) -> std::io::Result { + let payload = encode_twist(cmd); + let seq = self.seq.fetch_add(1, Ordering::Relaxed); + let packet = build_lcm_packet(&self.channel, &payload, seq); + self.socket.send_to(&packet, LCM_MULTICAST_ADDR) + } +} + +impl std::fmt::Debug for LcmPublisher { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("LcmPublisher") + .field("channel", &self.channel) + .field("seq", &self.seq.load(Ordering::Relaxed)) + .finish() + } +} + +/// Create a `ClickEvent` from position, entity path, and a millisecond timestamp. +pub fn click_event_from_ms( + position: [f32; 3], + entity_path: &str, + timestamp_ms: u64, +) -> ClickEvent { + let total_secs = (timestamp_ms / 1000) as i32; + let nanos = ((timestamp_ms % 1000) * 1_000_000) as i32; + ClickEvent { + x: position[0] as f64, + y: position[1] as f64, + z: position[2] as f64, + entity_path: entity_path.to_string(), + timestamp_sec: total_secs, + timestamp_nsec: nanos, + } +} + +/// Create a `ClickEvent` from position and entity path, using the current time. +pub fn click_event_now(position: [f32; 3], entity_path: &str) -> ClickEvent { + let now = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .unwrap_or_default(); + ClickEvent { + x: position[0] as f64, + y: position[1] as f64, + z: position[2] as f64, + entity_path: entity_path.to_string(), + timestamp_sec: now.as_secs() as i32, + timestamp_nsec: now.subsec_nanos() as i32, + } +} + +/// Create a `TwistCommand` from velocity values. +pub fn twist_command( + linear: [f64; 3], + angular: [f64; 3], +) -> TwistCommand { + TwistCommand { + linear_x: linear[0], + linear_y: linear[1], + linear_z: linear[2], + angular_x: angular[0], + angular_y: angular[1], + angular_z: angular[2], + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_point_stamped_fingerprint() { + fn rot(h: u64) -> u64 { + (h.wrapping_shl(1)).wrapping_add((h >> 63) & 1) + } + let time_hash = rot(0xde1d24a3a8ecb648); + let header_hash = rot(0xdbb33f5b4c19b8ea_u64.wrapping_add(time_hash)); + let point_hash = rot(0x573f2fdd2f76508f); + let ps_hash = + rot(0xf012413a2c8028c2_u64 + .wrapping_add(header_hash) + .wrapping_add(point_hash)); + assert_eq!(ps_hash, POINT_STAMPED_HASH); + } + + #[test] + fn test_twist_fingerprint() { + fn rot(h: u64) -> u64 { + (h.wrapping_shl(1)).wrapping_add((h >> 63) & 1) + } + let vector3_hash = rot(0x573f2fdd2f76508f); + let twist_hash = rot(0x3a4144772922add7_u64 + .wrapping_add(vector3_hash) + .wrapping_add(vector3_hash)); + assert_eq!(twist_hash, TWIST_HASH); + } + + #[test] + fn test_encode_twist_matches_python() { + // Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.3)) + let cmd = TwistCommand { + linear_x: 0.5, + linear_y: 0.0, + linear_z: 0.0, + angular_x: 0.0, + angular_y: 0.0, + angular_z: 0.3, + }; + + let encoded = encode_twist(&cmd); + + let expected_hex = "2e7c07d7cdf7e0273fe000000000000000000000000000000000000000000000000000000000000000000000000000003fd3333333333333"; + let expected: Vec = (0..expected_hex.len()) + .step_by(2) + .map(|i| u8::from_str_radix(&expected_hex[i..i + 2], 16).unwrap()) + .collect(); + + assert_eq!(encoded, expected, "Encoded bytes must match Python LCM output"); + assert_eq!(encoded.len(), 56, "Encoded length must be 56 bytes"); + } + + #[test] + fn test_encode_twist_zero() { + let cmd = TwistCommand { + linear_x: 0.0, + linear_y: 0.0, + linear_z: 0.0, + angular_x: 0.0, + angular_y: 0.0, + angular_z: 0.0, + }; + let encoded = encode_twist(&cmd); + assert_eq!(encoded.len(), 56); + let hash = u64::from_be_bytes(encoded[0..8].try_into().unwrap()); + assert_eq!(hash, TWIST_HASH); + } + + #[test] + fn test_encode_point_stamped_matches_python() { + let event = ClickEvent { + x: 1.5, + y: 2.5, + z: 3.5, + entity_path: "/world/grid".to_string(), + timestamp_sec: 1234, + timestamp_nsec: 5678, + }; + + let encoded = encode_point_stamped(&event); + + let expected_hex = "9cd764738ea629af00000000000004d20000162e0000000c2f776f726c642f67726964003ff80000000000004004000000000000400c000000000000"; + let expected: Vec = (0..expected_hex.len()) + .step_by(2) + .map(|i| u8::from_str_radix(&expected_hex[i..i + 2], 16).unwrap()) + .collect(); + + assert_eq!(encoded, expected, "Encoded bytes must match Python LCM output"); + } + + #[test] + fn test_encode_empty_frame_id() { + let event = ClickEvent { + x: 0.0, + y: 0.0, + z: 0.0, + entity_path: String::new(), + timestamp_sec: 0, + timestamp_nsec: 0, + }; + let encoded = encode_point_stamped(&event); + assert_eq!(encoded.len(), 49); + let str_len = i32::from_be_bytes([encoded[20], encoded[21], encoded[22], encoded[23]]); + assert_eq!(str_len, 1); + } + + #[test] + fn test_build_lcm_packet_format() { + let payload = vec![0xAA, 0xBB]; + let channel = "/test"; + let packet = build_lcm_packet(channel, &payload, 42); + assert_eq!(&packet[0..4], &LCM_MAGIC_SHORT.to_be_bytes()); + assert_eq!(&packet[4..8], &42u32.to_be_bytes()); + let null_pos = packet[8..].iter().position(|&b| b == 0).unwrap() + 8; + let channel_bytes = &packet[8..null_pos]; + assert_eq!(channel_bytes, b"/test"); + assert_eq!(&packet[null_pos + 1..], &[0xAA, 0xBB]); + } + + #[test] + fn test_build_lcm_packet_with_typed_channel() { + let payload = vec![0x01]; + let channel = "/clicked_point#geometry_msgs.PointStamped"; + let packet = build_lcm_packet(channel, &payload, 0); + let null_pos = packet[8..].iter().position(|&b| b == 0).unwrap() + 8; + let extracted_channel = std::str::from_utf8(&packet[8..null_pos]).unwrap(); + assert_eq!(extracted_channel, channel); + } + + #[test] + fn test_click_event_from_ms() { + let event = click_event_from_ms([1.0, 2.0, 3.0], "/world", 1234567); + assert_eq!(event.timestamp_sec, 1234); + assert_eq!(event.timestamp_nsec, 567_000_000); + assert_eq!(event.x, 1.0f64); + assert_eq!(event.entity_path, "/world"); + } + + #[test] + fn test_click_event_now() { + let event = click_event_now([0.0, 0.0, 0.0], "/test"); + let now_sec = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .unwrap() + .as_secs() as i32; + assert!((event.timestamp_sec - now_sec).abs() < 10); + } + + #[test] + fn test_twist_command() { + let cmd = twist_command([1.0, 0.0, 0.0], [0.0, 0.0, 0.5]); + assert_eq!(cmd.linear_x, 1.0); + assert_eq!(cmd.angular_z, 0.5); + } + + #[test] + fn test_lcm_publisher_creation() { + let publisher = LcmPublisher::new("/clicked_point#geometry_msgs.PointStamped".to_string()); + assert!(publisher.is_ok()); + let publisher_twist = LcmPublisher::new("/cmd_vel#geometry_msgs.Twist".to_string()); + assert!(publisher_twist.is_ok()); + } + + #[test] + fn test_full_packet_structure() { + let event = ClickEvent { + x: 1.0, + y: 2.0, + z: 3.0, + entity_path: "/world/robot".to_string(), + timestamp_sec: 100, + timestamp_nsec: 200, + }; + let payload = encode_point_stamped(&event); + let channel = "/clicked_point#geometry_msgs.PointStamped"; + let packet = build_lcm_packet(channel, &payload, 7); + let magic = u32::from_be_bytes([packet[0], packet[1], packet[2], packet[3]]); + assert_eq!(magic, LCM_MAGIC_SHORT); + let seqno = u32::from_be_bytes([packet[4], packet[5], packet[6], packet[7]]); + assert_eq!(seqno, 7); + let null_pos = packet[8..].iter().position(|&b| b == 0).unwrap() + 8; + let ch = std::str::from_utf8(&packet[8..null_pos]).unwrap(); + assert_eq!(ch, channel); + let data_start = null_pos + 1; + let hash_bytes: [u8; 8] = packet[data_start..data_start + 8].try_into().unwrap(); + let hash = u64::from_be_bytes(hash_bytes); + assert_eq!(hash, POINT_STAMPED_HASH); + } + + #[test] + fn test_sequence_number_increments() { + let publisher = + LcmPublisher::new("/test#geometry_msgs.PointStamped".to_string()).unwrap(); + assert_eq!(publisher.seq.load(Ordering::Relaxed), 0); + let seq1 = publisher.seq.fetch_add(1, Ordering::Relaxed); + assert_eq!(seq1, 0); + let seq2 = publisher.seq.fetch_add(1, Ordering::Relaxed); + assert_eq!(seq2, 1); + } +} diff --git a/dimos/src/interaction/mod.rs b/dimos/src/interaction/mod.rs new file mode 100644 index 000000000000..6fec89890734 --- /dev/null +++ b/dimos/src/interaction/mod.rs @@ -0,0 +1,9 @@ +pub mod handle; +pub mod keyboard; +pub mod lcm; +pub mod protocol; + +pub use handle::InteractionHandle; +pub use keyboard::KeyboardHandler; +pub use lcm::{ClickEvent, TwistCommand, LcmPublisher, click_event_from_ms, click_event_now, twist_command}; +pub use protocol::ViewerEvent; diff --git a/dimos/src/interaction/protocol.rs b/dimos/src/interaction/protocol.rs new file mode 100644 index 000000000000..cea94c1ac1ff --- /dev/null +++ b/dimos/src/interaction/protocol.rs @@ -0,0 +1,35 @@ +use serde::{Deserialize, Serialize}; + +/// Events sent from the viewer to the application. +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +pub enum ViewerEvent { + /// User clicked in a spatial view. + Click { + position: [f32; 3], + entity_path: Option, + view_id: String, + timestamp_ms: u64, + is_2d: bool, + }, +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_viewer_event_click_roundtrip() { + let event = ViewerEvent::Click { + position: [1.0, 2.0, 3.0], + entity_path: Some("world/robot".to_string()), + view_id: "view_123".to_string(), + timestamp_ms: 1234567890, + is_2d: false, + }; + + let encoded = bincode::serialize(&event).unwrap(); + let decoded: ViewerEvent = bincode::deserialize(&encoded).unwrap(); + + assert_eq!(event, decoded); + } +} diff --git a/dimos/src/lib.rs b/dimos/src/lib.rs new file mode 100644 index 000000000000..a397390f3710 --- /dev/null +++ b/dimos/src/lib.rs @@ -0,0 +1 @@ +pub mod interaction; diff --git a/dimos/src/viewer.rs b/dimos/src/viewer.rs new file mode 100644 index 000000000000..68151ce0955f --- /dev/null +++ b/dimos/src/viewer.rs @@ -0,0 +1,255 @@ +use std::cell::RefCell; +use std::rc::Rc; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; + +use clap::Parser; +use dimos_viewer::interaction::{LcmPublisher, KeyboardHandler, click_event_from_ms}; +use rerun::external::{eframe, egui, re_crash_handler, re_grpc_server, re_log, re_memory, re_viewer}; + +#[global_allocator] +static GLOBAL: re_memory::AccountingAllocator = + re_memory::AccountingAllocator::new(mimalloc::MiMalloc); + +/// LCM channel for click events (follows RViz convention) +const LCM_CHANNEL: &str = "/clicked_point#geometry_msgs.PointStamped"; +/// Minimum time between click events (debouncing) +const CLICK_DEBOUNCE_MS: u64 = 100; +/// Maximum rapid clicks to log as warning +const RAPID_CLICK_THRESHOLD: usize = 5; +/// Default gRPC listen port (9877 to avoid conflict with stock Rerun on 9876) +const DEFAULT_PORT: u16 = 9877; + +/// DimOS Interactive Viewer — a custom Rerun viewer with LCM click-to-navigate. +/// +/// Accepts the same CLI flags as the stock `rerun` binary so it can be spawned +/// seamlessly via `rerun_bindings.spawn(executable_name="dimos-viewer")`. +#[derive(Parser, Debug)] +#[command(name = "dimos-viewer", version, about)] +struct Args { + /// The gRPC port to listen on for incoming SDK connections. + #[arg(long, default_value_t = DEFAULT_PORT)] + port: u16, + + /// An upper limit on how much memory the viewer should use. + /// When this limit is reached, the oldest data will be dropped. + /// Examples: "75%", "16GB". + #[arg(long, default_value = "75%")] + memory_limit: String, + + /// An upper limit on how much memory the gRPC server should use. + /// Examples: "1GiB", "50%". + #[arg(long, default_value = "1GiB")] + server_memory_limit: String, + + /// Hide the Rerun welcome screen. + #[arg(long)] + hide_welcome_screen: bool, + + /// Hint that data will arrive shortly (suppresses "waiting for data" message). + #[arg(long)] + expect_data_soon: bool, +} + +/// Wraps re_viewer::App to add keyboard control interception. +struct DimosApp { + inner: re_viewer::App, + keyboard: KeyboardHandler, +} + +impl DimosApp { + fn new( + inner: re_viewer::App, + keyboard: KeyboardHandler, + ) -> Self { + Self { + inner, + keyboard, + } + } +} + +impl eframe::App for DimosApp { + fn ui(&mut self, ui: &mut egui::Ui, frame: &mut eframe::Frame) { + // Process keyboard input before delegating to Rerun + self.keyboard.process(ui.ctx()); + + // Always draw the keyboard HUD overlay (dims when inactive) + self.keyboard.draw_overlay(ui.ctx()); + + // Delegate to Rerun's main ui method + self.inner.ui(ui, frame); + } + + // Delegate all other methods to inner re_viewer::App + fn save(&mut self, storage: &mut dyn eframe::Storage) { + self.inner.save(storage); + } + + fn clear_color(&self, visuals: &egui::Visuals) -> [f32; 4] { + self.inner.clear_color(visuals) + } + + fn persist_egui_memory(&self) -> bool { + self.inner.persist_egui_memory() + } + + fn auto_save_interval(&self) -> std::time::Duration { + self.inner.auto_save_interval() + } + + fn raw_input_hook(&mut self, ctx: &egui::Context, raw_input: &mut egui::RawInput) { + self.inner.raw_input_hook(ctx, raw_input); + } +} + +#[tokio::main] +async fn main() -> Result<(), Box> { + let args = Args::parse(); + + let main_thread_token = re_viewer::MainThreadToken::i_promise_i_am_on_the_main_thread(); + re_log::setup_logging(); + re_crash_handler::install_crash_handlers(re_viewer::build_info()); + + // Listen for gRPC connections from Rerun's logging SDKs. + let listen_addr = format!("0.0.0.0:{}", args.port); + re_log::info!("Listening for SDK connections on {listen_addr}"); + let rx_log = re_grpc_server::spawn_with_recv( + listen_addr.parse()?, + Default::default(), + re_grpc_server::shutdown::never(), + ); + + // Create LCM publisher for click events + let lcm_publisher = LcmPublisher::new(LCM_CHANNEL.to_string()) + .expect("Failed to create LCM publisher"); + re_log::info!("LCM publisher created for channel: {LCM_CHANNEL}"); + + // Create keyboard handler + let keyboard_handler = KeyboardHandler::new() + .expect("Failed to create keyboard handler"); + re_log::info!("Keyboard handler initialized for WASD controls on /cmd_vel"); + + // State for debouncing and rapid click detection + let last_click_time = Rc::new(RefCell::new(Instant::now())); + let rapid_click_count = Rc::new(RefCell::new(0usize)); + + let mut native_options = re_viewer::native::eframe_options(None); + native_options.viewport = native_options + .viewport + .with_app_id("rerun_example_custom_callback"); + + let app_env = re_viewer::AppEnvironment::Custom("DimOS Interactive Viewer".to_owned()); + + let startup_options = re_viewer::StartupOptions { + on_event: Some(Rc::new({ + let last_click_time = last_click_time.clone(); + let rapid_click_count = rapid_click_count.clone(); + + move |event: re_viewer::ViewerEvent| { + if let re_viewer::ViewerEventKind::SelectionChange { items } = event.kind { + let mut has_position = false; + let mut no_position_count = 0; + + for item in items { + match item { + re_viewer::SelectionChangeItem::Entity { + entity_path, + view_name: _, + position: Some(pos), + .. + } => { + has_position = true; + + // Debouncing + let now = Instant::now(); + let elapsed = now.duration_since(*last_click_time.borrow()); + + if elapsed < Duration::from_millis(CLICK_DEBOUNCE_MS) { + let mut count = rapid_click_count.borrow_mut(); + *count += 1; + if *count == RAPID_CLICK_THRESHOLD { + re_log::warn!( + "Rapid click detected ({} clicks within {}ms)", + RAPID_CLICK_THRESHOLD, + CLICK_DEBOUNCE_MS + ); + } + continue; + } else { + *rapid_click_count.borrow_mut() = 0; + } + *last_click_time.borrow_mut() = now; + + let timestamp_ms = SystemTime::now() + .duration_since(UNIX_EPOCH) + .unwrap_or_default() + .as_millis() as u64; + + // Build click event and publish via LCM + let click = click_event_from_ms( + [pos.x, pos.y, pos.z], + &entity_path.to_string(), + timestamp_ms, + ); + + match lcm_publisher.publish(&click) { + Ok(_) => { + re_log::debug!( + "LCM click event published: entity={}, pos=({:.2}, {:.2}, {:.2})", + entity_path, + pos.x, + pos.y, + pos.z + ); + } + Err(err) => { + re_log::error!("Failed to publish LCM click event: {err:?}"); + } + } + } + re_viewer::SelectionChangeItem::Entity { position: None, .. } => { + no_position_count += 1; + } + _ => {} + } + } + + if !has_position && no_position_count > 0 { + re_log::trace!( + "Selection change without position data ({no_position_count} items). \ + This is normal for hover/keyboard navigation." + ); + } + } + } + })), + ..Default::default() + }; + + let window_title = "DimOS Interactive Viewer"; + eframe::run_native( + window_title, + native_options, + Box::new(move |cc| { + re_viewer::customize_eframe_and_setup_renderer(cc)?; + + let mut rerun_app = re_viewer::App::new( + main_thread_token, + re_viewer::build_info(), + app_env, + startup_options, + cc, + None, + re_viewer::AsyncRuntimeHandle::from_current_tokio_runtime_or_wasmbindgen()?, + ); + + rerun_app.add_log_receiver(rx_log); + + let dimos_app = DimosApp::new(rerun_app, keyboard_handler); + + Ok(Box::new(dimos_app)) + }), + )?; + + Ok(()) +} diff --git a/pyproject.toml b/pyproject.toml index 06dc3ef1b9e7..36e1200c5bef 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ [project] name = "rerun-workspace" -version = "0.28.0-alpha.1+dev" +version = "0.30.0a3" description = "Rerun Python workspace" requires-python = ">=3.10,<3.13" diff --git a/test_keyboard_lcm.py b/test_keyboard_lcm.py new file mode 100644 index 000000000000..9ecbc1e83ea9 --- /dev/null +++ b/test_keyboard_lcm.py @@ -0,0 +1,49 @@ +"""Verify dimos-viewer keyboard LCM output matches DimOS Twist expectations. + +Run: python test_keyboard_lcm.py +Requires: dimos-lcm (PYTHONPATH=.../dimos-lcm/generated/python_lcm_msgs) +""" + +import struct +import sys + +def test_twist_encoding(): + """Verify Twist encoding matches Python LCM reference.""" + # This is what the Rust viewer now produces for Twist (no Header) + # TwistCommand { linear_x: 0.5, angular_z: 0.3 } + def rot(h): + return ((h << 1) + ((h >> 63) & 1)) & 0xFFFFFFFFFFFFFFFF + + # Verify hash chain + vector3_hash = rot(0x573f2fdd2f76508f) + twist_hash = rot((0x3a4144772922add7 + vector3_hash + vector3_hash) & 0xFFFFFFFFFFFFFFFF) + assert twist_hash == 0x2e7c07d7cdf7e027, f"Twist hash mismatch: 0x{twist_hash:016x}" + + # Build expected encoding manually: hash + 6 doubles + buf = struct.pack(">q", twist_hash) # 8B fingerprint + buf += struct.pack(">d", 0.5) # linear.x + buf += struct.pack(">d", 0.0) # linear.y + buf += struct.pack(">d", 0.0) # linear.z + buf += struct.pack(">d", 0.0) # angular.x + buf += struct.pack(">d", 0.0) # angular.y + buf += struct.pack(">d", 0.3) # angular.z + assert len(buf) == 56, f"Expected 56 bytes, got {len(buf)}" + + expected_hex = "2e7c07d7cdf7e0273fe000000000000000000000000000000000000000000000000000000000000000000000000000003fd3333333333333" + assert buf.hex() == expected_hex, f"Encoding mismatch:\n got: {buf.hex()}\n expect: {expected_hex}" + + print("PASS: Twist encoding matches Python LCM reference (56 bytes)") + print(f" Channel: /cmd_vel#geometry_msgs.Twist") + print(f" Hash: 0x{twist_hash:016x}") + +def test_channel_name(): + """Verify channel follows DimOS convention.""" + channel = "/cmd_vel#geometry_msgs.Twist" + assert channel.startswith("/cmd_vel"), "Channel must start with /cmd_vel" + assert "#geometry_msgs.Twist" in channel, "Channel must include type suffix" + print(f"PASS: Channel name correct: {channel}") + +if __name__ == "__main__": + test_twist_encoding() + test_channel_name() + print("\nAll tests passed!")