r1pro-nav: dimSLAM fuses Point-LIO and wheel odometry into the planners' pose

dimos/control/coordinator.py

@@ -95,6 +95,8 @@ class ControlCoordinatorConfig(ModuleConfig):
 
     tick_rate: float = 100.0
     publish_joint_state: bool = True
+    # At most this often (None: every tick); each subscriber pays per message.
+    joint_state_hz: float | None = None
     # Transitional: goes away once every consumer reads per-robot streams.
     publish_robot_joint_states: bool = False
     joint_state_frame_id: str = "coordinator"
@@ -902,7 +904,9 @@ def start(self) -> None:
 
         # Create and start tick loop
         publish_cb = (
-            self.coordinator_joint_state.publish if self.config.publish_joint_state else None
+            _rate_limited(self.coordinator_joint_state.publish, self.config.joint_state_hz)
+            if self.config.publish_joint_state
+            else None
         )
         publish_robot_cb = (
             self._publish_robot_joint_state if self.config.publish_robot_joint_states else None
@@ -975,3 +979,20 @@ def stop(self) -> None:
     def get_tick_count(self) -> int:
         """Get the number of ticks since start."""
         return self._tick_loop.tick_count if self._tick_loop else 0
+
+
+def _rate_limited(
+    publish: "Callable[[JointState], None]", hz: float | None
+) -> "Callable[[JointState], None]":
+    """``publish``, dropping calls that come sooner than 1 / ``hz`` after the last one passed."""
+    if hz is None:
+        return publish
+    next_t = [0.0]
+
+    def limited(msg: JointState) -> None:
+        now = time.monotonic()
+        if now >= next_t[0]:
+            next_t[0] = now + 1.0 / hz
+            publish(msg)
+
+    return limited

dimos/control/test_coordinator_commands.py

@@ -34,7 +34,7 @@
 import pytest
 
 import dimos.control.coordinator as coord_mod
-from dimos.control.coordinator import ControlCoordinator
+from dimos.control.coordinator import ControlCoordinator, _rate_limited
 from dimos.control.task import (
     BaseControlTask,
     CoordinatorState,
@@ -357,3 +357,17 @@ def test_g1_reports_twist_stream_and_reset_command(self, coordinator):
         assert ("twist_command", "broadcast") in desc["streams"]
         assert "reset_runtime_state" in desc["commands"]
         assert desc["commands"]["reset_runtime_state"]["params"] == ["reactivate"]
+
+
+def test_rate_limited_publish_drops_calls_within_the_period(monkeypatch):
+    sent = []
+    clock = [10.0]
+    monkeypatch.setattr("dimos.control.coordinator.time.monotonic", lambda: clock[0])
+    publish = _rate_limited(sent.append, 30.0)
+    for t in (0.0, 0.01, 0.02, 0.034, 0.05, 0.07):
+        clock[0] = 10.0 + t
+        publish(t)
+
+    assert sent == [0.0, 0.034, 0.07]
+    publish_all = sent.append
+    assert _rate_limited(publish_all, None) is publish_all

dimos/mapping/dim_slam/dim_slam.py

@@ -99,24 +99,40 @@ class InitialStds(BaseModel):
     position: float = 0.01
     velocity: float = 0.1
     rotation: float = 0.05
+    # Without an IMU only.
+    angular_velocity: float = 0.1
     bias: float = 0.05
 
 
+class ProcessNoise(BaseModel):
+    """How fast the body's velocity can change unannounced; only used without an IMU."""
+
+    # m/s^2/sqrt(Hz).
+    linear_acceleration: float = 1.0
+    # rad/s^2/sqrt(Hz).
+    angular_acceleration: float = 1.0
+
+
 class SourceConfig(BaseModel):
     """One external odometry source and how much it is trusted. A variance below zero takes
     the message covariance, zero drops that dimension, above zero is a fixed variance. A
     drifting source's covariance describes its accumulated drift rather than the delta
     being fused, so a fixed value is usually the right answer."""
 
-    # The transform this source's estimates carry: each message's header.frame_id and
-    # child_frame_id. Both halves are needed because two sources can share a parent. A
-    # source whose parent is odom_frame_id is fused absolutely; anything else is fused as
-    # filter-anchored deltas, since its own pose has drifted.
+    # The transform each message carries (header.frame_id -> child_frame_id); two sources can share a parent.
     parent_frame_id: str = ""
     child_frame_id: str = ""
+    # pose: a pose that does not drift (lidar odometry); twist: twist only (wheels); pose_delta: a drifting pose.
+    kind: Literal["pose", "twist", "pose_delta"] = "pose_delta"
     pose_variances: Covariance = Field(default_factory=Covariance)
     # Body frame.
     twist_variances: Covariance = Field(default_factory=Covariance)
+    # pose only: re-align a source to the fused pose after it is rejected this long; 0 never re-aligns.
+    realign_after_seconds: float = 0.0
+    # Zero-velocity update: a twist at or under these (m/s, rad/s) is fused as standing still; 0 variance disables.
+    still_variance: float = 0.0
+    still_speed: float = 0.0
+    still_angular_speed: float = 0.0
 
 
 class DimSlamConfig(NativeModuleConfig):
@@ -163,7 +179,7 @@ class DimSlamConfig(NativeModuleConfig):
     # Off when something downstream owns odom -> output_frame_id.
     publish_tf: bool = True
 
-    # The filter itself steps at the IMU rate; this is only how often it emits.
+    # Output cadence, on a timer: each output is the filter predicted to its own stamp.
     publish_rate: float = 50.0
     replay_buffer_seconds: float = 0.5
     # Outlier gate in variance units, per measurement dimension. Smaller is more
@@ -172,14 +188,14 @@ class DimSlamConfig(NativeModuleConfig):
     # Caps the filter's own state rather than an incoming reading. 0 disables it.
     max_position_m: float = 10000.0
 
-    # Empty disables inertial propagation: the filter is seeded level from the first source
-    # message and holds its pose between them.
+    # Empty predicts by constant velocity instead (see process_noise).
     imus: list[ImuConfig] = Field(default_factory=list)
     # m/s^2. Why is gravity a config?
     # 1) different IMUs have different bias
     # 2) gravity changes with latitude by a non-trivial amount for IMUs (0.05 m/s^2)
     initial_gravity_estimate: float = 9.8
     initial_stds: InitialStds = Field(default_factory=InitialStds)
+    process_noise: ProcessNoise = Field(default_factory=ProcessNoise)
 
     # Trust in the tracker's pose, per SourceConfig semantics; the module registers the
     # tracker as a fusion source itself, so it never appears in `odom_sources`. The tracker
@@ -190,10 +206,10 @@ class DimSlamConfig(NativeModuleConfig):
     # One entry per external source (wheel odometry, ...), matched against each message's
     # header.frame_id and child_frame_id.
     odom_sources: list[SourceConfig] = Field(default_factory=list)
-    # A virtual zero-twist measurement applied with every source message, for the
-    # directions the platform cannot move in. Above zero pulls that dimension toward
-    # zero with this variance; zero leaves it free.
-    per_dimension_error_variance: Covariance = Field(default_factory=Covariance)
+    # A ground body: z, roll and pitch held at zero, sources fused in x, y and yaw only.
+    planar: bool = False
+    # Per body-twist axis, the variance of a soft zero-velocity measurement fused with every source message; 0 is free.
+    zero_velocity_variances: Covariance = Field(default_factory=Covariance)
 
     @model_validator(mode="after")
     def _multisensor_lists_its_rig(self) -> DimSlamConfig:

dimos/mapping/dim_slam/rust/Cargo.toml

@@ -11,7 +11,7 @@ path = "src/main.rs"
 [dependencies]
 # tag, not rev: nix's fetchGit only searches the ref the url names, so a bare rev is
 # unfetchable. Cargo.lock still pins the exact commit.
-dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.1" }
+dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta3" }
 # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents
 # and build.rs needs it for the rpath.
 cu_vslam_rs = "0.2.0"
@@ -26,4 +26,8 @@ validator = { version = "0.21", features = ["derive"] }
 [profile.release]
 lto = true
 
+# zenoh 1.10.1 plus upstream's startup-deadlock fix (eclipse-zenoh/zenoh issue 2581), which no release has yet.
+[patch.crates-io]
+zenoh = { git = "https://github.com/jeff-hykin/zenoh-vendor", tag = "zenoh-1.10.1-patch.2" }
+
 [workspace]

dimos/mapping/dim_slam/rust/crate-hashes.json

@@ -1,7 +1,8 @@
 {
-  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.1#dim_slam@0.1.0": "sha256-jx3QjYNclt3JFJOonGIAzfoJeJ5bhowx70a7LumF6IA=",
+  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta3#dim_slam@0.1.0": "sha256-cnt5MFlYfC/0SODg9glSCnR/UpyWqIl8X+H0pmg0Cvo=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
-  "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA="
+  "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
+  "git+https://github.com/jeff-hykin/zenoh-vendor?tag=zenoh-1.10.1-patch.2#zenoh@1.10.1": "sha256-WeW+O7IMZXqYtQK20aRnQm/zy2kmf4Lr1eb4Djk13iI="
 }