diff --git a/README.md b/README.md index fc7bd0013f..a5f5940e73 100644 --- a/README.md +++ b/README.md @@ -191,7 +191,7 @@ dimos run unitree-go2 | Run command | What it does | |-------------|-------------| | `dimos --replay run unitree-go2` | Quadruped navigation replay — SLAM, costmap, A* planning | -| `dimos --replay --replay-db go2_bigoffice run unitree-go2-memory` | Quadruped temporal memory replay | +| `dimos --replay --replay-db go2_bigoffice run unitree-go2` | Quadruped temporal memory replay | | `dimos --simulation run unitree-go2-agentic` | Quadruped agentic + MCP server in simulation | | `dimos --simulation run unitree-g1-sim` | Humanoid in MuJoCo simulation | | `dimos --replay run drone-basic` | Drone video + telemetry replay | diff --git a/dimos/cli/commands/lifecycle.py b/dimos/cli/commands/lifecycle.py index 866ab8ec55..4835b20756 100644 --- a/dimos/cli/commands/lifecycle.py +++ b/dimos/cli/commands/lifecycle.py @@ -27,7 +27,7 @@ import typer -from dimos.constants import CONFIG_DIR, LOG_DIR +from dimos.constants import CONFIG_DIR, LOG_DIR, RECORDINGS_DIR from dimos.core.daemon import ( fork_daemon, install_signal_handlers, @@ -149,6 +149,9 @@ def run( except BlueprintConfigError as error: typer.echo(f"Error: {error}", err=True) raise typer.Exit(2) from error + blueprint_name = "-".join(blueprint_names) + run_id = generate_run_id(blueprint_name) + preparsed_global_config.setdefault("recording_dir", str(RECORDINGS_DIR / run_id)) # Some blueprint modules select their composition at import time, so all # global sources must be visible before resolving the requested names. global_config.update(**preparsed_global_config) @@ -195,8 +198,6 @@ def run( if stale: logger.info(f"Cleaned {stale} stale run entries") - blueprint_name = "-".join(blueprint_names) - run_id = generate_run_id(blueprint_name) log_dir = LOG_DIR / run_id # Tag every descendant with the run id so the watchdog and stale-run diff --git a/dimos/core/coordination/blueprint_config/test_parser.py b/dimos/core/coordination/blueprint_config/test_parser.py index b75ee52707..0e01835a18 100644 --- a/dimos/core/coordination/blueprint_config/test_parser.py +++ b/dimos/core/coordination/blueprint_config/test_parser.py @@ -152,12 +152,17 @@ class CollisionModule(Module): "module-value", "--replay=false", "--no-obstacle-avoidance", + "--record", + "--record-format", + "sqlite", ], environ={}, ) assert parsed.global_config_values()["robot_ip"] == "192.0.2.10" assert parsed.global_config_values()["replay"] is False + assert parsed.global_config_values()["record"] is True + assert parsed.global_config_values()["record_format"] == "sqlite" assert parsed.global_config_values()["obstacle_avoidance"] is False assert parsed.module_kwargs("collisionmodule") == {"robot_ip": "module-value"} diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 8b5e448157..eaa9102d1b 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -60,6 +60,11 @@ class GlobalConfig(BaseSettings): simulation: str = "" replay: bool = False replay_db: str = "go2_short" + # --record: Recorders in the blueprint write under recording_dir (off unless set). + record: bool = False + record_format: Literal["mcap", "sqlite"] = "mcap" + # `dimos run` sets this to RECORDINGS_DIR/ (same id as the log dir). + recording_dir: str = "" new_memory: bool = False # How every zenoh session this process opens joins the network. zenoh_mode: ZenohProcessMode = "peer" diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py index 7189256d45..72dc8198a6 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py @@ -33,4 +33,4 @@ from dimos.experimental.robot.bosdyn.spot.blueprints.spot import spot from dimos.experimental.robot.bosdyn.spot.recorder import SpotRecorder -spot_record = autoconnect(spot, SpotRecorder.blueprint()) +spot_record = autoconnect(spot, SpotRecorder.blueprint()).global_config(record=True) diff --git a/dimos/experimental/world_belief/xarm6_blueprint.py b/dimos/experimental/world_belief/xarm6_blueprint.py index cf01ea4bb4..996234bdc9 100644 --- a/dimos/experimental/world_belief/xarm6_blueprint.py +++ b/dimos/experimental/world_belief/xarm6_blueprint.py @@ -134,4 +134,4 @@ def _rerun_blueprint() -> rrb.Blueprint: hardware=[_hw], tasks=[trajectory_task(_hw)], ), -).global_config(n_workers=8) +).global_config(n_workers=8, record=True) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 0c42313508..1facbff827 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -27,7 +27,7 @@ # add to existing .db (a missing --db is fetched via get_data before falling # back to building from scratch) - DB="mem2.db" + DB="memory.db" python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" # A quick-look .rrd (aggregated world lidar + pose path) is written next @@ -263,7 +263,7 @@ def _build_blueprint( (FastLio2Recorder, "fastlio_lidar", "lidar"), ] ) - .global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") + .global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db", record=True) ) diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 3ca9552686..e61c839af7 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -23,7 +23,7 @@ # add to existing .db (a missing --db is fetched via get_data before falling # back to building from scratch; a missing --pcap is likewise fetched) - DB="mem2.db" + DB="memory.db" python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" # A quick-look .rrd (aggregated world lidar + pose path) is written next @@ -342,7 +342,7 @@ def _build_blueprint( (PointlioRecorder, _LIDAR_STREAM, "lidar"), ] ) - .global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + .global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db", record=True) ) diff --git a/dimos/imitation/collection/blueprint.py b/dimos/imitation/collection/blueprint.py index 4039e78836..75b07df053 100644 --- a/dimos/imitation/collection/blueprint.py +++ b/dimos/imitation/collection/blueprint.py @@ -63,7 +63,7 @@ def _camera_if_real() -> tuple[Blueprint, ...]: EpisodeMonitorModule.blueprint(), # default button_map: toggle=B, discard=Y teleop_quest_xarm7, *_camera_if_real(), -) +).global_config(record=True) learning_collect_quest_piper = autoconnect( @@ -75,4 +75,4 @@ def _camera_if_real() -> tuple[Blueprint, ...]: EpisodeMonitorModule.blueprint(), # default button_map: toggle=B, discard=Y teleop_quest_piper, *_camera_if_real(), -) +).global_config(record=True) diff --git a/dimos/memory/module.py b/dimos/memory/module.py index 4293b561cf..cc750f9af2 100644 --- a/dimos/memory/module.py +++ b/dimos/memory/module.py @@ -31,17 +31,22 @@ from dimos.agents.annotation import skill from dimos.constants import DIMOS_PROJECT_ROOT, RECORDINGS_DIR from dimos.core.core import rpc +from dimos.core.global_config import global_config from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.memory.embed import EmbedImages +from dimos.memory.store.base import Store +from dimos.memory.store.mcap import McapStore from dimos.memory.store.null import NullStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow from dimos.memory.type.observation import EmbeddedObservation, Observation from dimos.models.embedding.base import EmbeddingModel +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger @@ -50,7 +55,6 @@ from reactivex.abc import DisposableBase from dimos.core.stream import Out - from dimos.msgs.geometry_msgs.Pose import Pose logger = setup_logger() @@ -72,6 +76,15 @@ def default_recording_dir() -> Path: return RECORDINGS_DIR / stamp +def recording_dir() -> Path: + """Where this run records (memory.*, pcaps, ...): ``--recording-dir``, else a fresh stamp.""" + return ( + Path(global_config.recording_dir) + if global_config.recording_dir + else default_recording_dir() + ) + + def stream_to_port(stream: Stream[T], out: Out[T]) -> DisposableBase: """Forward each observation's ``data`` from *stream* to a Module ``Out`` port. @@ -177,11 +190,13 @@ def stop(self) -> None: class MemoryModuleConfig(ModuleConfig): - db_path: str | Path = "recording.db" + db_path: str | Path | None = "recording.db" @field_validator("db_path", mode="before") @classmethod - def _resolve_path(cls, v: str | Path) -> Path: + def _resolve_path(cls, v: str | Path | None) -> Path | None: + if v is None: + return None p = Path(os.fspath(v)) if not p.is_absolute(): p = DIMOS_PROJECT_ROOT / p @@ -196,17 +211,34 @@ class MemoryModule(Module): """ config: MemoryModuleConfig - _store: SqliteStore | None = None + _store: Store | None = None @property - def store(self) -> SqliteStore: + def db_path(self) -> Path: + """``config.db_path``, defaulting to ``/memory.``.""" + g = self.config.g + if self.config.db_path is None: + base = Path(g.recording_dir) if g.recording_dir else default_recording_dir() + p = base / "memory" + else: + p = Path(self.config.db_path) + if p.suffix: + return p + return p.with_suffix(".mcap" if g.record_format == "mcap" else ".db") + + @property + def store(self) -> Store: if self._store is not None: return self._store - Path(self.config.db_path).parent.mkdir(parents=True, exist_ok=True) - self._store = self.register_disposable( - SqliteStore(path=str(self.config.db_path)), - ) + path = self.db_path + path.parent.mkdir(parents=True, exist_ok=True) + store: Store + if path.suffix == ".mcap": + store = McapStore(path=str(path), mode="w") + else: + store = SqliteStore(path=str(path)) + self._store = self.register_disposable(store) self._store.start() return self._store @@ -281,7 +313,7 @@ class RecorderConfig(MemoryModuleConfig): root_frame: str = "world" default_frame_id: str = "base_link" tf_tolerance: float = 0.5 - db_path: str | Path = "recording.db" + db_path: str | Path | None = None # default: recording_dir()/memory. # Also record the live tf stream (under "tf") alongside the In ports. record_tf: bool = True # Rename recorded streams: {port_name: db_stream_name}. Conceptually this is @@ -347,9 +379,10 @@ def start(self) -> None: super().start() if self.config.g.replay: - logger.info( - "Replay mode active — Recorder disabled, leaving %s untouched", self.config.db_path - ) + logger.info("Replay mode active — Recorder disabled") + return + if not self.config.g.record: + logger.info("Recording off — pass --record to write %s", self.db_path) return self._pose_setters = self._collect_pose_setters() @@ -358,7 +391,7 @@ def start(self) -> None: # shouldn't need to know about files (SqliteStore specific), and # .live() subs need to know how to re-sub in case of a restart of # this module in a deployed blueprint. - db_path = Path(self.config.db_path) + db_path = self.db_path if db_path.exists(): if self.config.on_existing is OnExisting.APPEND: pass # keep the db; _prepare_streams handles any per-stream replacement @@ -484,3 +517,22 @@ def on_tf(msg: TFMessage) -> None: pass self.register_disposable(Disposable(self.tf.subscribe(on_tf))) + + +class OdomRecorder(Recorder): + """Records ``color_image``, ``lidar``, ``odom`` (+ tf), posing each frame at the latest odom.""" + + color_image: In[Image] + lidar: In[PointCloud2] + odom: In[PoseStamped] + + _last_odom: Pose | None = None + + @pose_setter_for("odom") + async def _odom_pose(self, msg: PoseStamped) -> Pose | None: + self._last_odom = msg + return self._last_odom + + @pose_setter_for("lidar") + async def _lidar_pose(self, msg: PointCloud2) -> Pose | None: + return self._last_odom diff --git a/dimos/memory/store/mcap.py b/dimos/memory/store/mcap.py index 26f2c04876..13815cc2eb 100644 --- a/dimos/memory/store/mcap.py +++ b/dimos/memory/store/mcap.py @@ -12,15 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Read-only memory store backed by an mcap file. +"""Memory store backed by an mcap file. -Generic and codec-injected — it knows nothing about any robot. The caller -supplies ``codecs`` (DDS/wire topic -> codec that decodes a message's stored -bytes) and an optional ``streams`` map (friendly stream name -> topic). See -``dimos.robot.unitree.go2.dds.store.Go2McapStore`` for the Go2 wiring. +Write mode (``mode="w"``) is what a Recorder uses: one channel per stream, +``message_encoding="dimos-obs"``, schema name = payload type path, schema data += codec id. Each message is an envelope: 4-byte big-endian header length, JSON +header ``{"pose": [x,y,z,qx,qy,qz,qw] | null, "tags": {...}}``, codec payload. +No blobs, vectors, or embeddings. -Read-only: no append, blobs, vectors, or embeddings. Payloads decode lazily on -``obs.data``; ts and counts are cheap (counts come from the mcap index). +Read mode decodes those channels with the recorded codec. Foreign channels +(e.g. Go2 DDS captures) need injected ``codecs`` (topic -> codec) and an +optional ``streams`` map (friendly stream name -> topic). See +``dimos.robot.unitree.go2.dds.store.Go2McapStore`` for the Go2 wiring. +Payloads decode lazily on ``obs.data``; counts come from the mcap index. """ from __future__ import annotations @@ -28,10 +32,14 @@ from collections.abc import Iterator, Mapping from dataclasses import replace from functools import partial -from typing import Any, Protocol, runtime_checkable +import json +import os +import struct +import threading +from typing import Any, Literal, Protocol, runtime_checkable from dimos.memory.backend import Backend -from dimos.memory.codecs.base import codec_for +from dimos.memory.codecs.base import Codec, codec_for, codec_from_id, codec_id, resolve_payload_type from dimos.memory.notifier.subject import SubjectNotifier from dimos.memory.observationstore.base import ObservationStore, ObservationStoreConfig from dimos.memory.store.base import Store, StoreConfig @@ -60,6 +68,30 @@ def decode(self, data: bytes) -> bytes: _BYTES_CODEC = _BytesCodec() +DIMOS_ENCODING = "dimos-obs" +_HDR = struct.Struct(">I") + + +class _DimosCodec: + """Adapts a recorded dimos codec (schema id + payload type) to :class:`StreamCodec`.""" + + def __init__(self, codec: Codec[Any], payload_type: type) -> None: + self.codec = codec + self.payload_type = payload_type + + def decode(self, data: bytes) -> Any: + return self.codec.decode(data[_HDR.size + _HDR.unpack_from(data)[0] :]) + + +def _pack(obs: Observation[Any], payload: bytes) -> bytes: + header = json.dumps({"pose": obs.pose_tuple, "tags": obs.tags}).encode() + return _HDR.pack(len(header)) + header + payload + + +def _unpack_header(data: bytes) -> dict[str, Any]: + n = _HDR.unpack_from(data)[0] + return json.loads(data[_HDR.size : _HDR.size + n]) # type: ignore[no-any-return] + def _slug(topic: str) -> str: """Auto stream name from a topic: drop the ``rt/`` prefix and ``/`` -> ``_``. @@ -96,11 +128,15 @@ def _iter(self, reverse: bool = False) -> Iterator[Observation[Any]]: decode, dtype, n = self._codec.decode, self._codec.payload_type, self._count with open(self._path, "rb") as f: msgs = make_reader(f).iter_messages(topics=[self._topic], reverse=reverse) - for i, (_s, _c, m) in enumerate(msgs): + for i, (_s, ch, m) in enumerate(msgs): + hdr = _unpack_header(m.data) if ch.message_encoding == DIMOS_ENCODING else {} + pose = hdr.get("pose") yield Observation( id=(n - 1 - i) if reverse else i, ts=m.log_time / 1e9, data_type=dtype, + pose_tuple=tuple(pose) if pose else None, + tags=hdr.get("tags"), _loader=partial(decode, m.data), ) @@ -128,19 +164,63 @@ def fetch_by_ids(self, ids: list[int]) -> list[Observation[Any]]: return [o for o in self._iter() if o.id in want] def insert(self, obs: Observation[Any]) -> int: - raise NotImplementedError("McapStore is read-only") + raise NotImplementedError("McapStore opened read-only") + + +class McapWriteObservationStore(ObservationStore[Any]): + """Append-only channel writer. Encodes the payload itself (no blob store).""" + + config: McapObservationStoreConfig + + def __init__( + self, *, name: str, writer: Any, channel_id: int, codec: Codec[Any], lock: threading.Lock + ) -> None: + super().__init__(name=name) + self._writer = writer + self._channel_id = channel_id + self._codec = codec + self._lock = lock + self._n = 0 + + @property + def name(self) -> str: + return self.config.name + + def insert(self, obs: Observation[Any]) -> int: + data = _pack(obs, self._codec.encode(obs.data)) + t = int(obs.ts * 1e9) + with self._lock: + row_id = self._n + self._n += 1 + self._writer.add_message( + channel_id=self._channel_id, log_time=t, publish_time=t, data=data, sequence=row_id + ) + return row_id + + def query(self, q: StreamQuery) -> Iterator[Observation[Any]]: + raise NotImplementedError("McapStore opened write-only; reopen the file to read") + + def count(self, q: StreamQuery) -> int: + return self._n + + def fetch_by_ids(self, ids: list[int]) -> list[Observation[Any]]: + raise NotImplementedError("McapStore opened write-only; reopen the file to read") class McapStoreConfig(StoreConfig): path: str = "" + mode: Literal["r", "w"] = "r" class McapStore(Store): - """A memory store backed by an mcap file (read-only). + """A memory store backed by an mcap file. - Every channel present in the file with a codec is exposed. Names default to + Read mode exposes every channel in the file: dimos-recorded channels decode + with their recorded codec; others need an injected codec. Names default to the slugified topic (see :func:`_slug`); ``streams`` (friendly name -> topic) overrides the name for specific topics. + + Write mode creates the file; each ``stream(name, payload_type)`` opens a channel. """ config: McapStoreConfig @@ -148,36 +228,98 @@ class McapStore(Store): def __init__( self, *, - codecs: Mapping[str, StreamCodec], + codecs: Mapping[str, StreamCodec] | None = None, streams: dict[str, str] | None = None, **kwargs: Any, ) -> None: - from mcap.reader import make_reader # optional dep (go2/unitree extra) - super().__init__(**kwargs) - self._codecs = codecs - name_of = {topic: name for name, topic in (streams or {}).items()} # topic -> override - with open(self.config.path, "rb") as f: - summary = make_reader(f).get_summary() + self._codecs: dict[str, StreamCodec] = dict(codecs or {}) self._stream_topic: dict[str, str] = {} # stream name -> topic self._available: dict[str, int] = {} # stream name -> message count # Channels with no registered codec are still exposed, as Stream[bytes] via # _BYTES_CODEC — reachable but undecoded. _raw maps their stream name to the # source schema so summary() can flag them [raw bytes: ]. self._raw: dict[str, str | None] = {} # raw stream name -> source schema - if summary is not None and summary.statistics is not None: - for cid, ch in summary.channels.items(): - count = summary.statistics.channel_message_counts.get(cid, 0) - name = name_of.get(ch.topic) or _slug(ch.topic) - self._stream_topic[name] = ch.topic - self._available[name] = count - if ch.topic not in self._codecs: - sch = summary.schemas.get(ch.schema_id) - self._raw[name] = sch.name if sch else None + self._writer: Any = None + self._lock = threading.Lock() + if self.config.mode == "w": + self._open_writer() + else: + self._scan(streams) + + def _open_writer(self) -> None: + from mcap.writer import Writer + + parent = os.path.dirname(self.config.path) + if parent: + os.makedirs(parent, exist_ok=True) + self._file = open(self.config.path, "wb") + self._writer = Writer(self._file) + self._writer.start(profile="dimos", library="dimos") + + def _scan(self, streams: dict[str, str] | None) -> None: + from mcap.reader import make_reader + + name_of = {topic: name for name, topic in (streams or {}).items()} # topic -> override + with open(self.config.path, "rb") as f: + summary = make_reader(f).get_summary() + if summary is None or summary.statistics is None: + return + for cid, ch in summary.channels.items(): + count = summary.statistics.channel_message_counts.get(cid, 0) + name = name_of.get(ch.topic) or _slug(ch.topic) + self._stream_topic[name] = ch.topic + self._available[name] = count + sch = summary.schemas.get(ch.schema_id) + if ch.message_encoding == DIMOS_ENCODING and sch is not None: + ptype = resolve_payload_type(sch.name) + self._codecs[ch.topic] = _DimosCodec( + codec_from_id(sch.data.decode(), sch.name), ptype + ) + elif ch.topic not in self._codecs: + self._raw[name] = sch.name if sch else None + + def _create_write_backend( + self, name: str, payload_type: type | None, raw_codec: Any + ) -> Backend[Any]: + if payload_type is None: + raise TypeError(f"Stream {name!r}: payload_type is required to record") + codec = self._resolve_codec(payload_type, raw_codec) + module = f"{payload_type.__module__}.{payload_type.__qualname__}" + with self._lock: + schema_id = self._writer.register_schema( + name=module, encoding="dimos", data=codec_id(codec).encode() + ) + channel_id = self._writer.register_channel( + topic=name, message_encoding=DIMOS_ENCODING, schema_id=schema_id + ) + self._stream_topic[name] = name + obs = McapWriteObservationStore( + name=name, writer=self._writer, channel_id=channel_id, codec=codec, lock=self._lock + ) + return Backend( + metadata_store=obs, + codec=codec, + data_type=payload_type, + blob_store=None, + vector_store=None, + notifier=SubjectNotifier(), + ) + + def delete_stream(self, name: str) -> None: + raise NotImplementedError("McapStore cannot delete streams; record to a new file") def list_streams(self) -> list[str]: return sorted(set(self._available) | set(self._streams)) + def stop(self) -> None: + super().stop() + if self._writer is not None: + with self._lock: + self._writer.finish() + self._file.close() + self._writer = None + def summary(self) -> str: """Base summary, tagging codecless streams with ``[raw bytes: ]``.""" lines = [] @@ -192,6 +334,8 @@ def summary(self) -> str: def _create_backend( self, name: str, payload_type: type | None = None, **config: Any ) -> Backend[Any]: + if self._writer is not None: + return self._create_write_backend(name, payload_type, config.get("codec")) if name not in self._available: raise KeyError(f"No stream {name!r}. Available: {sorted(self._available)}") topic = self._stream_topic[name] diff --git a/dimos/memory/store/test_mcap.py b/dimos/memory/store/test_mcap.py new file mode 100644 index 0000000000..01d46515a8 --- /dev/null +++ b/dimos/memory/store/test_mcap.py @@ -0,0 +1,62 @@ +# 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. + +from pathlib import Path + +import numpy as np +import pytest + +from dimos.memory.store.mcap import McapStore +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image + + +def test_write_then_read_roundtrip(tmp_path: Path) -> None: + path = tmp_path / "rec.mcap" + w = McapStore(path=str(path), mode="w") + w.start() + imgs = w.stream("color_image", Image) + odom = w.stream("odom", PoseStamped) + nums = w.stream("nums", int) + img = Image(data=np.zeros((4, 4, 3), dtype=np.uint8)) + imgs.append(img, ts=1.0, pose=(1, 2, 3, 0, 0, 0, 1), tags={"reception_ts": 1.5}) + imgs.append(img, ts=2.0) + odom.append(PoseStamped(ts=3.0), ts=3.0) + nums.append(7, ts=4.0) + w.stop() + + r = McapStore(path=str(path)) + r.start() + assert r.list_streams() == ["color_image", "nums", "odom"] + got = list(r.stream("color_image")) + assert [o.ts for o in got] == [1.0, 2.0] + assert got[0].pose_tuple == (1, 2, 3, 0, 0, 0, 1) + assert got[0].tags == {"reception_ts": 1.5} + assert got[1].pose_tuple is None + assert isinstance(got[0].data, Image) + assert got[0].data.data.shape == (4, 4, 3) + assert isinstance(next(iter(r.stream("odom"))).data, PoseStamped) + assert next(iter(r.stream("nums"))).data == 7 + assert r.stream("color_image").count() == 2 + r.stop() + + +def test_write_only_cannot_query(tmp_path: Path) -> None: + w = McapStore(path=str(tmp_path / "rec.mcap"), mode="w") + w.start() + s = w.stream("nums", int) + s.append(1) + with pytest.raises(NotImplementedError): + list(s) + w.stop() diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 6924ee9963..22f23f2486 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -225,7 +225,7 @@ def _copy_recording(src: Path, dest: Path) -> None: @app.command() def ingest( source: Path = typer.Argument( - ..., help="Recording to ingest: a mem2.db file or the directory holding one" + ..., help="Recording to ingest: a memory.db file or the directory holding one" ), name: str = typer.Option(..., "--name", help="Dataset name; becomes data/.db"), lidar_stream: str = typer.Option("pointlio_lidar", "--lidar-stream"), @@ -236,7 +236,7 @@ def ingest( force: bool = typer.Option(False, "--force", help="Overwrite dataset and manifest"), ) -> None: """Register a recording as a dataset: copy, map, generate cases.""" - src = source / "mem2.db" if source.is_dir() else source + src = source / "memory.db" if source.is_dir() else source if not src.exists(): raise typer.BadParameter(f"{src} does not exist") manifest = manifest_path(name) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 671a8baf57..e3445e8ba4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -132,7 +132,6 @@ "unitree-go2-holonomic-controller": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_holonomic_controller:unitree_go2_holonomic_controller", "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", - "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", "unitree-go2-mid360-record": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record:unitree_go2_mid360_record", "unitree-go2-mls-htc": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_mls_htc:unitree_go2_mls_htc", "unitree-go2-multi": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_multi:unitree_go2_multi", @@ -188,6 +187,7 @@ "detection3-d-module": "dimos.perception.detection.module3D.Detection3DModule", "drone-camera-module": "dimos.robot.drone.camera_module.DroneCameraModule", "drone-connection-module": "dimos.robot.drone.connection_module.DroneConnectionModule", + "drone-recorder": "dimos.robot.drone.blueprints.basic.drone_basic.DroneRecorder", "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "episode-monitor-module": "dimos.imitation.collection.episode_monitor.EpisodeMonitorModule", @@ -207,7 +207,6 @@ "go2-command-module": "dimos.teleop.hosted.go2_command.Go2CommandModule", "go2-connection": "dimos.robot.unitree.go2.connection.GO2Connection", "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection.Go2FleetConnection", - "go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2.Go2Memory", "go2-mid360-recorder": "dimos.robot.unitree.go2.go2_mid360_recorder.Go2Mid360Recorder", "go2-mid360-static-tf": "dimos.robot.unitree.go2.go2_mid360_static_transforms.Go2Mid360StaticTf", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", @@ -248,6 +247,7 @@ "object-tracker3-d": "dimos.perception.experimental.object_tracker_3d.ObjectTracker3D", "object-tracking": "dimos.perception.experimental.object_tracker.ObjectTracking", "observe-skill": "dimos.agents.skills.observe_skill.ObserveSkill", + "odom-recorder": "dimos.memory.module.OdomRecorder", "open-arm-teleop-coordinator": "dimos.robot.manipulators.openarm.blueprints.teleop.OpenArmTeleopCoordinator", "osm-skill": "dimos.agents.skills.osm.OsmSkill", "path-following-coordinator": "dimos.control.path_following_coordinator.PathFollowingCoordinator", diff --git a/dimos/robot/assembly/mid360_realsense_30.py b/dimos/robot/assembly/mid360_realsense_30.py index 50956d1318..665118d55d 100644 --- a/dimos/robot/assembly/mid360_realsense_30.py +++ b/dimos/robot/assembly/mid360_realsense_30.py @@ -137,7 +137,7 @@ class Mid360RealsenseRecorder(PointlioRecorder): Mid360RealsenseRecorder.blueprint(), # Continuously republishes the rig's mount frames onto tf (no latched static tf). Mid360RealsenseStaticTf.blueprint(), -).global_config(n_workers=8) +).global_config(n_workers=8, record=True) # Same rig, also capturing a raw .pcap of the Mid-360 UDP stream. mid360_realsense_record_with_pcap = autoconnect( diff --git a/dimos/robot/drone/blueprints/basic/drone_basic.py b/dimos/robot/drone/blueprints/basic/drone_basic.py index 4fbee93840..b4c364a47a 100644 --- a/dimos/robot/drone/blueprints/basic/drone_basic.py +++ b/dimos/robot/drone/blueprints/basic/drone_basic.py @@ -20,6 +20,11 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.core.stream import In +from dimos.memory.module import Recorder, pose_setter_for +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image from dimos.robot.drone.camera_module import DroneCameraModule from dimos.robot.drone.connection_module import DroneConnectionModule from dimos.visualization.vis_module import vis_module @@ -66,6 +71,23 @@ def _drone_rerun_blueprint() -> Any: _vis = vis_module(global_config.viewer, rerun_config=_rerun_config) + +class DroneRecorder(Recorder): + color_image: In[Image] + odom: In[PoseStamped] + + _last_odom: Pose | None = None + + @pose_setter_for("odom") + async def _odom_pose(self, msg: PoseStamped) -> Pose | None: + self._last_odom = msg + return self._last_odom + + @pose_setter_for("color_image") + async def _image_pose(self, msg: Image) -> Pose | None: + return self._last_odom + + # Determine connection string based on replay flag connection_string = "udp:0.0.0.0:14550" video_port = 5600 @@ -80,4 +102,5 @@ def _drone_rerun_blueprint() -> Any: outdoor=False, ), DroneCameraModule.blueprint(camera_intrinsics=[1000.0, 1000.0, 960.0, 540.0]), + DroneRecorder.blueprint(), ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py index 4fcc998164..59542529ed 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py @@ -27,7 +27,6 @@ from dimos.core.global_config import global_config from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.hardware.sensors.lidar.pointlio.module import PointLio -from dimos.memory.module import default_recording_dir from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk @@ -35,8 +34,6 @@ from dimos.robot.unitree.g1.g1_tf_publisher import G1TfPublisher from dimos.visualization.vis_module import vis_module -_RECORDING_DIR = default_recording_dir() - def _g1_record_rerun_blueprint() -> Any: """Split layout: camera feed + 3D world view side by side.""" @@ -123,12 +120,12 @@ def _convert_depth_camera_info(camera_info: CameraInfo) -> Any: (RealSenseCamera, "depth_camera_info", "realsense_depth_camera_info"), ] ), - G1Recorder.blueprint(db_path=str(_RECORDING_DIR / "mem2.db")), + G1Recorder.blueprint(), # Mount frames onto tf, base_link edge live from the waist joints. G1TfPublisher.blueprint(), # Viewer keyboard teleop feeds MovementManager via tele_cmd_vel. _record_vis, -).global_config(n_workers=12, robot_model="unitree_g1") +).global_config(n_workers=12, robot_model="unitree_g1", record=True) if __name__ == "__main__": diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py index 8fc4b90633..1a08c3edd4 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py @@ -27,6 +27,7 @@ from dimos.hardware.sensors.camera.zed import compat as zed from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels.module import VoxelGridMapper +from dimos.memory.module import OdomRecorder from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -129,8 +130,9 @@ def _create_webcam() -> Webcam: VoxelGridMapper.blueprint(), CostMapper.blueprint(), WavefrontFrontierExplorer.blueprint(), + OdomRecorder.blueprint().remappings([(OdomRecorder, "lidar", "pointcloud")]), ) - .global_config(n_workers=4, robot_model="unitree_g1") + .global_config(n_workers=5, robot_model="unitree_g1") .transports( { # G1 uses Twist for movement commands diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index c55c6e1b9c..eb3e9f81a8 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -18,6 +18,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.memory.module import OdomRecorder from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module @@ -104,6 +105,7 @@ def _go2_rerun_blueprint() -> Any: }, } + _with_vis = autoconnect( vis_module( viewer_backend=global_config.viewer, @@ -116,7 +118,8 @@ def _go2_rerun_blueprint() -> Any: autoconnect( _with_vis, GO2Connection.blueprint(), - ).global_config(n_workers=4, robot_model="unitree_go2") + OdomRecorder.blueprint(), + ).global_config(n_workers=5, robot_model="unitree_go2") # we temporarily disabled sensor timestamps # and are derriving all timestmaps upon reception # this is because image webrtc stream doesn't have timestamps, diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py index e30aefcf84..6fdeb022f2 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -36,7 +36,7 @@ from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder -from dimos.memory.module import default_recording_dir +from dimos.memory.module import recording_dir from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.go2.go2_mid360_recorder import Go2Mid360Recorder @@ -50,9 +50,6 @@ _TELEOP_ANGULAR_SPEED = 0.6 -_RECORDING_DIR = default_recording_dir() - - unitree_go2_mid360_record = autoconnect( MovementManager.blueprint(), GO2Connection.blueprint(publish_tf=False).remappings( @@ -73,7 +70,7 @@ (PointLio, "odometry", "pointlio_odometry"), ] ), - Go2Mid360Recorder.blueprint(db_path=str(_RECORDING_DIR / "mem2.db")), + Go2Mid360Recorder.blueprint(), # Continuously republishes the rig's mount frames onto tf (no latched static tf). Go2Mid360StaticTf.blueprint(), # Pygame keyboard teleop (WASD drive + Q/E strafe). Its cmd_vel feeds @@ -85,12 +82,12 @@ (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"), ] ), -).global_config(n_workers=12, robot_model="unitree_go2") +).global_config(n_workers=12, robot_model="unitree_go2", record=True) if _RECORD_PCAP: unitree_go2_mid360_record = autoconnect( unitree_go2_mid360_record, - Mid360PcapRecorder.blueprint(pcap_path=str(_RECORDING_DIR / "mid360.pcap")), + Mid360PcapRecorder.blueprint(pcap_path=str(recording_dir() / "mid360.pcap")), ) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 92907d4513..6c73fe606d 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -15,12 +15,9 @@ """3d navigation on Go2 with ray tracing and MLS planning""" -from datetime import datetime import os -from pathlib import Path from typing import Any -from dimos.constants import RECORDINGS_DIR from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.stream import In @@ -28,7 +25,7 @@ from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.memory.module import pose_setter_for +from dimos.memory.module import pose_setter_for, recording_dir from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower @@ -69,17 +66,6 @@ async def _odom_go2_pose(self, msg: PoseStamped) -> PoseStamped: _RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on") -def _recording_dir() -> Path: - now = datetime.now().astimezone() - stamp = ( - now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-" + now.strftime("%Z") - ) - return RECORDINGS_DIR / stamp - - -_RECORDING_DIR = _recording_dir() - - def _render_global_map(msg: Any) -> Any: return msg.to_rerun() @@ -178,16 +164,16 @@ def _static_robot_body(rr: Any) -> list[Any]: if _RECORD: unitree_go2_nav_3d = autoconnect( unitree_go2_nav_3d, - Go2Mid360Recorder.blueprint(db_path=str(_RECORDING_DIR / "mem2.db")).remappings( + Go2Mid360Recorder.blueprint().remappings( [ (Go2Mid360Recorder, "pointlio_lidar", "lidar"), (Go2Mid360Recorder, "pointlio_odometry", "odometry"), ] ), - ) + ).global_config(record=True) if _RECORD_PCAP: unitree_go2_nav_3d = autoconnect( unitree_go2_nav_3d, - Mid360PcapRecorder.blueprint(pcap_path=_RECORDING_DIR / "mid360.pcap"), + Mid360PcapRecorder.blueprint(pcap_path=recording_dir() / "mid360.pcap"), ) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index 308071f16c..a583539b37 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -13,19 +13,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from pathlib import Path from dimos.core.coordination.blueprints import autoconnect -from dimos.core.stream import In from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper from dimos.mapping.relocalization.module import RelocalizationModule from dimos.mapping.voxels.module import VoxelGridMapper -from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( WavefrontFrontierExplorer, @@ -49,31 +42,6 @@ ).global_config(n_workers=10, robot_model="unitree_go2") -class Go2MemoryConfig(RecorderConfig): - db_path: str | Path = "recording_go2.db" - - -class Go2Memory(Recorder): - color_image: In[Image] - lidar: In[PointCloud2] - odom: In[PoseStamped] - config: Go2MemoryConfig - - _last_odom_pose: Pose | None = None - - @pose_setter_for("odom") - async def _odom_pose(self, msg: PoseStamped) -> Pose | None: - self._last_odom_pose = msg - return self._last_odom_pose - - @pose_setter_for("lidar") - async def _lidar_pose(self, msg: PointCloud2) -> Pose | None: - # Yes, it doesn't make sense to register lidar at the odom pose because the - # go2 lidar is in the world frame, but map.py (for now) needs this. - # TODO: fix map.py to use a transform frame - return getattr(self, "_last_odom_pose", None) - - unitree_go2_markers = ( autoconnect( unitree_go2, @@ -91,15 +59,10 @@ async def _lidar_pose(self, msg: PointCloud2) -> Pose | None: ), } ) - .global_config(n_workers=11, robot_model="unitree_go2") + .global_config(n_workers=12, robot_model="unitree_go2") ) unitree_go2_relocalization = autoconnect( unitree_go2, RelocalizationModule.blueprint(), -).global_config(n_workers=11) - -unitree_go2_memory = autoconnect( - unitree_go2, - Go2Memory.blueprint(), ).global_config(n_workers=12) diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 018e04985e..0487c82f73 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -32,8 +32,8 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.resource import CompositeResource from dimos.core.stream import In, Out -from dimos.memory.replay import Replay, ReplayStream, resolve_db_path -from dimos.memory.store.sqlite import SqliteStore +from dimos.memory.cli.dataset import open_dataset +from dimos.memory.replay import Replay, ReplayStream from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -179,9 +179,7 @@ def __init__( # type: ignore[no-untyped-def] def replay(self) -> Replay: # One shared store + Replay so lidar/odom/video advance against the # same wall-clock anchor on subscribe. - store = self.register_disposable( - SqliteStore(path=str(resolve_db_path(self.dataset)), must_exist=True) - ) + store = self.register_disposable(open_dataset(self.dataset)) store.start() return store.replay(loop=self._loop, seek=self._seek, duration=self._duration) diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 8385bdf983..df045b5fab 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -35,6 +35,7 @@ from dimos.core.baked_host import baked_host from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.mapping.ray_tracing.module import RayTracingVoxelMap, RayTracingVoxelMapConfig from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC @@ -160,7 +161,13 @@ def _rerun_config(visual_override: dict[str, Any] | None = None) -> dict[str, An vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config()), GO2Zenoh.blueprint(mid360_mount=MID360_MOUNT), MovementManager.blueprint(), -).global_config(transport="zenoh", n_workers=4, robot_model="unitree_go2") + PointlioRecorder.blueprint().remappings( + [ + (PointlioRecorder, "pointlio_lidar", "lidar"), + (PointlioRecorder, "pointlio_odometry", "odometry"), + ] + ), +).global_config(transport="zenoh", n_workers=5, robot_model="unitree_go2") # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. diff --git a/docs/capabilities/navigation/relocalization.md b/docs/capabilities/navigation/relocalization.md index 0b431223a6..c411aa52d5 100644 --- a/docs/capabilities/navigation/relocalization.md +++ b/docs/capabilities/navigation/relocalization.md @@ -8,36 +8,36 @@ Relocalization lets a Go2 navigate on a previously built map instead of only on This guide takes four steps: -1. Record a walk-through with `unitree-go2-memory` +1. Record a walk-through with `unitree-go2` 2. Build the premap with `dimos map global {DB_NAME} --export` 3. Test relocalization in replay, no robot needed 4. Deploy on the live Go2 -Throughout this guide, `{DB_NAME}` is the stem of your recording, for example `recording_go2` for `recording_go2.db`. For `map_file`, pass the same stem and dimOS appends `.pc2.lcm` automatically. +Throughout this guide, `{DB_NAME}` is the path of your recording, for example `recordings/20260826-165100-unitree-go2/memory.mcap`. For `map_file`, pass the same stem and dimOS appends `.pc2.lcm` automatically. ## 1. Record a run Drive the Go2 through the space you want as your premap. Close loops when you can because PGO uses revisits to correct drift. ```bash -dimos --robot-ip {YOUR_ROBOT_IP} run unitree-go2-memory +dimos --record --robot-ip {YOUR_ROBOT_IP} run unitree-go2 ``` If `ROBOT_IP` is set in the environment or `.env`, you can omit `--robot-ip`: ```bash -dimos run unitree-go2-memory +dimos --record run unitree-go2 ``` -This writes `recording_go2.db` to the repo root (`DIMOS_PROJECT_ROOT`) and records `lidar`, `odom`, and `color_image` plus the live TF tree. The recorder stamps lidar frames with the latest odom pose so `dimos map global` can reconstruct poses later- see [`Go2Memory`](/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py). +This writes `recordings//memory.mcap` (e.g. `recordings/20260826-165100-unitree-go2/memory.mcap`, the same id as the log dir) under the checkout (`~/.local/state/dimos/recordings/` for an installed package) and records `lidar`, `odom`, and `color_image` plus the live TF tree. `--record-format sqlite` writes `memory.db` instead. The recorder stamps lidar frames with the latest odom pose so `dimos map global` can reconstruct poses later- see [`OdomRecorder`](/dimos/memory/module.py). ### Quick validation (optional) Before building a premap, inspect the recording: ```bash -dimos mem summary recording_go2 -dimos map replay recording_go2 --duration 60 +dimos mem summary {DB_NAME} +dimos map replay {DB_NAME} --duration 60 ``` `summary` prints stream names and time ranges. `replay` opens Rerun so you can confirm lidar and odometry look sane. @@ -47,7 +47,7 @@ dimos map replay recording_go2 --duration 60 Export a loop-closed global map as `.pc2.lcm`: ```bash -dimos map global recording_go2 --export +dimos map global {DB_NAME} --export ``` | Flag | Effect | @@ -66,7 +66,7 @@ dimos map global recording_go2 --export Examples: ```bash -dimos map global recording_go2 --export --no-gui +dimos map global {DB_NAME} --export --no-gui dimos map global ./recordings/office_walk.db --export dimos map global data/go2_hongkong_office.db --export ``` @@ -76,8 +76,8 @@ Sample log: ``` running PGO twopass map... Pass 1: 908 frames, 1 keyframes -exporting PGO twopass map to .../recording_go2.pc2.lcm... -wrote .../recording_go2.pc2.lcm +exporting PGO twopass map to .../{DB_NAME}.pc2.lcm... +wrote .../{DB_NAME}.pc2.lcm ``` Open the companion `{DB_NAME}.rrd` in Rerun to verify loop closure before deploying to hardware. @@ -87,8 +87,8 @@ Open the companion `{DB_NAME}.rrd` in Rerun to verify loop closure before deploy Test alignment without the robot. `unitree-go2-relocalization` is `unitree-go2` plus `RelocalizationModule`: ```bash -dimos --replay --replay-db recording_go2 run unitree-go2-relocalization \ - --map-file=recording_go2 +dimos --replay --replay-db {DB_NAME} run unitree-go2-relocalization \ + --map-file={DB_NAME} ``` `map_file` resolves `{DB_NAME}.pc2.lcm` with the same search order as above (cwd, then project root, then `data/`). @@ -96,7 +96,7 @@ dimos --replay --replay-db recording_go2 run unitree-go2-relocalization \ ### Reading the logs ``` -Relocalization module started: map_file='recording_go2' loaded_map.frame_id='map' +Relocalization module started: map_file='{DB_NAME}' loaded_map.frame_id='map' relocalize skipped: n_pts=37770 < MIN_LOCAL_POINTS=50000 relocalize rejected: fitness=0.433 < threshold=0.45 time_cost=8.1s n_pts=57385 relocalize: fitness=0.657 time_cost=3.0s n_pts=64703 reloc_t=[-0.007, -0.01, -0.102] TF 'world' -> 'map' published_t=[0.007, 0.009, 0.102] @@ -119,7 +119,7 @@ Run the replay test first. On hardware, use the same blueprint and `map_file`: ```bash dimos --robot-ip {YOUR_ROBOT_IP} run unitree-go2-relocalization \ - --map-file=recording_go2 + --map-file={DB_NAME} ``` Before sending navigation goals, walk through this checklist: @@ -160,7 +160,7 @@ Note that [`CostMapper`](/dimos/mapping/costmapper.py) builds the costmap from t | File | Format | Produced by | Consumed by | |------|--------|-------------|-------------| -| `{name}.db` | memory SQLite (`lidar`, `odom`, `color_image`, …) | `unitree-go2-memory` | `dimos map *`, `--replay-db` | +| `{name}.db` | memory SQLite (`lidar`, `odom`, `color_image`, …) | `unitree-go2` | `dimos map *`, `--replay-db` | | `{name}.pc2.lcm` | LCM-encoded `PointCloud2` premap | `dimos map global --export` | `RelocalizationModule` (`map_file`) | | `{name}.rrd` | Rerun recording (visual QA) | `dimos map global` | Rerun viewer | @@ -189,7 +189,7 @@ To accept all candidates for visualization only (not for production nav): ```bash dimos run unitree-go2-relocalization \ - --map-file=recording_go2 \ + --map-file={DB_NAME} \ --fitness-threshold=0.0 ``` diff --git a/docs/platforms/quadruped/go2/index.md b/docs/platforms/quadruped/go2/index.md index d093c6ffb5..10b7afe608 100644 --- a/docs/platforms/quadruped/go2/index.md +++ b/docs/platforms/quadruped/go2/index.md @@ -14,7 +14,7 @@ | `dimos run unitree-go2-agentic-ollama` | Agent with local Ollama models | | `dimos run unitree-go2-spatial` | Navigation + spatial memory | | `dimos run unitree-go2-detection` | Navigation + object detection | -| `dimos run unitree-go2-memory` | Navigation + record `lidar`/`odom`/`color_image` to `.db` | +| `dimos run unitree-go2` | Navigation; `--record` writes `lidar`/`odom`/`color_image` to `recordings//memory.mcap` | | `dimos run unitree-go2-relocalization` | Navigation + align live scans to a saved `.pc2.lcm` premap | ## Deep Dive diff --git a/docs/quickstart.md b/docs/quickstart.md index 0c5967542f..d6164c62b2 100644 --- a/docs/quickstart.md +++ b/docs/quickstart.md @@ -120,7 +120,7 @@ Manage the background run with `dimos status`, `dimos log -f`, and `dimos stop`. | Command | What it does | | --- | --- | | `dimos --replay run unitree-go2` | Quadruped navigation replay with SLAM, costmap, and A-star planning | -| `dimos --replay --replay-db go2_bigoffice run unitree-go2-memory` | Quadruped spatial memory replay | +| `dimos --replay --replay-db go2_bigoffice run unitree-go2` | Quadruped spatial memory replay | | `dimos --simulation run unitree-go2-agentic` | Quadruped LLM agent plus MCP server in simulation (needs `OPENAI_API_KEY`) | | `dimos --simulation run unitree-g1-sim` | Humanoid in MuJoCo simulation | | `dimos --replay run drone-basic` | Drone video and telemetry replay | diff --git a/pyproject.toml b/pyproject.toml index f336bab1b8..b5c0d26d47 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -105,6 +105,7 @@ dependencies = [ "PyTurboJPEG==1.8.2", "imagecodecs>=2024.6.1", # JPEG-XL for CompressedImage "jsonlines>=4,<5", + "mcap>=1.2.0", # default recording format # Core "numpy>=1.26.4", "scipy>=1.15.1", @@ -278,7 +279,6 @@ unitree-dds = [ "dimos[unitree]", "unitree-sdk2py-dimos>=1.0.2", "cyclonedds>=0.10.5", - "mcap>=1.2.0", # decode Go2 DDS mcap recordings (go2 dds store) ] manipulation = [ @@ -505,10 +505,6 @@ lint = [ tests-self-hosted = [ {include-group = "tests"}, "dimos[agents,perception,manipulation,sim,unitree,misc]", - # go2 dds store tests decode mcap (pure-Python). The full `unitree-dds` extra - # pulls `cyclonedds`, whose wheel needs a CycloneDDS C lib the ros-dev image - # doesn't expose to the build — and these tests never open a live DDS link. - "mcap>=1.2.0", # Needed to compile the in-tree extensions. "pybind11>=2.12", ] diff --git a/uv.lock b/uv.lock index 068aa4da09..6d2b1fbf86 100644 --- a/uv.lock +++ b/uv.lock @@ -1726,6 +1726,7 @@ dependencies = [ { name = "lazy-loader" }, { name = "llvmlite" }, { name = "lz4" }, + { name = "mcap" }, { name = "numba" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, @@ -2022,7 +2023,6 @@ unitree-dds = [ { name = "langchain-ollama" }, { name = "langchain-openai" }, { name = "lap" }, - { name = "mcap" }, { name = "moondream" }, { name = "ollama" }, { name = "omegaconf" }, @@ -2213,7 +2213,6 @@ tests-self-hosted = [ { name = "langchain-openai" }, { name = "lap" }, { name = "maturin" }, - { name = "mcap" }, { name = "md-babel-py" }, { name = "moondream" }, { name = "mujoco" }, @@ -2314,7 +2313,7 @@ requires-dist = [ { name = "manifold3d", marker = "extra == 'apriltag'", specifier = ">=2.5.0" }, { name = "matplotlib", marker = "extra == 'graspgenx'", specifier = ">=3.7.1" }, { name = "matplotlib", marker = "extra == 'manipulation'", specifier = ">=3.7.1" }, - { name = "mcap", marker = "extra == 'unitree-dds'", specifier = ">=1.2.0" }, + { name = "mcap", specifier = ">=1.2.0" }, { name = "moondream", marker = "extra == 'perception'" }, { name = "mujoco", marker = "extra == 'sim'", specifier = ">=3.3.4" }, { name = "numba", specifier = ">=0.60.0" }, @@ -2550,7 +2549,6 @@ tests-self-hosted = [ { name = "langchain-openai", specifier = ">=1,<2" }, { name = "lap", specifier = ">=0.5.12" }, { name = "maturin", specifier = ">=1.7" }, - { name = "mcap", specifier = ">=1.2.0" }, { name = "md-babel-py", specifier = ">=1.4.0" }, { name = "moondream" }, { name = "mujoco", specifier = ">=3.3.4" }, @@ -4382,6 +4380,12 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/7b/91/984aca2ec129e2757d1e4e3c81c3fcda9d0f85b74670a094cc443d9ee949/joblib-1.5.3-py3-none-any.whl", hash = "sha256:5fc3c5039fc5ca8c0276333a188bbd59d6b7ab37fe6632daa76bc7f9ec18e713", size = 309071, upload-time = "2025-12-15T08:41:44.973Z" }, ] +[[package]] +name = "jsmin" +version = "3.0.1" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/5e/73/e01e4c5e11ad0494f4407a3f623ad4d87714909f50b17a06ed121034ff6e/jsmin-3.0.1.tar.gz", hash = "sha256:c0959a121ef94542e807a674142606f7e90214a2b3d1eb17300244bbb5cc2bfc", size = 13925, upload-time = "2022-01-16T20:35:59.13Z" } + [[package]] name = "jsonlines" version = "4.0.0" @@ -4394,12 +4398,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/f8/62/d9ba6323b9202dd2fe166beab8a86d29465c41a0288cbe229fac60c1ab8d/jsonlines-4.0.0-py3-none-any.whl", hash = "sha256:185b334ff2ca5a91362993f42e83588a360cf95ce4b71a73548502bda52a7c55", size = 8701, upload-time = "2023-09-01T12:34:42.563Z" }, ] -[[package]] -name = "jsmin" -version = "3.0.1" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/5e/73/e01e4c5e11ad0494f4407a3f623ad4d87714909f50b17a06ed121034ff6e/jsmin-3.0.1.tar.gz", hash = "sha256:c0959a121ef94542e807a674142606f7e90214a2b3d1eb17300244bbb5cc2bfc", size = 13925, upload-time = "2022-01-16T20:35:59.13Z" } - [[package]] name = "jsonpatch" version = "1.33"