fix: dimos map global and dimos map replay to use tf

dimos/control/blueprints/mobile.py

@@ -140,6 +140,8 @@ def _flowbase_twist_base(
                 "publish_free_paths": False,
             },
             simple_planner={
+                # FastLio2 publishes odom -> mid360_link (no separate body frame).
+                "body_frame": "mid360_link",
                 "cell_size": 0.2,
                 "obstacle_height_threshold": 0.15,
                 "inflation_radius": 0.3,  # FlowBase footprint smaller than G1's 0.5

dimos/experimental/go2_tf_tree_fix.py

@@ -0,0 +1,125 @@
+# 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.
+
+"""Fix the tf tree of go2 recordings made on andrew/feat/navigation-dev.
+
+Those recordings contain two competing pose chains:
+
+    world->base_link (from go2 odom)
+    world->body (from pointlio)
+        base_link->front_camera->{mid360_link, camera_optical}
+        base_link->camera_link->camera_optical (go2 driver)
+
+This rewrites them into a single tree rooted at the pointlio estimate:
+
+    world->go2_base_link->go2_camera_link->go2_camera_optical
+    world->mid360_link (from pointlio, was world->body)
+        mid360_link->base_link->front_camera->camera_optical
+
+The pointlio_odometry child frame is renamed body->mid360_link to match.
+
+Usage: uv run python -m dimos.experimental.go2_tf_tree_fix <recording.db>
+(edits the db in place)
+"""
+
+import argparse
+import sqlite3
+
+from dimos.msgs.geometry_msgs.Transform import Transform
+from dimos.msgs.nav_msgs.Odometry import Odometry
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+
+GO2_CHAIN_RENAMES = {
+    "base_link": "go2_base_link",
+    "camera_link": "go2_camera_link",
+    "camera_optical": "go2_camera_optical",
+}
+
+
+def rewrite_tf_message(message: TFMessage, latest_base_to_front: list[Transform | None]) -> bool:
+    changed = False
+    for transform in message.transforms:
+        if transform.frame_id == "base_link" and transform.child_frame_id == "front_camera":
+            latest_base_to_front[0] = transform
+
+    new_transforms = []
+    for transform in message.transforms:
+        edge = (transform.frame_id, transform.child_frame_id)
+        if edge == ("world", "base_link"):
+            transform.child_frame_id = "go2_base_link"
+            changed = True
+        elif edge == ("world", "body"):
+            transform.child_frame_id = "mid360_link"
+            changed = True
+        elif edge in (("base_link", "camera_link"), ("camera_link", "camera_optical")):
+            transform.frame_id = GO2_CHAIN_RENAMES[transform.frame_id]
+            transform.child_frame_id = GO2_CHAIN_RENAMES[transform.child_frame_id]
+            changed = True
+        elif edge == ("front_camera", "mid360_link"):
+            base_to_front = latest_base_to_front[0]
+            if base_to_front is None:
+                raise RuntimeError("saw front_camera->mid360_link before base_link->front_camera")
+            base_to_mid = base_to_front.to_matrix() @ transform.to_matrix()
+            transform = Transform.from_matrix(base_to_mid).inverse()
+            transform.frame_id = "mid360_link"
+            transform.child_frame_id = "base_link"
+            transform.ts = base_to_front.ts
+            changed = True
+        new_transforms.append(transform)
+    message.transforms = new_transforms
+    return changed
+
+
+def fix_database(db_path: str) -> None:
+    connection = sqlite3.connect(db_path)
+
+    latest_base_to_front: list[Transform | None] = [None]
+    tf_updates = 0
+    for row_id, data in connection.execute("SELECT id, data FROM tf_blob ORDER BY id"):
+        message = TFMessage.lcm_decode(bytes(data))
+        if rewrite_tf_message(message, latest_base_to_front):
+            connection.execute(
+                "UPDATE tf_blob SET data = ? WHERE id = ?", (message.lcm_encode(), row_id)
+            )
+            tf_updates += 1
+    print(f"rewrote {tf_updates} tf messages")
+
+    odom_updates = 0
+    for row_id, data in connection.execute("SELECT id, data FROM pointlio_odometry_blob"):
+        odometry = Odometry.lcm_decode(bytes(data))
+        if odometry.child_frame_id == "body":
+            odometry.child_frame_id = "mid360_link"
+            connection.execute(
+                "UPDATE pointlio_odometry_blob SET data = ? WHERE id = ?",
+                (odometry.lcm_encode(), row_id),
+            )
+            odom_updates += 1
+    print(f"renamed child frame on {odom_updates} pointlio_odometry messages")
+
+    connection.commit()
+    integrity = connection.execute("PRAGMA integrity_check").fetchone()[0]
+    connection.execute("PRAGMA wal_checkpoint(TRUNCATE)")
+    connection.close()
+    print(f"integrity_check: {integrity}")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("db_path", help="recording .db to fix in place")
+    arguments = parser.parse_args()
+    fix_database(arguments.db_path)
+
+
+if __name__ == "__main__":
+    main()

dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix

@@ -13,8 +13,9 @@
       flake = false;
     };
     fast-lio = {
-      # v0.3.0-quiet-logs + get_body_cloud() (sensor-frame cloud).
-      url = "github:dimensionalOS/dimos-module-fastlio2?ref=jeff/feat/fastlio-body-cloud";
+      # get_body_cloud()/get_body_cloud_down() with the IMU<-lidar extrinsic
+      # applied (PR #1); retarget to jeff/feat/fastlio-body-cloud once merged.
+      url = "github:dimensionalOS/dimos-module-fastlio2?ref=ivan/fix/body-cloud-imu-extrinsic";
       flake = false;
     };
     lcm-extended = {

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

@@ -59,7 +59,6 @@ static FastLio* g_fastlio = nullptr;
 static std::string g_lidar_topic;
 static std::string g_odometry_topic;
 static std::string g_frame_id;  // required via --frame_id
-static std::string g_child_frame_id;  // required via --child_frame_id
 static std::string g_sensor_frame_id;  // required via --sensor_frame_id
 static float g_frequency = 10.0f;
 
@@ -154,7 +153,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times
 
     nav_msgs::Odometry msg;
     msg.header = make_header(g_frame_id, timestamp);
-    msg.child_frame_id = g_child_frame_id;
+    msg.child_frame_id = g_sensor_frame_id;
 
     msg.pose.pose.position.x = odom.pose.pose.position.x;
     msg.pose.pose.position.y = odom.pose.pose.position.y;
@@ -399,7 +398,6 @@ int main(int argc, char** argv) {
     std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155");
     g_frequency = mod.arg_float("frequency", 10.0f);
     g_frame_id = mod.arg_required("frame_id");
-    g_child_frame_id = mod.arg_required("child_frame_id");
     g_sensor_frame_id = mod.arg_required("sensor_frame_id");
     float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f);
     float odom_freq = mod.arg_float("odom_freq", 50.0f);

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

@@ -52,7 +52,7 @@
 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.cmu_nav.frames import FRAME_BODY, FRAME_ODOM
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
 from dimos.spec import perception
 
 # Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes.
@@ -71,12 +71,9 @@ class FastLio2Config(NativeModuleConfig):
     lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_FASTLIO_LIDAR_IP"))
     frequency: float = 10.0
 
-    # Odometry is published as frame_id (fixed) -> child_frame_id (moving body),
+    # Odometry is published as frame_id (fixed) -> sensor_frame_id (moving sensor),
     # and also broadcast on TF. The point cloud is stamped with sensor_frame_id
-    # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet
-    # transformed into the body frame).
     frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
     sensor_frame_id: str = "mid360_link"
 
     # FAST-LIO internal processing rates
@@ -150,7 +147,7 @@ def _on_odom_for_tf(self, msg: Odometry) -> None:
         self.tf.publish(
             Transform(
                 frame_id=self.frame_id,
-                child_frame_id=self.config.child_frame_id,
+                child_frame_id=self.config.sensor_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,

dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix

@@ -13,7 +13,9 @@
       flake = false;
     };
     fast-lio = {
-      url = "github:dimensionalOS/dimos-module-fastlio2/pointlio";
+      # Point-LIO fork (split out of dimos-module-fastlio2's pointlio branch).
+      # Repo is org-internal for now, hence git+ssh instead of github:.
+      url = "git+ssh://git@github.com/dimensionalOS/dimos-module-pointlio?ref=main";
       flake = false;
     };
     lcm-extended = {

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

@@ -77,7 +77,6 @@ static std::vector<double> parse_doubles(const std::string& csv) {
 static std::string g_lidar_topic;
 static std::string g_odometry_topic;
 static std::string g_frame_id;          // required via --frame_id
-static std::string g_child_frame_id;     // required via --child_frame_id
 static std::string g_sensor_frame_id;    // required via --sensor_frame_id
 static float g_frequency = 10.0f;
 
@@ -158,7 +157,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times
 
     nav_msgs::Odometry msg;
     msg.header = make_header(g_frame_id, timestamp);
-    msg.child_frame_id = g_child_frame_id;
+    msg.child_frame_id = g_sensor_frame_id;
 
     // Pose in the SLAM/sensor frame.
     msg.pose.pose.position.x = odom.pose.pose.position.x;
@@ -382,7 +381,6 @@ int main(int argc, char** argv) {
     std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155");
     g_frequency = mod.arg_float("frequency", 10.0f);
     g_frame_id = mod.arg_required("frame_id");
-    g_child_frame_id = mod.arg_required("child_frame_id");
     g_sensor_frame_id = mod.arg_required("sensor_frame_id");
     float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f);
     float odom_freq = mod.arg_float("odom_freq", 50.0f);

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

@@ -58,7 +58,7 @@
 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.cmu_nav.frames import FRAME_BODY, FRAME_ODOM
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
 from dimos.spec import perception
 
 # Human-readable enums; the C++ binary (main.cpp) maps these strings to
@@ -81,12 +81,13 @@ class PointLioConfig(NativeModuleConfig):
     lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP"))
     frequency: float = 10.0
 
-    # Odometry is published as frame_id (fixed) -> child_frame_id (moving body),
-    # and also broadcast on TF. The point cloud is stamped with sensor_frame_id
-    # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet
-    # transformed into the body frame).
+    # Odometry is published as frame_id (fixed) -> sensor_frame_id (moving), and
+    # also broadcast on TF. Point-LIO runs with identity extrinsics, so its
+    # tracked pose IS the sensor frame — the rest of the robot tree (base_link,
+    # cameras) hangs off it via static transforms (see Go2Mid360StaticTf). The
+    # point cloud is stamped with sensor_frame_id (undistorted scan in the
+    # sensor frame).
     frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
     sensor_frame_id: str = "mid360_link"
 
     # Point-LIO internal processing rates (Hz)
@@ -186,7 +187,7 @@ def _on_odom_for_tf(self, msg: Odometry) -> None:
         self.tf.publish(
             Transform(
                 frame_id=self.frame_id,
-                child_frame_id=self.config.child_frame_id,
+                child_frame_id=self.config.sensor_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,