Barebones Loop Closure

changes.md

@@ -0,0 +1,27 @@
+# PR #1645 (nav1 / loop closure) — Paul Review Fixes
+
+## Commits (local, not pushed)
+
+### 1. `357a2b178` — Hoist scipy Rotation import to top-level
+- Was imported inline 3 times in per-keyframe hot paths
+- **Revert:** `git revert 357a2b178`
+
+### 2. `a89b471f3` — Replace assert with explicit None check
+- `assert` stripped in `python -O` — unsafe for production robot code
+- **Revert:** `git revert a89b471f3`
+
+### 3. `d0ffec6cf` — Fix timestamp falsy check
+- `msg.ts=0.0` (epoch) is valid but falsy → incorrectly used `time.time()`
+- Now uses `is not None`
+- **Revert:** `git revert d0ffec6cf`
+
+### 4. `e6837e425` — Vectorize column carve with np.isin
+- Python loop over 50k+ points replaced with vectorized `np.isin`
+- **Revert:** `git revert e6837e425`
+
+## Not addressed (need Jeff's input)
+- PGO file is 554 lines doing 4 things — split into algorithm.py + module.py?
+- ICP + GTSAM optimization on subscriber thread — move to worker queue?
+- Hardcoded camera offsets in PGO TF publishing — should be in config or camera module?
+- Second GO2Connection in smartnav — intentional for publish_tf=False?
+- Loop closure picks first KD-tree candidate, not best

dimos/core/native_module.py

@@ -40,7 +40,6 @@ class MyCppModule(NativeModule):
 
 from __future__ import annotations
 
-import collections
 import enum
 import inspect
 import json
@@ -56,6 +55,7 @@ class MyCppModule(NativeModule):
 
 from dimos.core.core import rpc
 from dimos.core.module import Module, ModuleConfig
+from dimos.utils.change_detect import did_change
 from dimos.utils.logging_config import setup_logger
 
 if sys.version_info < (3, 13):
@@ -81,9 +81,10 @@ class NativeModuleConfig(ModuleConfig):
     extra_env: dict[str, str] = Field(default_factory=dict)
     shutdown_timeout: float = 10.0
     log_format: LogFormat = LogFormat.TEXT
+    rebuild_on_change: list[str] | None = None
 
     # Override in subclasses to exclude fields from CLI arg generation
