Nav pt4: Fix tranform frames

bin/ci-check

@@ -0,0 +1 @@
+/home/dimensional/dimos/.ignore.enhance/items/bin/ci-check

dimos/core/module.py

@@ -247,11 +247,14 @@ def __setstate__(self, state) -> None:  # type: ignore[no-untyped-def]
         self._tools = {}
         self._tools_lock = threading.Lock()
 
+    _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/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
@@ -83,11 +82,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 +166,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 +182,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/msgs/nav_msgs/LineSegments3D.py

@@ -23,10 +23,23 @@
 import time
 from typing import TYPE_CHECKING, BinaryIO
 
+from dimos_lcm.geometry_msgs import (
+    Point as LCMPoint,
+    Pose as LCMPose,
+    PoseStamped as LCMPoseStamped,
+    Quaternion as LCMQuaternion,
+)
 from dimos_lcm.nav_msgs import Path as LCMPath
+from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime
 
 from dimos.types.timestamped import Timestamped
 
+
+def _sec_nsec(ts: float) -> list[int]:
+    s = int(ts)
+    return [s, int((ts - s) * 1_000_000_000)]
+
+
 if TYPE_CHECKING:
     from rerun._baseclasses import Archetype
 
@@ -57,7 +70,33 @@ def __init__(
         self._traversability = traversability or [1.0] * len(self._segments)
 
     def lcm_encode(self) -> bytes:
-        raise NotImplementedError("Encoded on C++ side")
+        lcm_msg = LCMPath()
+        lcm_msg.poses = []
+
+        header_sec_nsec = _sec_nsec(self.ts)
+        for idx, (p1, p2) in enumerate(self._segments):
+            trav = self._traversability[idx] if idx < len(self._traversability) else 1.0
+            for endpoint, w_field in ((p1, trav), (p2, 0.0)):
+                pose = LCMPoseStamped()
+                pose.header = LCMHeader()
+                pose.header.stamp = LCMTime()
+                [pose.header.stamp.sec, pose.header.stamp.nsec] = header_sec_nsec
+                pose.header.frame_id = self.frame_id
+                pose.pose = LCMPose()
+                pose.pose.position = LCMPoint()
+                pose.pose.position.x = endpoint[0]
+                pose.pose.position.y = endpoint[1]
+                pose.pose.position.z = endpoint[2]
+                pose.pose.orientation = LCMQuaternion()
+                pose.pose.orientation.w = float(w_field)
+                lcm_msg.poses.append(pose)
+
+        lcm_msg.poses_length = len(lcm_msg.poses)
+        lcm_msg.header = LCMHeader()
+        lcm_msg.header.stamp = LCMTime()
+        [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = header_sec_nsec
+        lcm_msg.header.frame_id = self.frame_id
+        return lcm_msg.lcm_encode()  # type: ignore[no-any-return]
 
     @classmethod
     def lcm_decode(cls, data: bytes | BinaryIO) -> LineSegments3D:

dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py

@@ -90,6 +90,7 @@ class WavefrontConfig(ModuleConfig):
     info_gain_threshold: float = 0.03
     num_no_gain_attempts: int = 2
     goal_timeout: float = 15.0
+    frame_id: str = "world"
 
 
 class WavefrontFrontierExplorer(Module):
@@ -751,7 +752,7 @@ def stop_exploration(self) -> bool:
             goal = PoseStamped(
                 position=self.latest_odometry.position,
                 orientation=self.latest_odometry.orientation,
-                frame_id="world",
+                frame_id=self.config.frame_id,
                 ts=self.latest_odometry.ts,
             )
             self.goal_request.publish(goal)
@@ -792,7 +793,7 @@ def _exploration_loop(self) -> None:
                 goal_msg.position.y = goal.y
                 goal_msg.position.z = 0.0
                 goal_msg.orientation.w = 1.0  # No rotation
-                goal_msg.frame_id = "world"
+                goal_msg.frame_id = self.config.frame_id
                 goal_msg.ts = self.latest_costmap.ts
 
                 self.goal_request.publish(goal_msg)

dimos/navigation/nav_stack/benchmarks/pose_graph_kitti360/README.md

@@ -0,0 +1,115 @@
+# Pose-graph SLAM benchmark on KITTI-360
+
+Generic loop-closure benchmark. Drop in any pose-graph SLAM module that
+exposes the standard interface and the runner will replay a KITTI-360
+sequence at it, watch its loop-closure output, and score precision /
+recall / F1 against KITTI's ground-truth pose trajectory.
+
+The module under test never sees KITTI — it only sees streams. The
+runner provides two helper modules:
+
+| Module                       | Role                                                      |
+|------------------------------|-----------------------------------------------------------|
+| `Kitti360PlaybackModule`     | Publishes `registered_scan` + `odometry` from disk        |
+| Your pose-graph SLAM module  | Consumes those, publishes `pose_graph_edges` + `loop_closure` |
+| `PoseGraphScoringModule`     | Subscribes to the outputs, accumulates metrics            |
+
+`autoconnect` wires the three together by stream name.
+
+## Required interface for the module under test
+
+```python
+class YourPoseGraphModule(Module):
+    registered_scan: In[PointCloud2]
+    odometry: In[Odometry]
+
+    pose_graph_edges: Out[NavPath]   # loop edges tagged orientation.w == 0.4
+    loop_closure: Out[NavPath]       # one message per loop-closure update
+```
+
+Edge convention on `pose_graph_edges`: poses are paired
+`(start, end, start, end, …)`. Odometry edges use `orientation.w = 1.0`,
+loop-closure edges use `orientation.w = 0.4`. The timestamp on each
+endpoint's `PoseStamped` header is the keyframe's *creation* timestamp,
+which the scorer uses to map the endpoint back to its originating
+KITTI frame_id.
+
+## Dataset
+
+Download from <https://www.cvlibs.net/datasets/kitti360> (Test SLAM 3D
+split is enough). Expected layout:
+
+```
+<kitti360-root>/
+├── calibration/
+├── data_3d_raw/
+│   └── 2013_05_28_drive_<NNNN>_sync/velodyne_points/data/*.bin
+└── data_poses/
+    └── 2013_05_28_drive_<NNNN>_sync/cam0_to_world.txt
+```
+
+Sequence IDs map onto the drive numbers: `2 → drive_0002`, `4 → drive_0004`,
+`8 → drive_0008`, etc.
+
+## Quickstart
+
+```python
+from pathlib import Path
+from dimos.navigation.nav_stack.benchmarks.pose_graph_kitti360.runner import (
+    run_benchmark,
+)
+from dimos.navigation.nav_stack.modules.pgo.pgo_module import PgoModule  # your module
+
+results = run_benchmark(
+    module_under_test=PgoModule.blueprint(),
+    kitti360_root=Path("~/datasets/kitti360").expanduser(),
+    sequence_id=2,
+    max_scans=None,         # None = full sequence (~3k frames for seq 2)
+    publish_interval_sec=0.02,
+)
+
+print(results)
+# {
+#   "true_positive": ..., "false_positive": ..., "false_negative": ...,
+#   "precision": ..., "recall": ..., "f1": ...,
+#   "detected_loop_edges": ..., "loop_closure_events": ...,
+#   "wallclock_seconds": ..., "sequence_id": 2,
+# }
+```
+
+## Ground-truth definition
+
+A loop pair `(i, j)` counts as ground truth if:
+
+* frame gap `|i − j| ≥ DEFAULT_MIN_FRAME_GAP` (default 50), AND
+* lidar-position distance ≤ `DEFAULT_MAX_LOOP_DISTANCE_M` (default 4.0 m).
+
+Tune via `min_frame_gap=` and `max_loop_distance_m=` on `run_benchmark`.
+
+A detected edge `(i, j)` is a **true positive** if `j` is in the
+ground-truth valid-loop set for `i` (or vice-versa). Otherwise it's a
+false positive. Ground-truth queries with no detection in their valid
+set become false negatives.
+
+## Files
+
+| File | What it does |
+|------|--------------|
+| `runner.py`              | `run_benchmark()` — builds the blueprint, polls playback, returns scores |
+| `playback.py`            | `Kitti360PlaybackModule` — streams scan + odom messages from disk        |
+| `scoring.py`             | `PoseGraphScoringModule`, `LoopMetrics` — accumulates TP/FP/FN           |
+| `kitti360_loader.py`     | `load_kitti360_sequence()` — reads velodyne `.bin` + `cam0_to_world.txt` |
+| `loop_groundtruth.py`    | `compute_loop_groundtruth()` + thresholds                                |
+
+## Tips
+
+- Start with `max_scans=200` for a smoke test; you should see playback
+  hit ~95% and a couple of GT pairs before paying for the full 3000-scan
+  run (~2.5 min wall on a Mac).
+- Recall is bounded by your module's loop-search radius. KITTI ground
+  truth uses 4 m; if your module searches a 1 m radius, recall floors
+  near zero by construction even on a perfect descriptor.
+- The scorer maps edge endpoints back to frame_ids via timestamps. If
+  your module rewrites pose timestamps after iSAM2 optimization, keep
+  the **creation** timestamp on the `PoseStamped` header so the lookup
+  still works.