never-merge: Go2 3D navigation (MLS planner, ray tracing) + mid360 mount shim

dimos/constants.py

@@ -53,3 +53,6 @@
 DEFAULT_THREAD_JOIN_TIMEOUT = 2.0
 
 DEFAULT_BUILD_NATIVE = False
+
+DEFAULT_WORLD_FRAME = "world"
+DEFAULT_ROBOT_FRAME = "base_link"

dimos/core/module.py

@@ -109,6 +109,11 @@ class ModuleConfig(BaseConfig):
     tf_transport: type[TFSpec] = LCMTF  # type: ignore[type-arg]
     frame_id_prefix: str | None = None
     frame_id: str | None = None
+    # TODO: later expose frame remappings, somehow, at the blueprint level
+    # frame mapping is how others can remap frame names, in the future we should have a way to do this remapping at the blueprint level too
+    # (static-transform publishing lives on the TfModule subclass, but frame_mapping
+    #  stays here because blueprint-level frame namespacing applies to every module)
+    frame_mapping: dict[str, str] = Field(default_factory=dict)
     g: GlobalConfig = global_config
 
 
@@ -149,6 +154,7 @@ def __init__(self, config_args: dict[str, Any]) -> None:
         self._tools = {}
         self._tools_lock = threading.Lock()
         self._loop, self._loop_thread = get_loop()
