simplify

Author: jeff-hykin

dimos/e2e_tests/test_smartnav_replay.py

@@ -14,9 +14,9 @@
 
 """Integration test for the unitree_go2_smartnav blueprint using replay data.
 
-Builds the smartnav pipeline (GO2Connection → OdomAdapter → PGO → CostMapper →
+Builds the smartnav pipeline (GO2Connection → PGO → CostMapper →
 ReplanningAStarPlanner) in replay mode and verifies that data flows end-to-end:
-  - PGO receives scans and odom, publishes corrected_odometry + global_map
+  - PGO receives scans and raw odom (PoseStamped), publishes corrected_odometry + global_map
   - CostMapper receives global_map, publishes global_costmap
 """
 
@@ -34,8 +34,7 @@
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.navigation.smartnav.modules.odom_adapter.odom_adapter import odom_adapter
-from dimos.navigation.smartnav.modules.pgo.pgo import PGO
+from dimos.navigation.loop_closure.pgo import PGO
 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic
 from dimos.robot.unitree.go2.connection import GO2Connection
 
@@ -55,13 +54,12 @@ def smartnav_coordinator():
         n_workers=1,
     )
 
-    # Minimal pipeline: GO2Connection → OdomAdapter → PGO → CostMapper
+    # Minimal pipeline: GO2Connection → PGO → CostMapper
     # Skip ReplanningAStarPlanner and WavefrontFrontierExplorer to avoid
     # needing a goal and cmd_vel sink.
     bp = autoconnect(
         unitree_go2_basic,
         PGO.blueprint(),
-        odom_adapter(),
         cost_mapper(),
     ).global_config(
         n_workers=1,
@@ -187,35 +185,25 @@ def test_costmapper_produces_costmap(self, smartnav_coordinator):
         msg = collector.messages[0]
         assert isinstance(msg, OccupancyGrid), f"Expected OccupancyGrid, got {type(msg)}"
 
-    def test_odom_adapter_converts_bidirectionally(self, smartnav_coordinator):
-        """OdomAdapter should convert PoseStamped→Odometry and Odometry→PoseStamped."""
+    def test_pgo_produces_corrected_pose_stamped(self, smartnav_coordinator):
+        """PGO should publish corrected pose as PoseStamped on the odom output."""
         coord = smartnav_coordinator
 
-        from dimos.navigation.smartnav.modules.odom_adapter.odom_adapter import OdomAdapter
-
-        adapter = None
+        pgo_mod = None
         for mod in coord.all_modules:
-            if isinstance(mod, OdomAdapter):
-                adapter = mod
+            if isinstance(mod, PGO):
+                pgo_mod = mod
                 break
-        assert adapter is not None, "OdomAdapter not found in coordinator"
+        assert pgo_mod is not None, "PGO module not found in coordinator"
 
-        # Collect outputs from both directions
-        odom_out = _StreamCollector()
-        ps_out = _StreamCollector()
-        adapter.odometry._transport.subscribe(odom_out.callback)
-        adapter.odom._transport.subscribe(ps_out.callback)
+        collector = _StreamCollector()
+        pgo_mod.odom._transport.subscribe(collector.callback)
 
         coord.start()
 
-        # OdomAdapter.odometry (PoseStamped→Odometry) should fire from replay odom
-        assert odom_out.wait(count=3, timeout=30), (
-            f"OdomAdapter did not produce Odometry output (got {len(odom_out.messages)})"
-        )
-        assert isinstance(odom_out.messages[0], Odometry)
-
-        # OdomAdapter.odom (Odometry→PoseStamped) fires when PGO publishes corrected_odometry
-        assert ps_out.wait(count=1, timeout=30), (
-            f"OdomAdapter did not produce PoseStamped output (got {len(ps_out.messages)})"
+        assert collector.wait(count=3, timeout=30), (
+            f"PGO did not produce PoseStamped odom output (got {len(collector.messages)})"
         )
-        assert isinstance(ps_out.messages[0], PoseStamped)
+        msg = collector.messages[0]
+        assert isinstance(msg, PoseStamped), f"Expected PoseStamped, got {type(msg)}"
+        assert msg.frame_id == "map"

dimos/navigation/smartnav/__init__.py dimos/navigation/loop_closure/__init__.pydimos/navigation/smartnav/__init__.py renamed to dimos/navigation/loop_closure/__init__.py

@@ -34,6 +34,7 @@
 
 from dimos.core.module import Module, ModuleConfig
 from dimos.core.stream import In, Out
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 
@@ -362,16 +363,18 @@ class PGO(Module[PGOConfig]):
 
     Ports:
         registered_scan (In[PointCloud2]): World-frame registered point cloud.
-        odometry (In[Odometry]): Current pose estimate from SLAM.
-        corrected_odometry (Out[Odometry]): Loop-closure-corrected pose.
+        raw_odom (In[PoseStamped]): Raw pose from robot (PoseStamped).
+        corrected_odometry (Out[Odometry]): Loop-closure-corrected pose (Odometry).
+        odom (Out[PoseStamped]): Loop-closure-corrected pose (PoseStamped, for downstream).
         global_static_map (Out[PointCloud2]): Accumulated keyframe map.
     """
 
     default_config = PGOConfig
 
     registered_scan: In[PointCloud2]
-    odometry: In[Odometry]
+    raw_odom: In[PoseStamped]
     corrected_odometry: Out[Odometry]
+    odom: Out[PoseStamped]
     global_static_map: Out[PointCloud2]
 
     def __init__(self, **kwargs) -> None:  # type: ignore[no-untyped-def]
@@ -401,7 +404,7 @@ def __setstate__(self, state: dict) -> None:
 
     def start(self) -> None:
         self._pgo = _SimplePGO(self.config)
-        self.odometry._transport.subscribe(self._on_odom)
+        self.raw_odom._transport.subscribe(self._on_raw_odom)
         self.registered_scan._transport.subscribe(self._on_scan)
         self._running = True
         self._thread = threading.Thread(target=self._publish_loop, daemon=True)
@@ -414,17 +417,17 @@ def stop(self) -> None:
             self._thread.join(timeout=3.0)
         super().stop()
 
-    def _on_odom(self, msg: Odometry) -> None:
+    def _on_raw_odom(self, msg: PoseStamped) -> None:
         from scipy.spatial.transform import Rotation
 
         q = [
-            msg.pose.orientation.x,
-            msg.pose.orientation.y,
-            msg.pose.orientation.z,
-            msg.pose.orientation.w,
+            msg.orientation.x,
+            msg.orientation.y,
+            msg.orientation.z,
+            msg.orientation.w,
         ]
         r = Rotation.from_quat(q).as_matrix()
-        t = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
+        t = np.array([msg.x, msg.y, msg.z])
         with self._lock:
             self._latest_r = r
             self._latest_t = t
@@ -484,6 +487,14 @@ def _publish_corrected_odom(self, r: np.ndarray, t: np.ndarray, ts: float) -> No
         )
         self.corrected_odometry._transport.publish(odom)
 
+        ps = PoseStamped(
+            ts=ts,
+            frame_id="map",
+            position=[float(t[0]), float(t[1]), float(t[2])],
+            orientation=[float(q[0]), float(q[1]), float(q[2]), float(q[3])],
+        )
+        self.odom._transport.publish(ps)
+
     def _publish_loop(self) -> None:
         """Periodically publish global map."""
         pgo = self._pgo

…s/navigation/smartnav/modules/pgo/pgo.py dimos/navigation/loop_closure/pgo.pydimos/navigation/smartnav/modules/pgo/pgo.py renamed to dimos/navigation/loop_closure/pgo.py dimos/navigation/smartnav/modules/pgo/pgo.py renamed to dimos/navigation/loop_closure/pgo.py

@@ -16,13 +16,14 @@
 """Go2 SmartNav blueprint: PGO + CostMapper + ReplanningAStarPlanner.
 
 Uses PGO for loop-closure-corrected odometry and global map from the Go2's
-world-frame lidar + drifted odom. OdomAdapter bridges PoseStamped <-> Odometry
-between GO2Connection and PGO.
+world-frame lidar + drifted odom. PGO accepts PoseStamped input directly and
+outputs both corrected Odometry and PoseStamped.
 
 Data flow:
     GO2Connection.lidar (remapped → registered_scan) → PGO
-    GO2Connection.odom (remapped → raw_odom) → OdomAdapter → PGO.odometry
-    PGO.corrected_odometry → OdomAdapter → odom → ReplanningAStarPlanner
+    GO2Connection.odom (remapped → raw_odom) → PGO
+    PGO.corrected_odometry (Odometry)
+    PGO.odom (PoseStamped) → ReplanningAStarPlanner
     PGO.global_map → CostMapper → ReplanningAStarPlanner
     ReplanningAStarPlanner.cmd_vel → GO2Connection
 """
@@ -33,15 +34,13 @@
     wavefront_frontier_explorer,
 )
 from dimos.navigation.replanning_a_star.module import replanning_a_star_planner
-from dimos.navigation.smartnav.modules.odom_adapter.odom_adapter import odom_adapter
-from dimos.navigation.smartnav.modules.pgo.pgo import PGO
+from dimos.navigation.loop_closure.pgo import PGO
 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic
 from dimos.robot.unitree.go2.connection import GO2Connection
 
 unitree_go2_smartnav = autoconnect(
     unitree_go2_basic,
     PGO.blueprint(),
-    odom_adapter(),
     cost_mapper(),
     replanning_a_star_planner(),
     wavefront_frontier_explorer(),