Nav pt5: Dynamic Global Map with Loop Closure Voxel Transform

.github/workflows/ci.yml

@@ -64,7 +64,7 @@ jobs:
         run: ./bin/run-doc-codeblocks --ci --no-cache
 
   tests:
-    timeout-minutes: 20
+    timeout-minutes: 25
     strategy:
       matrix:
         pyver: ['3.10', '3.11', '3.12', '3.13', '3.14']

.pre-commit-config.yaml

@@ -117,7 +117,7 @@ repos:
             git ls-files "*Cargo.toml" | while read -r m; do
               cargo locate-project --manifest-path "$m" --workspace --message-format plain
             done | sort -u | while read -r root; do
-              cargo fmt --manifest-path "$root" --all --check
+              cargo fmt --manifest-path "$root" --all
             done
           '
         language: system

dimos/conftest.py

@@ -17,6 +17,7 @@
 import hashlib
 import os
 import platform
+import shutil
 import tempfile
 import threading
 
@@ -74,6 +75,10 @@ def _has_ros() -> bool:
         return False
 
 
+def _has_nix() -> bool:
+    return shutil.which("nix") is not None
+
+
 def _is_macos() -> bool:
     return platform.system() == "Darwin"
 
@@ -90,6 +95,7 @@ def pytest_configure(config):
     config.addinivalue_line("markers", "skipif_no_openai: skip when OPENAI_API_KEY is not set")
     config.addinivalue_line("markers", "skipif_no_alibaba: skip when ALIBABA_API_KEY is not set")
     config.addinivalue_line("markers", "skipif_no_ros: skip when ROS dependencies are not present")
+    config.addinivalue_line("markers", "skipif_no_nix: skip when the `nix` binary is not on PATH")
     config.addinivalue_line("markers", "skipif_macos_bug: skip known-buggy tests on macOS")
     config.addinivalue_line("markers", "skipif_macos: skip tests not intended to run on macOS")
 
@@ -122,6 +128,7 @@ def pytest_collection_modifyitems(config, items):
         "skipif_no_openai": (not os.getenv("OPENAI_API_KEY"), "OPENAI_API_KEY not set"),
         "skipif_no_alibaba": (not os.getenv("ALIBABA_API_KEY"), "ALIBABA_API_KEY not set"),
         "skipif_no_ros": (not _has_ros(), "ROS dependencies are not present"),
+        "skipif_no_nix": (not _has_nix(), "nix binary is not on PATH"),
         "skipif_macos_bug": (_is_macos(), "Some tests are buggy on Mac OS"),
         "skipif_macos": (_is_macos(), "Not intended to run on macOS"),
     }

dimos/control/blueprints/mobile.py

