Nav pt4: Fix tranform frames

.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']

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/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]
@@ -232,6 +237,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 +252,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
@@ -163,6 +173,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 +183,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 +207,6 @@ def start(self) -> None:
             )
             return
 
-        self._maybe_build()
-
         topics = self._collect_topics()
 
         cmd = [self.config.executable]
@@ -234,16 +249,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 +388,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));

dimos/hardware/sensors/lidar/fastlio2/module.py

@@ -61,7 +61,6 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM
 from dimos.spec import mapping, perception
 from dimos.utils.generic import get_local_ips
 from dimos.utils.logging_config import setup_logger
@@ -74,6 +73,8 @@ class FastLio2Config(NativeModuleConfig):
     cwd: str | None = "cpp"
     executable: str = "result/bin/fastlio2_native"
     build_command: str | None = "nix build .#fastlio2_native"
+    # The fastlio2 binary emits [DIMOS_NATIVE_READY] after Livox SDK is up.
+    ready_timeout_sec: float = 15.0
     # Livox SDK hardware config
     host_ip: str = "192.168.1.5"
     lidar_ip: str = "192.168.1.155"
@@ -83,11 +84,9 @@ class FastLio2Config(NativeModuleConfig):
     # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init.
     mount: Pose = Pose()
 
-    # Frame IDs for output messages.  "odom" reflects that FastLio2 provides
-    # locally-smooth, continuous odometry (no loop-closure jumps).  PGO
-    # publishes the map→odom correction via TF.
-    frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
+    frame_id: str = "start_point"
+    child_frame_id: str = "current_point"
+    sensor_frame: str = "mid360_link"
 
     # FAST-LIO internal processing rates
     msr_freq: float = 50.0
@@ -169,10 +168,11 @@ def start(self) -> None:
         )
 
     def _on_odom_for_tf(self, msg: Odometry) -> None:
+        ts = msg.ts or time.time()
         self.tf.publish(
             Transform(
-                frame_id=FRAME_ODOM,
-                child_frame_id=FRAME_BODY,
+                frame_id=self.config.frame_id,
+                child_frame_id=self.config.child_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,
@@ -184,7 +184,23 @@ def _on_odom_for_tf(self, msg: Odometry) -> None:
                     msg.pose.orientation.z,
                     msg.pose.orientation.w,
                 ),
-                ts=msg.ts or time.time(),
+                ts=ts,
+            )
+        )
+        # Static sensor mount
+        mount = self.config.mount
+        self.tf.publish(
+            Transform(
+                frame_id=self.config.child_frame_id,
+                child_frame_id=self.config.sensor_frame,
+                translation=Vector3(mount.x, mount.y, mount.z),
+                rotation=Quaternion(
+                    mount.orientation.x,
+                    mount.orientation.y,
+                    mount.orientation.z,
+                    mount.orientation.w,
+                ),
+                ts=ts,
             )
         )
 

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

@@ -297,6 +297,11 @@ int main(int argc, char** argv) {
 
     printf("[mid360] 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: periodically emit accumulated point clouds
     auto frame_interval = std::chrono::microseconds(
         static_cast<int64_t>(1e6 / g_frequency));

dimos/hardware/sensors/lidar/livox/module.py

@@ -55,6 +55,8 @@ class Mid360Config(NativeModuleConfig):
     cwd: str | None = "cpp"
     executable: str = "result/bin/mid360_native"
     build_command: str | None = "nix build .#mid360_native"
+    # The mid360 binary emits [DIMOS_NATIVE_READY] after Livox SDK is up.
+    ready_timeout_sec: float = 15.0
     host_ip: str = "192.168.1.5"
     lidar_ip: str = "192.168.1.155"
     frequency: float = 10.0