+        self.frame_mapping = self._setup_frame_mapping()
         try:
             self.rpc = self.config.rpc_transport(  # type: ignore[call-arg]
                 rpc_timeouts=self.config.rpc_timeouts,
@@ -191,6 +197,34 @@ def stop(self) -> None:
         super().stop()
         self._close_module()
 
+    def _setup_frame_mapping(self) -> dict[str, str]:
+        # frame_mapping resolution stays on the base Module (every module exposes a
+        # resolved frame_mapping for blueprint-level namespacing). Static-transform
+        # publishing lives on the TfModule subclass.
+        frame_mapping_field = type(self.config).model_fields["frame_mapping"]
+        if not hasattr(frame_mapping_field, "default_factory") or not callable(
+            frame_mapping_field.default_factory
+        ):
+            raise ValueError(
+                f"""In the {self.name!r} module config definition, the frame_mapping needs to be a pydantic field, not a dict"""
+            )
+        # given something like:
+        # class MyConfig:
+        #     frame_mapping: dict[str, str] = Field(default_factory=lambda: dict(
+        #         body="base_link",
+        #         parent="world",
+        #     ))
+        # the "body" is what I call a common_name
+        # "base_link" is the REAL frame id that other modules can query/use
+        existing_frames: dict[str, str] = frame_mapping_field.default_factory()  # type: ignore[call-arg]
+        final_frame_mapping = {**existing_frames, **self.config.frame_mapping}
+        for existing_frame, remapped_frame in final_frame_mapping.items():
+            if existing_frame not in existing_frames:
+                raise ValueError(
+                    f"""On module {self.name}, tried to map {existing_frame!r} to {remapped_frame!r} but that first frame doesn't exist. The existing ones are: {list(existing_frames.keys())!r} """
+                )
+        return final_frame_mapping
+
     def _close_module(self) -> None:
         with self._module_closed_lock:
             if self._module_closed:

dimos/core/tf_module.py

@@ -0,0 +1,142 @@
+# Copyright 2025-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.
+import threading
+import time
+from typing import Any
+
+from pydantic import Field
+
+from dimos.core.core import rpc
+from dimos.core.module import Module, ModuleConfig
+from dimos.msgs.geometry_msgs.Transform import Transform
+
+
+class TfModuleConfig(ModuleConfig):
+    static_transforms: dict[str, Transform] = Field(default_factory=dict)
+    # TODO: in the future we should make self.tf.publish error if it tried to publish a transform that references a frame that is not mentioned in this dict (same with self.tf.get)
+    static_publish_interval: float = 1.0
+
+
+class TfModule(Module):
+    """A Module that republishes its config's static (non-moving) transforms on an interval.
+
+    Modules that need to publish fixed frames inherit from this instead of Module
+    directly, so the base Module stays free of transform-publishing machinery that
+    most modules don't need. `frame_mapping` resolution still lives on Module (every
+    module needs it for blueprint-level frame namespacing); only the static-transform
+    publishing is added here.
+    """
+
+    config: TfModuleConfig
+
+    def __init__(self, **kwargs: Any) -> None:
+        super().__init__(**kwargs)
+        self._static_publish_thread: threading.Thread | None = None
+        # created lazily in start() so the module stays picklable for worker deployment
+        self._static_publish_stop: threading.Event | None = None
+        self.static_transforms = self._resolve_static_transforms()
+
+    @rpc
+    def start(self) -> None:
+        # NOTE: there's basically always going to be some inital race around static transform frames and tf.get's
+        # publishing the statics before main starts helps mitigate/reduce that
+        self._start_static_publish()
+        super().start()
+
+    @rpc
+    def stop(self) -> None:
+        self._stop_static_publish()
+        super().stop()
+
+    def _resolve_static_transforms(self) -> dict[str, Transform]:
+        frame_mapping_field = type(self.config).model_fields["frame_mapping"]
+        existing_frames: dict[str, str] = frame_mapping_field.default_factory()  # type: ignore[misc,union-attr,call-arg]
+
+        # step1 translate urdf_name=>common_name (see Module._setup_frame_mapping for what
+        # "common name" vs "real frame id" means)
+        reverse_mapping = {value: key for key, value in existing_frames.items()}
+        static_transforms_common_names = {
+            reverse_mapping.get(urdf_frame_id, urdf_frame_id): Transform(
+                translation=transform.translation,
+                rotation=transform.rotation,
+                frame_id=reverse_mapping.get(transform.frame_id, transform.frame_id),
+                child_frame_id=reverse_mapping.get(
+                    transform.child_frame_id, transform.child_frame_id
+                ),
+            )
+            for urdf_frame_id, transform in self.config.static_transforms.items()
+        }
+        # step2 map common_name=>real_frame_id using the module's resolved frame_mapping
+        final_frame_mapping = self.frame_mapping
+        return {
+            final_frame_mapping.get(common_frame_id, common_frame_id): Transform(
+                translation=transform.translation,
+                rotation=transform.rotation,
+                frame_id=final_frame_mapping.get(transform.frame_id, transform.frame_id),
+                child_frame_id=final_frame_mapping.get(
+                    transform.child_frame_id, transform.child_frame_id
+                ),
+            )
+            for common_frame_id, transform in static_transforms_common_names.items()
+        }
+
+    def _start_static_publish(self) -> None:
+        self._static_publish()
+        if not self.static_transforms or self.config.static_publish_interval <= 0:
+            return
+        self._static_publish_stop = threading.Event()
+        self._static_publish_thread = threading.Thread(
+            target=self._static_publisher,
+            daemon=True,
+        )
+        self._static_publish_thread.start()
+
+    # TODO: later this should be replaced with latching streams
+    def _static_publisher(self) -> None:
+        stop = self._static_publish_stop
+        assert stop is not None
+        while not stop.wait(self.config.static_publish_interval):
+            self._static_publish()
+
+    def _static_publish(self) -> None:
+        if not self.static_transforms:
+            return
+        now = time.time()
+        self.tf.publish_static(
+            *(
+                Transform(
+                    translation=transform.translation,
+                    rotation=transform.rotation,
+                    frame_id=transform.frame_id,
+                    child_frame_id=transform.child_frame_id,
+                    ts=now,
+                )
+                for transform in self.static_transforms.values()
+            )
+        )
+        self._on_static_publish()
+
+    def _on_static_publish(self) -> None:
+        """
+        This is a callback for modules to publish other data (ex: camera info) in the static loop
+        This should be rarely used, but exists for the few cases where it is needed
+        """
+
+    def _stop_static_publish(self) -> None:
+        if self._static_publish_stop is not None:
+            self._static_publish_stop.set()
+        if self._static_publish_thread and self._static_publish_thread.is_alive():
+            self._static_publish_thread.join(timeout=self._loop_thread_timeout)
+        self._static_publish_thread = None
+        self._static_publish_stop = None

dimos/hardware/sensors/lidar/pointlio/config/no_gravity_align.yaml

@@ -0,0 +1,62 @@
+# default.yaml with gravity self-leveling turned off (mapping.gravity_align).
+# Used when the mount tilt is corrected by a known static transform instead of
+# letting Point-LIO level the odometry to gravity. Keep the rest in sync with
+# default.yaml.
+common:
+    con_frame: false
+    con_frame_num: 1
+    cut_frame: false
+    cut_frame_time_interval: 0.1
+    time_lag_imu_to_lidar: 0.0
+
+preprocess:
+    lidar_type: 1
+    scan_line: 4
+    scan_rate: 10
+    timestamp_unit: 3        # 3 = nanosecond
+    blind: 0.5
+    point_filter_num: 3
+
+mapping:
+    use_imu_as_input: false
+    prop_at_freq_of_imu: true
+    check_satu: true
+    init_map_size: 10
+    space_down_sample: true
+    satu_acc: 3.0
+    satu_gyro: 35.0
+    acc_norm: 1.0
+    plane_thr: 0.1
+    filter_size_surf: 0.2
+    filter_size_map: 0.5
+    ivox_grid_resolution: 2.0
+    ivox_nearby_type: 6
+    cube_side_length: 1000.0
+    det_range: 100.0
+    fov_degree: 360.0
+    imu_en: true
+    start_in_aggressive_motion: false
+    extrinsic_est_en: false
+    imu_time_inte: 0.005
+    lidar_meas_cov: 0.01
+    acc_cov_input: 0.1
+    vel_cov: 20.0
+    gyr_cov_input: 0.01
+    gyr_cov_output: 1000.0
+    acc_cov_output: 500.0
+    b_gyr_cov: 0.0001
+    b_acc_cov: 0.0001
+    imu_meas_acc_cov: 0.01
+    imu_meas_omg_cov: 0.01
+    match_s: 81.0
+    gravity_align: false
+    gravity: [0.0, 0.0, -9.810]
+    gravity_init: [0.0, 0.0, -9.810]
+    extrinsic_T: [-0.011, -0.02329, 0.04412]   # Mid-360 IMU->lidar offset (m)
+    extrinsic_R: [1.0, 0.0, 0.0,
+                  0.0, 1.0, 0.0,
+                  0.0, 0.0, 1.0]
+
+odometry:
+    publish_odometry_without_downsample: false
+    odom_only: false

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

@@ -16,6 +16,8 @@
 
 #include <lcm/lcm-cpp.hpp>
 
+#include <Eigen/Dense>
+
 #include <atomic>
 #include <boost/make_shared.hpp>
 #include <chrono>
@@ -81,6 +83,17 @@ 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;
 
+// Optional rigid transform applied to the published cloud + odometry (and, via
+// the Python TF republisher that reads the odometry, the odom->body TF). Lets a
+// physically-tilted mount be reported as a different mount without touching the
+// SLAM core. Identity unless --transform supplies a row-major 4x4 (16 doubles).
+// Cloud points are moved p' = C @ p; the body pose is conjugated T' = C @ T @
+// inv(C) and the twist rotated by C's rotation, so cloud and odometry stay
+// mutually consistent.
+static bool g_has_transform = false;
+static Eigen::Matrix4d g_transform = Eigen::Matrix4d::Identity();
+static Eigen::Matrix4d g_transform_inv = Eigen::Matrix4d::Identity();
+
 // Frame accumulator (Livox SDK raw → CustomMsg)
 static std::mutex g_pc_mutex;
 // Serializes all Point-LIO EKF access. The SDK delivers IMU on its own callback
@@ -142,11 +155,22 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std
     pc.data_length = pc.row_step;
     pc.data.resize(pc.data_length);
 
+    const Eigen::Matrix3d transform_rotation = g_transform.block<3, 3>(0, 0);
+    const Eigen::Vector3d transform_translation = g_transform.block<3, 1>(0, 3);
     for (int point_idx = 0; point_idx < num_points; ++point_idx) {
         float* dst = reinterpret_cast<float*>(pc.data.data() + point_idx * 16);
-        dst[0] = cloud->points[point_idx].x;
-        dst[1] = cloud->points[point_idx].y;
-        dst[2] = cloud->points[point_idx].z;
+        double x = cloud->points[point_idx].x;
+        double y = cloud->points[point_idx].y;
+        double z = cloud->points[point_idx].z;
+        if (g_has_transform) {
+            const Eigen::Vector3d moved = transform_rotation * Eigen::Vector3d(x, y, z) + transform_translation;
+            x = moved.x();
+            y = moved.y();
+            z = moved.z();
+        }
+        dst[0] = static_cast<float>(x);
+        dst[1] = static_cast<float>(y);
+        dst[2] = static_cast<float>(z);
         dst[3] = cloud->points[point_idx].intensity;
     }
 
@@ -161,25 +185,48 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times
     msg.child_frame_id = g_child_frame_id;
 
     // Pose in the SLAM/sensor frame.
-    msg.pose.pose.position.x = odom.pose.pose.position.x;
-    msg.pose.pose.position.y = odom.pose.pose.position.y;
-    msg.pose.pose.position.z = odom.pose.pose.position.z;
-    msg.pose.pose.orientation.x = odom.pose.pose.orientation.x;
-    msg.pose.pose.orientation.y = odom.pose.pose.orientation.y;
-    msg.pose.pose.orientation.z = odom.pose.pose.orientation.z;
-    msg.pose.pose.orientation.w = odom.pose.pose.orientation.w;
+    Eigen::Vector3d position(
+        odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);
+    Eigen::Quaterniond orientation(
+        odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
+        odom.pose.pose.orientation.y, odom.pose.pose.orientation.z);
+    Eigen::Vector3d linear(
+        odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z);
+    Eigen::Vector3d angular(
+        odom.twist.twist.angular.x, odom.twist.twist.angular.y, odom.twist.twist.angular.z);
+
+    if (g_has_transform) {
+        Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
+        pose.block<3, 3>(0, 0) = orientation.toRotationMatrix();
+        pose.block<3, 1>(0, 3) = position;
+        const Eigen::Matrix4d faked = g_transform * pose * g_transform_inv;
+        const Eigen::Matrix3d transform_rotation = g_transform.block<3, 3>(0, 0);
+        position = faked.block<3, 1>(0, 3);
+        orientation = Eigen::Quaterniond(faked.block<3, 3>(0, 0));
+        orientation.normalize();
+        linear = transform_rotation * linear;
+        angular = transform_rotation * angular;
+    }
+
+    msg.pose.pose.position.x = position.x();
+    msg.pose.pose.position.y = position.y();
+    msg.pose.pose.position.z = position.z();
+    msg.pose.pose.orientation.x = orientation.x();
+    msg.pose.pose.orientation.y = orientation.y();
+    msg.pose.pose.orientation.z = orientation.z();
+    msg.pose.pose.orientation.w = orientation.w();
 
     for (int idx = 0; idx < 36; ++idx) {
         msg.pose.covariance[idx] = odom.pose.covariance[idx];
     }
 
     // Velocity from Point-LIO's IESKF state (its key output over FAST-LIO).
-    msg.twist.twist.linear.x = odom.twist.twist.linear.x;
-    msg.twist.twist.linear.y = odom.twist.twist.linear.y;
-    msg.twist.twist.linear.z = odom.twist.twist.linear.z;
-    msg.twist.twist.angular.x = odom.twist.twist.angular.x;
-    msg.twist.twist.angular.y = odom.twist.twist.angular.y;
-    msg.twist.twist.angular.z = odom.twist.twist.angular.z;
+    msg.twist.twist.linear.x = linear.x();
+    msg.twist.twist.linear.y = linear.y();
+    msg.twist.twist.linear.z = linear.z();
+    msg.twist.twist.angular.x = angular.x();
+    msg.twist.twist.angular.y = angular.y();
+    msg.twist.twist.angular.z = angular.z();
     std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance));
 
     g_lcm->publish(g_odometry_topic, &msg);
@@ -368,6 +415,16 @@ int main(int argc, char** argv) {
     if (auto gi = parse_doubles(mod.arg("gravity_init", "")); !gi.empty()) params.gravity_init = gi;
     if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et;
     if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er;
+    // Optional post-SLAM mount correction (row-major 4x4); identity when absent.
+    if (auto transform = parse_doubles(mod.arg("transform", "")); transform.size() == 16) {
+        g_has_transform = true;
+        for (int row = 0; row < 4; ++row) {
+            for (int col = 0; col < 4; ++col) {
+                g_transform(row, col) = transform[row * 4 + col];
+            }
+        }
+        g_transform_inv = g_transform.inverse();
+    }
     // odometry
     params.publish_odometry_without_downsample =
         mod.arg_bool("publish_odometry_without_downsample", params.publish_odometry_without_downsample);