refactor: generated CDR Python trajectory followers

.github/workflows/message-codegen.yml

@@ -23,6 +23,10 @@ on:
       - dimos/mapping/voxels/**
       - dimos/navigation/base.py
       - dimos/navigation/tf_pose.py
+      - dimos/navigation/spec.py
+      - dimos/navigation/local_planner/profile.py
+      - dimos/navigation/local_planner/test_profile.py
+      - dimos/navigation/trajectory_follower/**
       - dimos/navigation/test_tf_pose.py
       - dimos/navigation/movement_manager/**
       - dimos/navigation/go2/replanning_a_star/**
@@ -292,3 +296,14 @@ jobs:
         run: |
           .native-review/bin/python -I -c 'from dimos.navigation import tf_pose; from dimos.navigation.movement_manager import movement_manager; from dimos.protocol.tf import tf; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (tf_pose,movement_manager,tf)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/navigation/test_tf_pose.py" "$PWD/dimos/navigation/movement_manager/test_movement_manager.py"
           MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/navigation/tf_pose.py dimos/navigation/movement_manager/movement_manager.py
+      - name: Verify generated trajectory followers and actual Rust controller parity
+        if: runner.os == 'Linux'
+        env:
+          OMP_NUM_THREADS: '2'
+          CARGO_TARGET_DIR: ${{ github.workspace }}/build/rust-controller-target
+        run: |
+          .native-review/bin/python -m pip install 'maturin==1.13.3'
+          .native-review/bin/maturin build --manifest-path dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml --features python --release --locked --jobs 2 --out build/controller-wheels --interpreter .native-review/bin/python
+          .native-review/bin/python -m pip install --no-deps build/controller-wheels/*.whl
+          .native-review/bin/python -I -c 'from dimos.navigation.trajectory_follower.basic import module as basic; from dimos.navigation.trajectory_follower.fancy import module, controller; from dimos.navigation.trajectory_follower.fancy.laws import seed,hinted; from dimos.navigation.local_planner import profile; import dimos_trajectory_follower; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (basic,module,controller,seed,hinted,profile,dimos_trajectory_follower)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/navigation/trajectory_follower/basic/test_module.py" "$PWD/dimos/navigation/trajectory_follower/fancy/test_module.py" "$PWD/dimos/navigation/trajectory_follower/fancy/test_controller.py" "$PWD/dimos/navigation/trajectory_follower/fancy/test_rust_parity.py" "$PWD/dimos/navigation/local_planner/test_profile.py" "$PWD/dimos/navigation/trajectory_follower/test_generated_ports.py"
+          MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/navigation/spec.py dimos/navigation/local_planner/profile.py dimos/navigation/trajectory_follower/basic/module.py dimos/navigation/trajectory_follower/fancy/{controller,module,laws/hinted,laws/seed}.py

dimos/models/embedding/clip.py

@@ -17,12 +17,12 @@
 from functools import cached_property
 from typing import overload
 
-from dimos.messages.sensor_msgs.msg import Image
 from PIL import Image as PILImage
 import torch
 import torch.nn.functional as functional
 from transformers import CLIPModel as HFCLIPModel, CLIPProcessor
 
+from dimos.messages.sensor_msgs.msg import Image
 from dimos.models.base import HuggingFaceModel
 from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig
 from dimos.msgs.image import image_to_rgb

dimos/models/embedding/dino.py

@@ -17,12 +17,12 @@
 from functools import cached_property
 from typing import Any, overload
 
-from dimos.messages.sensor_msgs.msg import Image
 from PIL import Image as PILImage
 import torch
 import torch.nn.functional as functional
 from transformers import AutoImageProcessor, AutoModel
 
+from dimos.messages.sensor_msgs.msg import Image
 from dimos.models.base import HuggingFaceModel
 from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig
 from dimos.msgs.image import image_to_rgb

dimos/models/embedding/siglip.py

@@ -17,12 +17,12 @@
 from functools import cached_property
 from typing import overload
 
-from dimos.messages.sensor_msgs.msg import Image
 from PIL import Image as PILImage
 import torch
 import torch.nn.functional as functional
 from transformers import SiglipModel as HFSiglipModel, SiglipProcessor
 
+from dimos.messages.sensor_msgs.msg import Image
 from dimos.models.base import HuggingFaceModel
 from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig
 from dimos.msgs.image import image_to_rgb

dimos/models/embedding/siglip2.py

@@ -17,12 +17,12 @@
 from functools import cached_property
 from typing import overload
 
-from dimos.messages.sensor_msgs.msg import Image
 from PIL import Image as PILImage
 import torch
 import torch.nn.functional as functional
 from transformers import SiglipModel as HFSiglipModel, SiglipProcessor
 
+from dimos.messages.sensor_msgs.msg import Image
 from dimos.models.base import HuggingFaceModel
 from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig
 from dimos.msgs.image import image_to_rgb

dimos/models/embedding/test_generated_images.py

@@ -29,20 +29,25 @@
 from dimos.msgs.image import image_from_array, image_view
 
 
-@pytest.mark.parametrize("model_type", [CLIPModel,DINOModel,SigLIPModel,SigLIP2Model])
-def test_generated_embeddings_keep_zero_source_time_and_convert_rgb(mocker,model_type):
-    processor,network=mocker.Mock(),mocker.Mock()
-    features=torch.tensor([[3.0,4.0]])
-    processor.return_value=BatchFeature(data={"pixel_values":torch.zeros(1,3,4,8)})
-    network.get_image_features.return_value=features
-    network.return_value=BaseModelOutputWithPooling(last_hidden_state=features[:,None,:],pooler_output=features)
-    mocker.patch.object(model_type,"_processor",new_callable=PropertyMock,return_value=processor)
-    mocker.patch.object(model_type,"_model",new_callable=PropertyMock,return_value=network)
-    image=image_from_array(np.full((4,8,3),[0,0,255],dtype=np.uint8),encoding="bgr8",
-        header=Header(frame_id="camera",stamp=Time(sec=0,nanosec=0)))
-    model=model_type(device="cpu")
-    result=model.embed(image)
+@pytest.mark.parametrize("model_type", [CLIPModel, DINOModel, SigLIPModel, SigLIP2Model])
+def test_generated_embeddings_keep_zero_source_time_and_convert_rgb(mocker, model_type):
+    processor, network = mocker.Mock(), mocker.Mock()
+    features = torch.tensor([[3.0, 4.0]])
+    processor.return_value = BatchFeature(data={"pixel_values": torch.zeros(1, 3, 4, 8)})
+    network.get_image_features.return_value = features
+    network.return_value = BaseModelOutputWithPooling(
+        last_hidden_state=features[:, None, :], pooler_output=features
+    )
+    mocker.patch.object(model_type, "_processor", new_callable=PropertyMock, return_value=processor)
+    mocker.patch.object(model_type, "_model", new_callable=PropertyMock, return_value=network)
+    image = image_from_array(
+        np.full((4, 8, 3), [0, 0, 255], dtype=np.uint8),
+        encoding="bgr8",
+        header=Header(frame_id="camera", stamp=Time(sec=0, nanosec=0)),
+    )
+    model = model_type(device="cpu")
+    result = model.embed(image)
     assert result.timestamp == 0.0
-    torch.testing.assert_close(result.vector,torch.tensor([0.6,0.8]))
-    assert processor.call_args.kwargs["images"][0].getpixel((0,0)) == (255,0,0)
-    np.testing.assert_array_equal(image_view(image)[0,0],[0,0,255])
+    torch.testing.assert_close(result.vector, torch.tensor([0.6, 0.8]))
+    assert processor.call_args.kwargs["images"][0].getpixel((0, 0)) == (255, 0, 0)
+    np.testing.assert_array_equal(image_view(image)[0, 0], [0, 0, 255])

dimos/models/embedding/treid.py

@@ -19,11 +19,11 @@
 
 from functools import cached_property
 
-from dimos.messages.sensor_msgs.msg import Image
 import torch
 import torch.nn.functional as functional
 from torchreid import utils as torchreid_utils
 
+from dimos.messages.sensor_msgs.msg import Image
 from dimos.models.base import LocalModel
 from dimos.models.embedding.base import Embedding, EmbeddingModel, EmbeddingModelConfig
 from dimos.msgs.image import image_to_bgr

dimos/navigation/local_planner/profile.py

@@ -25,7 +25,8 @@
 import numpy as np
 from numpy.typing import NDArray
 
-from dimos.msgs.nav_msgs.Path import Path
+from dimos.messages.nav_msgs.msg import Path
+from dimos.msgs.time import to_nanoseconds
 from dimos.navigation.embodiment.base import Embodiment
 
 _FAN_EPS = 1e-6  # segment shorter than this is a rotation, not a move
@@ -40,11 +41,11 @@ def decode_ceilings(path: Path, lo: float, hi: float) -> NDArray[np.float64] | N
     n = len(path.poses)
     if n < 2:
         return None
-    ts = np.array([p.ts for p in path.poses])
-    dt = np.diff(ts)
+    ts = np.array([to_nanoseconds(p.header.stamp) for p in path.poses], dtype=np.int64)
+    dt = np.diff(ts) / 1_000_000_000
     if np.any(dt < 0) or not np.any(dt > 0):
         return None
-    xy = np.array([[p.position.x, p.position.y] for p in path.poses]).reshape(-1, 2)
+    xy = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]).reshape(-1, 2)
     ds = np.linalg.norm(np.diff(xy, axis=0), axis=1)
     # a config is free to set min above max; ordering here rather than
     # trusting the caller keeps the clip a clip

dimos/navigation/local_planner/test_profile.py

@@ -14,10 +14,11 @@
 
 import numpy as np
 
-from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-from dimos.msgs.geometry_msgs.Quaternion import Quaternion
-from dimos.msgs.geometry_msgs.Vector3 import Vector3
-from dimos.msgs.nav_msgs.Path import Path
+from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped
+from dimos.messages.nav_msgs.msg import Path
+from dimos.messages.std_msgs.msg import Header
+from dimos.msgs.geometry import quaternion_from_euler
+from dimos.msgs.time import time_from_nanoseconds, time_from_seconds
 from dimos.navigation.embodiment.go2 import GO2
 from dimos.navigation.local_planner.profile import decode_ceilings
 
@@ -26,29 +27,39 @@
 
 def _pose(x: float, y: float, yaw: float = 0.0, ts: float = 0.0) -> PoseStamped:
     return PoseStamped(
-        ts=ts,
-        frame_id="odom",
-        position=Vector3(x, y, 0.0),
-        orientation=Quaternion.from_euler(Vector3(0, 0, yaw)),
+        header=Header(frame_id="odom", stamp=time_from_seconds(ts)),
+        pose=Pose(position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0, 0, yaw)),
     )
 
 
 def _path(n: int = 21, step: float = 0.1) -> Path:
-    return Path(frame_id="odom", poses=[_pose(i * step, 0.0, ts=1.0) for i in range(n)])
+    return Path(
+        header=Header(frame_id="odom", stamp=time_from_seconds(0.0)),
+        poses=[_pose(i * step, 0.0, ts=1.0) for i in range(n)],
+    )
 
 
 def _stamp(path: Path, dts: list[float], t0: float = 0.0) -> Path:
     t = t0
-    path.poses[0].ts = t
+    path.poses[0].header.stamp = time_from_seconds(t)
     for pose, dt in zip(path.poses[1:], dts, strict=True):
         t += dt
-        pose.ts = t
+        pose.header.stamp = time_from_seconds(t)
     return path
 
 
 def test_ceiling_is_segment_length_over_stamp_delta() -> None:
     speeds = np.linspace(MIN_SPEED, MAX_SPEED, 20)
-    path = _stamp(_path(), [0.1 / v for v in speeds])
+    # Keep a varied speed sweep while using exactly representable ROS time deltas.
+    # The old 0.1 / v fixture required sub-nanosecond timestamp precision.
+    positions = np.concatenate([[0.0], np.cumsum(speeds * 0.5)])
+    path = _stamp(
+        Path(
+            header=Header(frame_id="odom", stamp=time_from_seconds(0.0)),
+            poses=[_pose(float(x), 0.0) for x in positions],
+        ),
+        [0.5] * 20,
+    )
     ceilings = decode_ceilings(path, MIN_SPEED, MAX_SPEED)
     assert ceilings is not None
     # waypoint i carries segment i-1
@@ -62,16 +73,19 @@ def test_unstamped_path_decodes_none() -> None:
 
 def test_non_monotone_decodes_none() -> None:
     path = _stamp(_path(), [0.1 / MAX_SPEED] * 20)
-    path.poses[5].ts = 999.0
+    path.poses[5].header.stamp = time_from_seconds(999.0)
     assert decode_ceilings(path, MIN_SPEED, MAX_SPEED) is None
 
 
 def test_fan_inherits_ceiling() -> None:
-    poses = [_pose(0.0, 0.0, 0.0), _pose(0.1, 0.0, 0.0)]
-    poses += [_pose(0.1, 0.0, yaw) for yaw in (0.3, 0.6, 0.9)]  # rotation in place
-    poses += [_pose(0.2, 0.0, 0.9)]
+    poses = [_pose(0.0, 0.0, 0.0), _pose(MAX_SPEED * 0.1, 0.0, 0.0)]
+    poses += [_pose(MAX_SPEED * 0.1, 0.0, yaw) for yaw in (0.3, 0.6, 0.9)]  # rotation in place
+    poses += [_pose(MAX_SPEED * 0.2, 0.0, 0.9)]
     fan_dt = 0.3 / GO2.max_yaw_rate
-    path = _stamp(Path(frame_id="odom", poses=poses), [0.1 / MAX_SPEED] + [fan_dt] * 3 + [0.5])
+    path = _stamp(
+        Path(header=Header(frame_id="odom", stamp=time_from_seconds(0.0)), poses=poses),
+        [0.1] + [fan_dt] * 3 + [0.5],
+    )
     ceilings = decode_ceilings(path, MIN_SPEED, MAX_SPEED)
     assert ceilings is not None
     assert ceilings[3] == MAX_SPEED  # fan inherits, does not read as slow motion
@@ -86,8 +100,22 @@ def test_decode_clips_to_safe_band() -> None:
 
 
 def test_empty_and_single_pose() -> None:
-    assert decode_ceilings(Path(frame_id="odom", poses=[]), MIN_SPEED, MAX_SPEED) is None
-    assert decode_ceilings(Path(frame_id="odom", poses=[_pose(0, 0)]), MIN_SPEED, MAX_SPEED) is None
+    assert (
+        decode_ceilings(
+            Path(header=Header(frame_id="odom", stamp=time_from_seconds(0.0)), poses=[]),
+            MIN_SPEED,
+            MAX_SPEED,
+        )
+        is None
+    )
+    assert (
+        decode_ceilings(
+            Path(header=Header(frame_id="odom", stamp=time_from_seconds(0.0)), poses=[_pose(0, 0)]),
+            MIN_SPEED,
+            MAX_SPEED,
+        )
+        is None
+    )
 
 
 def test_stall_never_reads_as_schedule() -> None:
@@ -97,3 +125,12 @@ def test_stall_never_reads_as_schedule() -> None:
     ca, cb = decode_ceilings(a, MIN_SPEED, MAX_SPEED), decode_ceilings(b, MIN_SPEED, MAX_SPEED)
     assert ca is not None and cb is not None
     np.testing.assert_allclose(ca, cb)
+
+
+def test_exact_epoch_nanosecond_deltas_drive_the_profile_without_float_loss():
+    path = _path(n=3, step=0.1)
+    base = 1700000000123456789
+    for pose, delta in zip(path.poses, [0, 200000000, 700000000], strict=True):
+        pose.header.stamp = time_from_nanoseconds(base + delta)
+    ceilings = decode_ceilings(path, MIN_SPEED, MAX_SPEED)
+    np.testing.assert_allclose(ceilings, [0.5, 0.5, 0.2], rtol=0, atol=1e-15)

dimos/navigation/spec.py

@@ -26,11 +26,10 @@
 from typing import Protocol
 
 from dimos.core.stream import IO, In, Out
-from dimos.msgs.geometry_msgs.PointStamped import PointStamped
-from dimos.msgs.geometry_msgs.Twist import Twist
-from dimos.msgs.nav_msgs.Path import Path
-from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.messages.geometry_msgs.msg import PointStamped, Twist
+from dimos.messages.nav_msgs.msg import Path
+from dimos.messages.sensor_msgs.msg import PointCloud2
+from dimos.messages.tf2_msgs.msg import TFMessage
 
 
 class GlobalPlanner(Protocol):

dimos/navigation/trajectory_follower/basic/module.py

@@ -19,7 +19,6 @@
 import time
 from typing import Any
 
-from dimos_lcm.std_msgs import Bool
 import numpy as np
 from numpy.typing import NDArray
 from reactivex.disposable import Disposable
@@ -28,12 +27,13 @@
 from dimos.core.core import rpc
 from dimos.core.module import Module, ModuleConfig
 from dimos.core.stream import IO, In, Out
-from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-from dimos.msgs.geometry_msgs.Twist import Twist
-from dimos.msgs.geometry_msgs.Vector3 import Vector3
-from dimos.msgs.nav_msgs.Path import Path
-from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.messages.geometry_msgs.msg import PoseStamped, Twist, Vector3
+from dimos.messages.nav_msgs.msg import Path
+from dimos.messages.std_msgs.msg import Bool
+from dimos.messages.tf2_msgs.msg import TFMessage
+from dimos.msgs.geometry import yaw
 from dimos.navigation import spec
+from dimos.navigation.go2.replanning_a_star.controllers import _velocity
 from dimos.utils.logging_config import setup_logger
 from dimos.utils.trigonometry import angle_diff
 
@@ -103,7 +103,7 @@ def stop(self) -> None:
         self._stop_event.set()
         if self._thread is not None:
             self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT)
-        self.nav_cmd_vel.publish(Twist())
+        self.nav_cmd_vel.publish(_velocity())
         super().stop()
 
     def _lookup_pose(self) -> PoseStamped | None:
@@ -120,17 +120,19 @@ def _on_path(self, path: Path) -> None:
         if len(path.poses) == 0:
             with self._lock:
                 self._waypoints = None
-            self.nav_cmd_vel.publish(Twist())
+            self.nav_cmd_vel.publish(_velocity())
             return
-        waypoints = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=np.float32)
+        waypoints = np.array(
+            [[p.pose.position.x, p.pose.position.y] for p in path.poses], dtype=np.float32
+        )
         with self._lock:
             self._waypoints = waypoints
 
     def _on_stop(self, msg: Bool) -> None:
         if msg.data:
             with self._lock:
                 self._waypoints = None
-            self.nav_cmd_vel.publish(Twist())
+            self.nav_cmd_vel.publish(_velocity())
 
     def _follow(self) -> None:
         period = 1.0 / self.config.control_frequency
@@ -145,20 +147,20 @@ def _follow(self) -> None:
             self._stop_event.wait(max(0.0, period - elapsed))
 
     def _step(self, pose: PoseStamped, waypoints: NDArray[np.float32]) -> None:
-        position = np.array([pose.position.x, pose.position.y], dtype=np.float32)
+        position = np.array([pose.pose.position.x, pose.pose.position.y], dtype=np.float32)
         if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance:
-            self.nav_cmd_vel.publish(Twist())
+            self.nav_cmd_vel.publish(_velocity())
             with self._lock:
                 if self._waypoints is waypoints:
                     self._waypoints = None
-            self.goal_reached.publish(Bool(True))
+            self.goal_reached.publish(Bool(data=True))
             logger.info("Goal reached")
             return
 
         target = self._lookahead_point(waypoints, position)
         yaw_error = angle_diff(
             math.atan2(target[1] - position[1], target[0] - position[0]),
-            pose.orientation.euler[2],
+            yaw(pose.pose.orientation),
         )
 
         angular = max(