-    cli_exclude: frozenset[str] = frozenset()
+    cli_exclude: frozenset[str] = frozenset({"rebuild_on_change"})
 
     def to_cli_args(self) -> list[str]:
         """Auto-convert subclass config fields to CLI args.
@@ -132,11 +133,9 @@ class NativeModule(Module[_NativeConfig]):
     _process: subprocess.Popen[bytes] | None = None
     _watchdog: threading.Thread | None = None
     _stopping: bool = False
-    _last_stderr_lines: collections.deque[str]
 
     def __init__(self, **kwargs: Any) -> None:
         super().__init__(**kwargs)
-        self._last_stderr_lines = collections.deque(maxlen=50)
         self._resolve_paths()
 
     @rpc
@@ -158,25 +157,15 @@ def start(self) -> None:
         env = {**os.environ, **self.config.extra_env}
         cwd = self.config.cwd or str(Path(self.config.executable).resolve().parent)
 
-        module_name = type(self).__name__
-        logger.info(
-            f"Starting native process: {module_name}",
-            module=module_name,
-            cmd=" ".join(cmd),
-            cwd=cwd,
-        )
+        logger.info("Starting native process", cmd=" ".join(cmd), cwd=cwd)
         self._process = subprocess.Popen(
             cmd,
             env=env,
             cwd=cwd,
             stdout=subprocess.PIPE,
             stderr=subprocess.PIPE,
         )
-        logger.info(
-            f"Native process started: {module_name}",
-            module=module_name,
-            pid=self._process.pid,
-        )
+        logger.info("Native process started", pid=self._process.pid)
 
         self._stopping = False
         self._watchdog = threading.Thread(target=self._watch_process, daemon=True)
@@ -215,20 +204,10 @@ def _watch_process(self) -> None:
 
         if self._stopping:
             return
-
-        module_name = type(self).__name__
-        exe_name = Path(self.config.executable).name if self.config.executable else "unknown"
-
-        # Use buffered stderr lines from the reader thread for the crash report.
-        last_stderr = "\n".join(self._last_stderr_lines)
-
         logger.error(
-            f"Native process crashed: {module_name} ({exe_name})",
-            module=module_name,
-            executable=exe_name,
+            "Native process died unexpectedly",
             pid=self._process.pid,
             returncode=rc,
-            last_stderr=last_stderr[:500] if last_stderr else None,
         )
         self.stop()
 
@@ -242,13 +221,10 @@ def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None:
         if stream is None:
             return
         log_fn = getattr(logger, level)
-        is_stderr = level == "warning"
         for raw in stream:
             line = raw.decode("utf-8", errors="replace").rstrip()
             if not line:
                 continue
-            if is_stderr:
-                self._last_stderr_lines.append(line)
             if self.config.log_format == LogFormat.JSON:
                 try:
                     data = json.loads(line)
@@ -270,17 +246,33 @@ def _resolve_paths(self) -> None:
             self.config.executable = str(Path(self.config.cwd) / self.config.executable)
 
     def _maybe_build(self) -> None:
-        """Run ``build_command`` if the executable does not exist."""
+        """Run ``build_command`` if the executable does not exist or sources changed."""
         exe = Path(self.config.executable)
-        if exe.exists():
+
+        # Check if rebuild needed due to source changes
+        needs_rebuild = False
+        if self.config.rebuild_on_change and exe.exists():
+            if did_change(
+                self._build_cache_name(), self.config.rebuild_on_change, cwd=self.config.cwd
+            ):
+                logger.info("Source files changed, triggering rebuild", executable=str(exe))
+                needs_rebuild = True
+
+        if exe.exists() and not needs_rebuild:
             return
+
         if self.config.build_command is None:
             raise FileNotFoundError(
                 f"Executable not found: {exe}. "
                 "Set build_command in config to auto-build, or build it manually."
             )
+
+        # Don't unlink the exe before rebuilding — the build command is
+        # responsible for replacing it.  For nix builds the exe lives inside
+        # a read-only store; `nix build -o` atomically swaps the output
+        # symlink without touching store contents.
         logger.info(
-            "Executable not found, running build",
+            "Rebuilding" if needs_rebuild else "Executable not found, building",
             executable=str(exe),
             build_command=self.config.build_command,
         )
@@ -300,18 +292,18 @@ def _maybe_build(self) -> None:
             if line.strip():
                 logger.warning(line)
         if proc.returncode != 0:
-            stderr_tail = stderr.decode("utf-8", errors="replace").strip()[-1000:]
             raise RuntimeError(
-                f"Build command failed (exit {proc.returncode}): {self.config.build_command}\n"
-                f"stderr: {stderr_tail}"
+                f"Build command failed (exit {proc.returncode}): {self.config.build_command}"
             )
         if not exe.exists():
             raise FileNotFoundError(
-                f"Build command succeeded but executable still not found: {exe}\n"
-                f"Build output may have been written to a different path. "
-                f"Check that build_command produces the executable at the expected location."
+                f"Build command succeeded but executable still not found: {exe}"
             )
 
+        # Update the change cache so next check is clean
+        if self.config.rebuild_on_change:
+            did_change(self._build_cache_name(), self.config.rebuild_on_change, cwd=self.config.cwd)
+
     def _collect_topics(self) -> dict[str, str]:
         """Extract LCM topic strings from blueprint-assigned stream transports."""
         topics: dict[str, str] = {}

dimos/core/test_native_rebuild.py

@@ -0,0 +1,140 @@
+# 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.
+
+"""Tests for NativeModule rebuild-on-change integration."""
+
+from __future__ import annotations
+
+from pathlib import Path
+import stat
+
+import pytest
+
+from dimos.core.native_module import NativeModule, NativeModuleConfig
+from dimos.utils.change_detect import PathEntry
+
+
+@pytest.fixture(autouse=True)
+def _use_tmp_cache(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None:
+    """Redirect the change-detection cache to a temp dir for every test."""
+    monkeypatch.setattr(
+        "dimos.utils.change_detect._get_cache_dir",
+        lambda: tmp_path / "cache",
+    )
+
+
+@pytest.fixture()
+def build_env(tmp_path: Path) -> dict[str, Path]:
+    """Set up a temp directory with a source file, executable path, and marker path."""
+    src = tmp_path / "src"
+    src.mkdir()
+    (src / "main.c").write_text("int main() { return 0; }")
+
+    exe = tmp_path / "mybin"
+    marker = tmp_path / "build_ran.marker"
+
+    # Build script: create the executable and a marker file
+    build_script = tmp_path / "build.sh"
+    build_script.write_text(f"#!/bin/sh\ntouch {exe}\nchmod +x {exe}\ntouch {marker}\n")
+    build_script.chmod(build_script.stat().st_mode | stat.S_IEXEC)
+
+    return {"src": src, "exe": exe, "marker": marker, "build_script": build_script}
+
+
+class _RebuildConfig(NativeModuleConfig):
+    executable: str = ""
+    rebuild_on_change: list[PathEntry] | None = None
+
+
+class _RebuildModule(NativeModule[_RebuildConfig]):
+    default_config = _RebuildConfig
+
+
+def _make_module(build_env: dict[str, Path]) -> _RebuildModule:
+    """Create a _RebuildModule pointing at the temp build env."""
+    return _RebuildModule(
+        executable=str(build_env["exe"]),
+        build_command=f"sh {build_env['build_script']}",
+        rebuild_on_change=[str(build_env["src"])],
+        cwd=str(build_env["src"]),
+    )
+
+
+def test_rebuild_on_change_triggers_build(build_env: dict[str, Path]) -> None:
+    """When source files change, the build_command should re-run."""
+    mod = _make_module(build_env)
+    try:
+        exe = build_env["exe"]
+        marker = build_env["marker"]
+
+        # First build: exe doesn't exist → build runs
+        mod._maybe_build()
+        assert exe.exists()
+        assert marker.exists()
+        marker.unlink()
+
+        # No change → build should NOT run
+        mod._maybe_build()
+        assert not marker.exists()
+
+        # Modify source → build SHOULD run
+        (build_env["src"] / "main.c").write_text("int main() { return 1; }")
+        mod._maybe_build()
+        assert marker.exists(), "Build should have re-run after source change"
+    finally:
+        mod.stop()
+
+
+def test_no_change_skips_rebuild(build_env: dict[str, Path]) -> None:
+    """When sources haven't changed, build_command must not run again."""
+    mod = _make_module(build_env)
+    try:
+        marker = build_env["marker"]
+
+        # Initial build
+        mod._maybe_build()
+        assert marker.exists()
+        marker.unlink()
+
+        # Second call — nothing changed
+        mod._maybe_build()
+        assert not marker.exists(), "Build should have been skipped (no source changes)"
+    finally:
+        mod.stop()
+
+
+def test_rebuild_on_change_none_skips_check(build_env: dict[str, Path]) -> None:
+    """When rebuild_on_change is None, no change detection happens at all."""
+    exe = build_env["exe"]
+    marker = build_env["marker"]
+
+    mod = _RebuildModule(
+        executable=str(exe),
+        build_command=f"sh {build_env['build_script']}",
+        rebuild_on_change=None,
+        cwd=str(build_env["src"]),
+    )
+    try:
+        # Initial build
+        mod._maybe_build()
+        assert exe.exists()
+        assert marker.exists()
+        marker.unlink()
+
+        # Modify source — but rebuild_on_change is None, so no rebuild
+        (build_env["src"] / "main.c").write_text("int main() { return 1; }")
+        mod._maybe_build()
+        assert not marker.exists(), "Should not rebuild when rebuild_on_change is None"
+    finally:
+        mod.stop()