diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6026572388..307db25e0d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -58,6 +58,8 @@ "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "phone-go2-teleop": "dimos.teleop.phone.blueprints:phone_go2_teleop", + "sim-basic": "dimos.robot.sim.blueprints.basic.sim_basic:sim_basic", + "sim-nav": "dimos.robot.sim.blueprints.nav.sim_nav:sim_nav", "simple-phone-teleop": "dimos.teleop.phone.blueprints:simple_phone_teleop", "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", "unitree-g1": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1:unitree_g1", diff --git a/dimos/robot/sim/__init__.py b/dimos/robot/sim/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/robot/sim/blueprints/__init__.py b/dimos/robot/sim/blueprints/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/robot/sim/blueprints/basic/__init__.py b/dimos/robot/sim/blueprints/basic/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/robot/sim/blueprints/basic/sim_basic.py b/dimos/robot/sim/blueprints/basic/sim_basic.py new file mode 100644 index 0000000000..1fa4c9bc81 --- /dev/null +++ b/dimos/robot/sim/blueprints/basic/sim_basic.py @@ -0,0 +1,145 @@ +# Copyright 2026 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. + +"""Basic DimSim blueprint — connection + visualization.""" + +from typing import Any + +from dimos.core.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.core.transport import JpegLcmTransport +from dimos.msgs.sensor_msgs import Image +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.robot.sim.bridge import sim_bridge +from dimos.robot.sim.tf_module import sim_tf +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + + +class _SimLCM(LCM): # type: ignore[misc] + """LCM that JPEG-decodes image topics and standard-decodes everything else.""" + + _JPEG_TOPICS = frozenset({"/color_image"}) + + def decode(self, msg: bytes, topic: Any) -> Any: # type: ignore[override] + if getattr(topic, "topic", None) in self._JPEG_TOPICS: + return Image.lcm_jpeg_decode(msg) + return super().decode(msg, topic) + + +# DimSim sends JPEG-compressed images over LCM — use JpegLcmTransport to decode. +_transports_base = autoconnect().transports( + {("color_image", Image): JpegLcmTransport("/color_image", Image)} +) + + +def _convert_camera_info(camera_info: Any) -> Any: + # Log pinhole under TF tree (3D frustum) — NOT at the image entity. + # Pinhole at the image entity blocks rerun's 2D viewer. + import rerun as rr + + fx, fy = camera_info.K[0], camera_info.K[4] + cx, cy = camera_info.K[2], camera_info.K[5] + return [ + ( + "world/tf/camera_optical", + rr.Pinhole( + focal_length=[fx, fy], + principal_point=[cx, cy], + width=camera_info.width, + height=camera_info.height, + image_plane_distance=1.0, + ), + ), + ( + "world/tf/camera_optical", + rr.Transform3D(parent_frame="tf#/camera_optical"), + ), + ] + + +def _convert_color_image(image: Any) -> Any: + # Log image at both: + # world/color_image — 2D view (no pinhole ancestor) + # world/tf/camera_optical/image — 3D view (child of pinhole) + rerun_data = image.to_rerun() + return [ + ("world/color_image", rerun_data), + ("world/tf/camera_optical/image", rerun_data), + ] + + +def _convert_global_map(grid: Any) -> Any: + return grid.to_rerun(voxel_size=0.1, mode="boxes") + + +def _convert_navigation_costmap(grid: Any) -> Any: + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) + + +def _static_base_link(rr: Any) -> list[Any]: + return [ + rr.Boxes3D( + half_sizes=[0.3, 0.15, 0.12], + colors=[(0, 180, 255)], + ), + rr.Transform3D(parent_frame="tf#/base_link"), + ] + + +rerun_config = { + "pubsubs": [_SimLCM(autoconf=True)], + "visual_override": { + "world/camera_info": _convert_camera_info, + "world/color_image": _convert_color_image, + "world/global_map": _convert_global_map, + "world/navigation_costmap": _convert_navigation_costmap, + "world/pointcloud": None, + }, + "static": { + "world/tf/base_link": _static_base_link, + }, +} + +match global_config.viewer_backend: + case "foxglove": + from dimos.robot.foxglove_bridge import foxglove_bridge + + with_vis = autoconnect( + _transports_base, + foxglove_bridge(shm_channels=["/color_image#sensor_msgs.Image"]), + ) + case "rerun": + from dimos.visualization.rerun.bridge import rerun_bridge + + with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) + case "rerun-web": + from dimos.visualization.rerun.bridge import rerun_bridge + + with_vis = autoconnect(_transports_base, rerun_bridge(viewer_mode="web", **rerun_config)) + case _: + with_vis = _transports_base + +sim_basic = autoconnect( + with_vis, + sim_bridge(), + sim_tf(), + websocket_vis(), +).global_config(n_workers=4, robot_model="dimsim") + +__all__ = ["sim_basic"] diff --git a/dimos/robot/sim/blueprints/nav/__init__.py b/dimos/robot/sim/blueprints/nav/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/robot/sim/blueprints/nav/sim_nav.py b/dimos/robot/sim/blueprints/nav/sim_nav.py new file mode 100644 index 0000000000..fa2e829ffe --- /dev/null +++ b/dimos/robot/sim/blueprints/nav/sim_nav.py @@ -0,0 +1,35 @@ +# Copyright 2026 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. + +"""DimSim navigation blueprint — basic + mapping + planning + exploration.""" + +from dimos.core.blueprints import autoconnect +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.pointclouds.occupancy import ( + HeightCostConfig, +) +from dimos.mapping.voxels import voxel_mapper +from dimos.navigation.frontier_exploration import wavefront_frontier_explorer +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.robot.sim.blueprints.basic.sim_basic import sim_basic + +sim_nav = autoconnect( + sim_basic, + voxel_mapper(voxel_size=0.1, publish_interval=0.5), + cost_mapper(algo="height_cost", config=HeightCostConfig(can_pass_under=1.5, smoothing=2.0)), + replanning_a_star_planner(), + wavefront_frontier_explorer(), +).global_config(n_workers=6, robot_model="dimsim") + +__all__ = ["sim_nav"] diff --git a/dimos/robot/sim/bridge.py b/dimos/robot/sim/bridge.py new file mode 100644 index 0000000000..10545c1cf0 --- /dev/null +++ b/dimos/robot/sim/bridge.py @@ -0,0 +1,214 @@ +# Copyright 2026 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. + +"""NativeModule wrapper for the DimSim bridge subprocess. + +Launches the DimSim bridge (Deno CLI) as a managed subprocess. The bridge +publishes sensor data (odom, lidar, images) directly to LCM — no Python +decode/re-encode hop. Python only handles lifecycle and TF (via DimSimTF). + +Usage:: + + from dimos.robot.sim.bridge import sim_bridge + from dimos.robot.sim.tf_module import sim_tf + from dimos.core.blueprints import autoconnect + + autoconnect(sim_bridge(), sim_tf(), some_consumer()).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import os +from pathlib import Path +import shutil +from typing import TYPE_CHECKING + +from dimos import spec +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.core.stream import In, Out + from dimos.msgs.geometry_msgs import PoseStamped, Twist + from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 + +logger = setup_logger() + +_DIMSIM_JSR = "jsr:@antim/dimsim" + + +def _find_deno() -> str: + """Find the deno binary.""" + return shutil.which("deno") or str(Path.home() / ".deno" / "bin" / "deno") + + +def _find_local_cli() -> Path | None: + """Find local DimSim/dimos-cli/cli.ts for development.""" + repo_root = Path(__file__).resolve().parents[4] + candidate = repo_root / "DimSim" / "dimos-cli" / "cli.ts" + return candidate if candidate.exists() else None + + +@dataclass(kw_only=True) +class DimSimBridgeConfig(NativeModuleConfig): + """Configuration for the DimSim bridge subprocess.""" + + # Set to deno binary — resolved in _resolve_paths(). + executable: str = "deno" + build_command: str | None = None + cwd: str | None = None + + scene: str = "apt" + port: int = 8090 + local: bool = False # Use local DimSim repo instead of installed CLI + + # These fields are handled via extra_args, not to_cli_args(). + cli_exclude: frozenset[str] = frozenset({"scene", "port", "local"}) + + # Populated by _resolve_paths() — deno run args + dev subcommand + scene/port. + extra_args: list[str] = field(default_factory=list) + + +class DimSimBridge(NativeModule, spec.Camera, spec.Pointcloud): + """NativeModule that manages the DimSim bridge subprocess. + + The bridge (Deno process) handles Browser-LCM translation and publishes + sensor data directly to LCM. Ports declared here exist for blueprint + wiring / autoconnect but data flows through LCM, not Python. + """ + + config: DimSimBridgeConfig + default_config = DimSimBridgeConfig + + # Sensor outputs (bridge publishes these directly to LCM) + odom: Out[PoseStamped] + color_image: Out[Image] + depth_image: Out[Image] + lidar: Out[PointCloud2] + pointcloud: Out[PointCloud2] + camera_info: Out[CameraInfo] + + # Control input (consumers publish cmd_vel to LCM, bridge reads it) + cmd_vel: In[Twist] + + def _resolve_paths(self) -> None: + """Resolve executable and build extra_args. + + Set DIMSIM_LOCAL=1 to use local DimSim repo instead of installed CLI. + """ + dev_args = ["dev", "--scene", self.config.scene, "--port", str(self.config.port)] + + # DIMSIM_HEADLESS=1 → launch headless Chrome (no browser tab needed) + # Uses CPU rendering (SwiftShader) by default — no GPU required for CI. + # Set DIMSIM_RENDER=gpu for Metal/ANGLE on macOS. + if os.environ.get("DIMSIM_HEADLESS", "").strip() in ("1", "true"): + render = os.environ.get("DIMSIM_RENDER", "cpu").strip() + dev_args.extend(["--headless", "--render", render]) + + # Allow env var override: DIMSIM_LOCAL=1 dimos run sim-nav + if os.environ.get("DIMSIM_LOCAL", "").strip() in ("1", "true"): + self.config.local = True + + if self.config.local: + cli_ts = _find_local_cli() + if not cli_ts: + raise FileNotFoundError( + "Local DimSim not found. Expected DimSim/dimos-cli/cli.ts " + "next to the dimos repo." + ) + logger.info(f"Using local DimSim: {cli_ts}") + self.config.executable = _find_deno() + self.config.extra_args = [ + "run", + "--allow-all", + "--unstable-net", + str(cli_ts), + *dev_args, + ] + self.config.cwd = None + return + + dimsim_path = shutil.which("dimsim") or str(Path.home() / ".deno" / "bin" / "dimsim") + self.config.executable = dimsim_path + self.config.extra_args = dev_args + self.config.cwd = None + + def _maybe_build(self) -> None: + """Ensure dimsim CLI, core assets, and scene are latest from S3.""" + if self.config.local: + return # Local dev — skip install + + import json + import subprocess + import urllib.request + + deno = _find_deno() + scene = self.config.scene + + # Check installed CLI version against S3 registry + dimsim = shutil.which("dimsim") + installed_ver = None + if dimsim: + try: + result = subprocess.run( + [dimsim, "--version"], + capture_output=True, + text=True, + timeout=5, + ) + installed_ver = result.stdout.strip() if result.returncode == 0 else None + except Exception: + pass + + # Fetch registry version from S3 (tiny JSON, fast) + registry_ver = None + try: + with urllib.request.urlopen( + "https://dimsim-assets.s3.amazonaws.com/scenes.json", timeout=5 + ) as resp: + registry_ver = json.loads(resp.read()).get("version") + except Exception: + pass + + if not dimsim or installed_ver != registry_ver: + logger.info( + f"Updating dimsim CLI: {installed_ver or 'not installed'}" + f" → {registry_ver or 'latest'}", + ) + subprocess.run( + [deno, "install", "-gAf", "--reload", "--unstable-net", _DIMSIM_JSR], + check=True, + ) + dimsim = shutil.which("dimsim") + if not dimsim: + raise FileNotFoundError("dimsim install failed — not found in PATH") + else: + logger.info(f"dimsim CLI up-to-date (v{installed_ver})") + + # setup/scene have version-aware caching (only downloads if version changed) + logger.info("Checking core assets...") + subprocess.run([dimsim, "setup"], check=True) + + logger.info(f"Checking scene '{scene}'...") + subprocess.run([dimsim, "scene", "install", scene], check=True) + + def _collect_topics(self) -> dict[str, str]: + """Bridge hardcodes LCM channel names — no topic args needed.""" + return {} + + +sim_bridge = DimSimBridge.blueprint + +__all__ = ["DimSimBridge", "DimSimBridgeConfig", "sim_bridge"] diff --git a/dimos/robot/sim/tf_module.py b/dimos/robot/sim/tf_module.py new file mode 100644 index 0000000000..54d3332d2f --- /dev/null +++ b/dimos/robot/sim/tf_module.py @@ -0,0 +1,176 @@ +# Copyright 2026 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. + +"""Lightweight TF publisher for DimSim. + +Subscribes to odometry from the DimSim bridge (via LCM, wired by autoconnect) +and publishes the transform chain: world -> base_link -> {camera_link -> +camera_optical, lidar_link}. Also publishes CameraInfo at 1 Hz, forwards +cmd_vel to the bridge, and exposes a ``move()`` RPC. + +This module replaces the TF / camera_info / cmd_vel parts of the old +DimSimConnection while the NativeModule bridge handles sensor data directly. +""" + +from __future__ import annotations + +import math +from threading import Thread +import time + +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs import ( + PoseStamped, + Quaternion, + Transform, + Twist, + Vector3, +) +from dimos.msgs.sensor_msgs import CameraInfo +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# DimSim captures at 960x432 with 80-degree horizontal FOV. +_DIMSIM_WIDTH = 960 +_DIMSIM_HEIGHT = 432 +_DIMSIM_FOV_DEG = 80 + + +def _camera_info_static() -> CameraInfo: + """Build CameraInfo for DimSim's virtual camera.""" + fov_rad = math.radians(_DIMSIM_FOV_DEG) + fx = (_DIMSIM_WIDTH / 2) / math.tan(fov_rad / 2) + fy = fx # square pixels + cx = _DIMSIM_WIDTH / 2.0 + cy = _DIMSIM_HEIGHT / 2.0 + + return CameraInfo( + frame_id="camera_optical", + height=_DIMSIM_HEIGHT, + width=_DIMSIM_WIDTH, + distortion_model="plumb_bob", + D=[0.0, 0.0, 0.0, 0.0, 0.0], + K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + binning_x=0, + binning_y=0, + ) + + +class DimSimTF(Module): + """Lightweight TF publisher for the DimSim simulator. + + Wired by autoconnect to receive odom from the bridge's LCM output. + Publishes TF transforms and camera intrinsics. Exposes ``move()`` RPC + for sending cmd_vel to the bridge. + """ + + # Odom input — autoconnect wires this to DimSimBridge.odom via LCM + odom: In[PoseStamped] + + # Outputs + camera_info: Out[CameraInfo] + cmd_vel: Out[Twist] + + _camera_info_thread: Thread | None = None + _latest_odom: PoseStamped | None = None + _odom_last_ts: float = 0.0 + _odom_count: int = 0 + + @classmethod + def _odom_to_tf(cls, odom: PoseStamped) -> list[Transform]: + """Build transform chain from odometry pose. + + Transform tree: world -> base_link -> {camera_link -> camera_optical, lidar_link} + """ + camera_link = Transform( + translation=Vector3(0.3, 0.0, 0.0), # camera 30cm forward + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ts=odom.ts, + ) + + camera_optical = Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), + frame_id="camera_link", + child_frame_id="camera_optical", + ts=odom.ts, + ) + + lidar_link = Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="lidar_link", + ts=odom.ts, + ) + + return [ + Transform.from_pose("base_link", odom), + camera_link, + camera_optical, + lidar_link, + ] + + def _on_odom(self, pose: PoseStamped) -> None: + """Handle incoming odometry — publish TF transforms.""" + self._latest_odom = pose + self._odom_count += 1 + + transforms = self._odom_to_tf(pose) + self.tf.publish(*transforms) + + def _publish_camera_info_loop(self) -> None: + """Publish camera intrinsics at 1 Hz.""" + while self._camera_info_thread is not None: + self.camera_info.publish(_camera_info_static()) + time.sleep(1.0) + + @rpc + def start(self) -> None: + super().start() + + from reactivex.disposable import Disposable + + self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) + + self._camera_info_thread = Thread(target=self._publish_camera_info_loop, daemon=True) + self._camera_info_thread.start() + + logger.info("DimSimTF started — listening for odom, publishing TF + camera_info") + + @rpc + def stop(self) -> None: + thread = self._camera_info_thread + self._camera_info_thread = None + if thread and thread.is_alive(): + thread.join(timeout=1.0) + super().stop() + + @rpc + def move(self, twist: Twist, duration: float = 0.0) -> bool: + """Send movement command to the simulator via cmd_vel.""" + self.cmd_vel.publish(twist) + return True + + +sim_tf = DimSimTF.blueprint + +__all__ = ["DimSimTF", "sim_tf"]