@@ -198,8 +198,7 @@ def _flowbase_twist_base(
     )
     .remappings(
         [
-            (FastLio2, "lidar", "registered_scan"),
-            (FastLio2, "global_map", "global_map_fastlio"),
+            (FastLio2, "global_map", "_global_map_fastlio"),
             # SimplePlanner / FarPlanner owns way_point — disconnect MovementManager's
             # redundant pass-through copy (matches unitree-g1-nav-onboard).
             (MovementManager, "way_point", "_mgr_way_point_unused"),

dimos/core/coordination/module_coordinator.py

@@ -201,6 +201,11 @@ def start_all_modules(self) -> None:
 
         safe_thread_map(modules, lambda m: m.start())
 
+        # All starts done — fire the system-ready barrier so producers that
+        # were awaiting it can begin publishing. See ModuleBase.wait_for_system_ready.
+        for module in modules:
+            module.on_system_ready()
+
         self._send_on_system_modules()
 
     def _resolve_class(self, cls: type[ModuleBase]) -> type[ModuleBase]:

dimos/core/module.py

@@ -123,12 +123,17 @@ class ModuleBase(Configurable, CompositeResource):
     _main_gen: AsyncGenerator[None, None] | None = None
     _tools: dict[str, Any]
     _tools_lock: threading.Lock
+    # Set by ModuleCoordinator after every module's start() has returned.
+    # Producers that publish into shared streams should await this before
+    # emitting their first message — see ModuleBase.wait_for_system_ready.
+    _system_ready: threading.Event
 
     def __init__(self, config_args: dict[str, Any]) -> None:
         super().__init__(**config_args)
         self._module_closed_lock = threading.Lock()
         self._tools = {}
         self._tools_lock = threading.Lock()
+        self._system_ready = threading.Event()
         self._loop, self._loop_thread = get_loop()
         try:
             self.rpc = self.config.rpc_transport(  # type: ignore[call-arg]
@@ -161,6 +166,23 @@ def build(self) -> None:
         Default is a no-op — override in subclasses that need a build step.
         """
 
+    async def wait_for_system_ready(self) -> None:
+        """Block until every module in the coordinator has finished start().
+
+        Producers (replay modules, sensor drivers, test playback) that emit
+        into shared streams should await this before their first publish, so
+        downstream subscribers (esp. native subprocesses) don't race-miss
+        early messages.
+        """
+        if self._system_ready.is_set():
+            return
+        await asyncio.to_thread(self._system_ready.wait)
+
+    @rpc
+    def on_system_ready(self) -> None:
+        """Coordinator hook: called once every module's start() has returned."""
+        self._system_ready.set()
+
     @rpc
     def start(self) -> None:
         self._start_main()
@@ -232,6 +254,7 @@ def __getstate__(self):  # type: ignore[no-untyped-def]
         state.pop("_main_gen", None)
         state.pop("_tools", None)
         state.pop("_tools_lock", None)
+        state.pop("_system_ready", None)
         return state
 
     def __setstate__(self, state) -> None:  # type: ignore[no-untyped-def]
@@ -246,12 +269,16 @@ def __setstate__(self, state) -> None:  # type: ignore[no-untyped-def]
         self._main_gen = None
         self._tools = {}
         self._tools_lock = threading.Lock()
+        self._system_ready = threading.Event()
+
+    _tf_lock: threading.Lock = threading.Lock()
 
     @property
     def tf(self):  # type: ignore[no-untyped-def]
         if self._tf is None:
-            # self._tf = self.config.tf_transport()
-            self._tf = LCMTF()
+            with self._tf_lock:
+                if self._tf is None:
+                    self._tf = LCMTF()
         return self._tf
 
     @tf.setter

dimos/core/native_module.py

@@ -83,6 +83,11 @@ def _set_process_to_die_when_parent_dies() -> None:
 
 logger = setup_logger()
 
+# Native binaries emit this marker on stderr after their LCM subscribes are
+# live. NativeModule.start() blocks until it sees it, ensuring upstream
+# publishers don't race ahead and drop messages.
+_READY_MARKER = "[DIMOS_NATIVE_READY]"
+
 
 class LogFormat(enum.Enum):
     TEXT = "text"
@@ -100,6 +105,11 @@ class NativeModuleConfig(ModuleConfig):
     shutdown_timeout: float = DEFAULT_THREAD_JOIN_TIMEOUT
     log_format: LogFormat = LogFormat.TEXT
     auto_build: bool = False
+    # How long start() blocks waiting for the subprocess to emit
+    # "[DIMOS_NATIVE_READY]" on stderr. Default 0 = no wait (legacy binaries
+    # that don't emit the marker). Subclasses whose binaries emit the marker
+    # should bump this to ~10s. See NativeModule.start.
+    ready_timeout_sec: float = 0.0
 
     # New version of Native Modules read json configs from stdin
     # Enable this to read from stdin instead of cli args
@@ -108,18 +118,38 @@ class NativeModuleConfig(ModuleConfig):
     cli_exclude: frozenset[str] = frozenset()
     cli_name_override: dict[str, str] = Field(default_factory=dict)
 
+    def _native_ignore_fields(self) -> set[str]:
+        """Inherited NativeModuleConfig fields *not* redeclared in any subclass.
+
+        A subclass that redeclares an inherited field (e.g. ``frame_id``) is
+        signalling it wants that field exposed — usually as a CLI arg or
+        stdin config entry — so we don't filter it out. Fields declared
+        directly on NativeModuleConfig (``executable``, ``cwd``, ...) configure
+        the wrapper itself and stay ignored even when redeclared to override
+        defaults.
+        """
+        ignore = set(NativeModuleConfig.model_fields)
+        wrapper_only = set(NativeModuleConfig.__annotations__)
+        for klass in self.__class__.__mro__:
+            if klass is NativeModuleConfig:
+                break
+            for field in getattr(klass, "__annotations__", {}):
+                if field not in wrapper_only:
+                    ignore.discard(field)
+        return ignore
+
     def to_config_dict(self) -> dict[str, Any]:
         """
         Return module-specific config fields as a plain dict (for stdin JSON).
         """
-        ignore_fields = set(NativeModuleConfig.model_fields)
+        ignore_fields = self._native_ignore_fields()
         return {
             k: v for k, v in self.model_dump().items() if k not in ignore_fields and v is not None
         }
 
     def to_cli_args(self) -> list[str]:
         """Convert subclass config fields to CLI args (--name value)."""
-        ignore_fields = {f for f in NativeModuleConfig.model_fields if f != "frame_id"}
+        ignore_fields = self._native_ignore_fields()
         args: list[str] = []
         for f in self.__class__.model_fields:
             if f in ignore_fields:
@@ -163,6 +193,7 @@ class NativeModule(Module):
     _watchdog: threading.Thread | None = None
     _stopping: bool = False
     _stop_lock: threading.Lock
+    _ready_event: threading.Event
 
     @functools.cached_property
     def _module_label(self) -> str:
@@ -172,13 +203,19 @@ def _module_label(self) -> str:
     def __init__(self, **kwargs: Any) -> None:
         super().__init__(**kwargs)
         self._stop_lock = threading.Lock()
+        self._ready_event = threading.Event()
 
         if self.config.cwd is not None and not Path(self.config.cwd).is_absolute():
             base_dir = Path(inspect.getfile(type(self))).resolve().parent
             self.config.cwd = str(base_dir / self.config.cwd)
         if not Path(self.config.executable).is_absolute() and self.config.cwd is not None:
             self.config.executable = str(Path(self.config.cwd) / self.config.executable)
 
+    @rpc
+    def build(self) -> None:
+        super().build()
+        self._maybe_build()
+
     @rpc
     def start(self) -> None:
         super().start()
@@ -190,8 +227,6 @@ def start(self) -> None:
             )
             return
 
-        self._maybe_build()
-
         topics = self._collect_topics()
 
         cmd = [self.config.executable]
@@ -234,16 +269,43 @@ def start(self) -> None:
             pid=self._process.pid,
         )
 
+        self._ready_event.clear()
         watchdog = threading.Thread(
             target=self._watch_process,
             daemon=True,
             name=f"native-watchdog-{self._module_label}",
         )
+        # Capture proc before launching the watchdog — if the subprocess dies,
+        # the watchdog calls stop() which nulls out self._process and this loop
+        # would crash otherwise.
+        proc = self._process
         with self._stop_lock:
             self._stopping = False
             self._watchdog = watchdog
         watchdog.start()
 
+        # Block until the subprocess emits [DIMOS_NATIVE_READY] on stderr, or
+        # the timeout fires. The watchdog's stderr reader sets _ready_event
+        # when it sees the marker. If the subprocess exits before that, drop
+        # out and let the watchdog handle the cleanup — start() always
+        # returns; failures surface later via the watchdog's stop() path.
+        if self.config.ready_timeout_sec > 0:
+            deadline = time.monotonic() + self.config.ready_timeout_sec
+            while not self._ready_event.is_set():
+                if proc.poll() is not None:
+                    break
+                remaining = deadline - time.monotonic()
+                if remaining <= 0:
+                    logger.warning(
+                        "Native process did not emit [DIMOS_NATIVE_READY] within "
+                        f"{self.config.ready_timeout_sec}s — proceeding anyway. "
+                        "Upstream publishers may race the subprocess's LCM subscribes.",
+                        module=self._module_label,
+                        pid=proc.pid,
+                    )
+                    break
+                self._ready_event.wait(timeout=min(remaining, 0.5))
+
     @rpc
     def stop(self) -> None:
         # Capture refs under lock, but signal/wait/join outside it to avoid
@@ -346,6 +408,9 @@ def _read_log_stream(
             line = raw.decode("utf-8", errors="replace").rstrip()
             if not line:
                 continue
+            if _READY_MARKER in line:
+                self._ready_event.set()
+                continue
             if self.config.log_format == LogFormat.JSON:
                 try:
                     data = json.loads(line)

dimos/core/test_core.py

@@ -77,7 +77,7 @@ def test_classmethods() -> None:
     # Check that we have the expected RPC methods
     assert "navigate_to" in class_rpcs, "navigate_to should be in rpcs"
     assert "start" in class_rpcs, "start should be in rpcs"
-    assert len(class_rpcs) == 7
+    assert len(class_rpcs) == 8
 
     # Check that the values are callable
     assert callable(class_rpcs["navigate_to"]), "navigate_to should be callable"

dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp

@@ -510,6 +510,11 @@ int main(int argc, char** argv) {
 
     if (debug) printf("[fastlio2] SDK started, waiting for device...\n");
 
+    // NativeModule.start() in Python reads stderr for this marker and only
+    // returns once it sees it.
+    fprintf(stderr, "[DIMOS_NATIVE_READY]\n");
+    fflush(stderr);
+
     // Main loop
     auto frame_interval = std::chrono::microseconds(
         static_cast<int64_t>(1e6 / g_frequency));