From 025153cf3c67d062c2ce9ed8916055a114570c65 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 19 Oct 2026 22:40:40 -0700 Subject: [PATCH 1/2] refactor: migrate odometry history and Path rendering to CDR --- .github/workflows/message-codegen.yml | 7 + dimos/mapping/odometry_hist.py | 52 +++-- dimos/mapping/test_generated_odometry_hist.py | 205 ++++++++++++++++++ dimos/visualization/rerun/cdr.py | 25 ++- 4 files changed, 265 insertions(+), 24 deletions(-) create mode 100644 dimos/mapping/test_generated_odometry_hist.py diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index 13679a52c11..2fc79174369 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -21,6 +21,8 @@ on: - dimos/memory/** - dimos/mapping/occupancy/** - dimos/mapping/pointclouds/occupancy.py + - dimos/mapping/odometry_hist.py + - dimos/mapping/test_generated_odometry_hist.py - dimos/mapping/costmapper.py - dimos/mapping/test_generated_costmapper.py - dimos/protocol/cdr_mcap.py @@ -239,3 +241,8 @@ jobs: .native-review/bin/python -m pip install 'numba==0.68.0' .native-review/bin/python -I -c 'import dimos.mapping.costmapper as module; import dimos.mapping.pointclouds.occupancy as algorithms; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (module, algorithms)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -o 'markers=self_hosted: optional project fixture case' --strict-markers -m 'not self_hosted' -q "$PWD/dimos/mapping/test_generated_costmapper.py" MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/pointclouds/occupancy.py dimos/mapping/costmapper.py + - name: Verify generated odometry history, exact stamps and Path rendering + if: runner.os == 'Linux' + run: | + .native-review/bin/python -I -c 'import dimos.mapping.odometry_hist as history; import dimos.visualization.rerun.cdr as viewer; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (history, viewer)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/test_generated_odometry_hist.py" "$PWD/dimos/visualization/rerun/test_cdr.py" + MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/odometry_hist.py dimos/visualization/rerun/cdr.py diff --git a/dimos/mapping/odometry_hist.py b/dimos/mapping/odometry_hist.py index f368a7fed1b..1509245bbea 100644 --- a/dimos/mapping/odometry_hist.py +++ b/dimos/mapping/odometry_hist.py @@ -15,6 +15,7 @@ from __future__ import annotations from collections import deque +from copy import deepcopy import math from typing import Any @@ -22,13 +23,17 @@ 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.nav_msgs.Path import Path +from dimos.messages.geometry_msgs.msg import PoseStamped +from dimos.messages.nav_msgs.msg import Odometry, Path +from dimos.msgs.nav_msgs.Path import Path as LegacyPath +from dimos.msgs.time import to_nanoseconds +from dimos.visualization.rerun.cdr import path_archetype -def path_at_true_height(path: Path) -> Any: +def path_at_true_height(path: Path | LegacyPath) -> Any: """The default z lift clears a costmap a bare odometry demo has none of.""" + if isinstance(path, Path): + return path_archetype(path, z_offset=0.0, radii=0.02) return path.to_rerun(z_offset=0.0, radii=0.02) @@ -50,37 +55,38 @@ class OdometryHist(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._poses: deque[PoseStamped] = deque(maxlen=self.config.max_poses) - # None, not 0.0: a recording stamped from zero would suppress the first path. - self._last_publish_ts: float | None = None + # None distinguishes an unpublished history from a recording starting at zero. + self._last_publish_ns: int | None = None self._unpublished = False async def handle_odometry(self, msg: Odometry) -> None: - position = msg.pose.position + position = msg.pose.pose.position + stamp_ns = to_nanoseconds(msg.header.stamp) point = (position.x, position.y, position.z) - frame_id = self.config.frame_id or msg.frame_id + frame_id = self.config.frame_id or msg.header.frame_id previous = self._poses[-1] if self._poses else None if ( previous is None - or math.dist((previous.x, previous.y, previous.z), point) >= self.config.min_step_meters - ): - orientation = msg.pose.orientation - self._poses.append( - PoseStamped( - ts=msg.ts, - frame_id=frame_id, - position=list(point), - orientation=[orientation.x, orientation.y, orientation.z, orientation.w], - ) + or math.dist( + (previous.pose.position.x, previous.pose.position.y, previous.pose.position.z), + point, ) + >= self.config.min_step_meters + ): + header = deepcopy(msg.header) + header.frame_id = frame_id + self._poses.append(PoseStamped(header=header, pose=deepcopy(msg.pose.pose))) self._unpublished = True if not self._unpublished: return - if self._last_publish_ts is not None: - elapsed = msg.ts - self._last_publish_ts + if self._last_publish_ns is not None: + elapsed = stamp_ns - self._last_publish_ns # A replay restart moves the stamp backwards; publish rather than wait it out. - if 0.0 <= elapsed < self.config.min_publish_interval_seconds: + if 0 <= elapsed < self.config.min_publish_interval_seconds * 1_000_000_000: return - self._last_publish_ts = msg.ts + self._last_publish_ns = stamp_ns self._unpublished = False - self.odom_hist.publish(Path(ts=msg.ts, frame_id=frame_id, poses=list(self._poses))) + header = deepcopy(msg.header) + header.frame_id = frame_id + self.odom_hist.publish(Path(header=header, poses=deepcopy(list(self._poses)))) diff --git a/dimos/mapping/test_generated_odometry_hist.py b/dimos/mapping/test_generated_odometry_hist.py new file mode 100644 index 00000000000..8a8ff882d85 --- /dev/null +++ b/dimos/mapping/test_generated_odometry_hist.py @@ -0,0 +1,205 @@ +# 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. + +import asyncio +from contextlib import ExitStack +import json +from threading import Event + +import numpy as np +import pytest + +from dimos.core.cdr_transport import CdrTransport +from dimos.mapping.odometry_hist import OdometryHist, path_at_true_height +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos.messages.nav_msgs.msg import Odometry, Path +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as LegacyPoseStamped +from dimos.msgs.nav_msgs.Path import Path as LegacyPath +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.rpc.pubsubrpc import LCMRPC +from dimos.visualization.rerun.cdr import generated_archetypes, path_archetype + + +class MemoryRPC(LCMRPC): + """Use real RPC and lifecycle code without an external network.""" + + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + +def odometry(ns, x): + return Odometry( + header=Header(frame_id="odom", stamp=time_from_nanoseconds(ns)), + child_frame_id="base_link", + pose=PoseWithCovariance( + pose=Pose(position=Point(x=x, y=2, z=3), orientation=Quaternion(w=1, x=0, y=0, z=0)), + covariance=np.zeros(36, dtype=np.float64), + ), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), + covariance=np.zeros(36, dtype=np.float64), + ), + ) + + +def _send(module, message): + assert module._loop is not None + asyncio.run_coroutine_threadsafe(module.handle_odometry(message), module._loop).result( + timeout=5 + ) + + +@pytest.fixture +def history_factory(): + with ExitStack() as cleanup: + + def create(**kwargs): + wire = CdrTransport("odometry_history", Path, backend="lcm", url="memq://") + cleanup.callback(wire.stop) + module = OdometryHist(rpc_transport=MemoryRPC, **kwargs) + cleanup.callback(module.stop) + module.odom_hist.transport = wire + ready = Event() + received = [] + + def receive(value): + received.append(value) + ready.set() + + cleanup.callback(wire.subscribe(receive)) + return module, received, ready + + yield create, cleanup + + +@pytest.mark.parametrize("base", [0, 1700000000123456789]) +def test_interval_queue_pose_stamps_replay_restart_and_bound_are_preserved(history_factory, base): + create, _ = history_factory + module, received, ready = create(max_poses=3) + + _send(module, odometry(base, 0)) + assert ready.wait(5) + assert to_nanoseconds(received[-1].header.stamp) == base + ready.clear() + _send(module, odometry(base + 50000000, 0.1)) + assert len(received) == 1 + _send(module, odometry(base + 100000000, 0.1)) + assert ready.wait(5) + assert len(received) == 2 + assert to_nanoseconds(received[-1].header.stamp) == base + 100000000 + assert [to_nanoseconds(value.header.stamp) for value in received[-1].poses] == [ + base, + base + 50000000, + ] + ready.clear() + _send(module, odometry(base + 100000001, 0.2)) + assert len(received) == 2 + _send(module, odometry(base - 1, 0.3)) + assert ready.wait(5) + assert len(received) == 3 + assert len(received[-1].poses) == 3 + assert to_nanoseconds(received[-1].header.stamp) == base - 1 + assert [to_nanoseconds(value.header.stamp) for value in received[-1].poses] == [ + base + 50000000, + base + 100000001, + base - 1, + ] + + +@pytest.mark.parametrize("override,expected", [("", "odom"), ("map", "map")]) +def test_snapshot_ownership_step_threshold_and_frame_override(history_factory, override, expected): + create, _ = history_factory + module, received, ready = create(frame_id=override, min_publish_interval_seconds=0) + original = odometry(1700000000123456789, 0) + + _send(module, original) + assert ready.wait(5) + first = received[-1] + first.poses[0].pose.position.x = 99 + original.pose.pose.position.x = 88 + ready.clear() + _send(module, odometry(1700000000123456790, 0.01)) + assert len(received) == 1 + _send(module, odometry(1700000000123456791, 0.1)) + assert ready.wait(5) + + assert len(received) == 2 + assert [pose.pose.position.x for pose in received[-1].poses] == [0, 0.1] + assert received[-1].header.frame_id == expected + assert [pose.header.frame_id for pose in received[-1].poses] == [expected, expected] + assert to_nanoseconds(received[-1].header.stamp) == 1700000000123456791 + + +def test_real_input_port_dispatches_generated_odometry_into_cdr_path(history_factory): + create, cleanup = history_factory + incoming = CdrTransport("history_input", Odometry, backend="lcm", url="memq://") + cleanup.callback(incoming.stop) + module, received, ready = create() + module.odometry.transport = incoming + assert module.odometry.type is Odometry + assert module.odom_hist.type is Path + module.start() + source = odometry(1700000000123456789, 1) + + incoming.broadcast(None, source) + + assert ready.wait(5) + assert len(received) == 1 + assert received[0].header == source.header + assert received[0].poses[0].pose == source.pose.pose + + +def test_generated_path_rendering_retains_geometry_frame_and_exact_fields(): + header = Header(frame_id="map", stamp=time_from_nanoseconds(1700000000123456789)) + path = Path( + header=header, + poses=[ + PoseStamped( + header=header, + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(w=1, x=0, y=0, z=0) + ), + ) + ], + ) + + entries = generated_archetypes(path, "world/path") + + assert [name for name, _ in entries] == ["world/path", "world/path", "world/path/message"] + line, frame, text = [value for _, value in entries] + assert line.strips.as_arrow_array().to_pylist() == [[[1.0, 2.0, 3.5]]] + assert frame.parent_frame.as_arrow_array().to_pylist() == ["tf#/map"] + fields = json.loads(text.text.as_arrow_array().to_pylist()[0]) + assert fields["header"]["stamp"]["nanosec"] == 123456789 + assert fields["poses"][0]["pose"]["position"] == {"x": 1, "y": 2, "z": 3} + assert path_at_true_height(path).strips.as_arrow_array().to_pylist() == [[[1.0, 2.0, 3.0]]] + legacy = LegacyPath( + ts=0, + frame_id="map", + poses=[ + LegacyPoseStamped(position=[1, 2, 3], orientation=[0, 0, 0, 1], ts=0, frame_id="map") + ], + ) + assert path_at_true_height(legacy).strips.as_arrow_array().to_pylist() == [[[1.0, 2.0, 3.0]]] + assert path_archetype(Path(header=header, poses=[])).strips.as_arrow_array().to_pylist() == [] diff --git a/dimos/visualization/rerun/cdr.py b/dimos/visualization/rerun/cdr.py index 548402f831d..746e79c23fe 100644 --- a/dimos/visualization/rerun/cdr.py +++ b/dimos/visualization/rerun/cdr.py @@ -24,6 +24,7 @@ from dimos.messages.dimos_msgs.msg import EntityMarkers from dimos.messages.geometry_msgs.msg import PointStamped, PoseStamped +from dimos.messages.nav_msgs.msg import Path from dimos.messages.sensor_msgs.msg import CameraInfo, CompressedImage, Image, PointCloud2 from dimos.msgs.image import image_from_compressed, image_to_rgb, image_view from dimos.msgs.pointcloud import pointcloud_rgb, pointcloud_xyz @@ -119,6 +120,24 @@ def entity_points(message: EntityMarkers) -> rr.Points3D: ) +def path_archetype( + message: Path, + *, + color: tuple[int, int, int] = (0, 255, 128), + z_offset: float = 0.5, + radii: float = 0.05, +) -> rr.LineStrips3D: + import rerun as rr + + if not message.poses: + return rr.LineStrips3D([]) + points = [ + [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + z_offset] + for pose in message.poses + ] + return rr.LineStrips3D([points], colors=[color], radii=radii) + + def generated_archetypes(message: Any, entity: str) -> RerunMulti | None: """Render stamped geometry and its exact fields; other domains remain explicit.""" import rerun as rr @@ -132,7 +151,11 @@ def generated_archetypes(message: Any, entity: str) -> RerunMulti | None: if frame: entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{frame}"))) return entries - if type(message) is EntityMarkers: + if type(message) is Path: + entries = [(entity, path_archetype(message))] + if message.header.frame_id: + entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{message.header.frame_id}"))) + elif type(message) is EntityMarkers: entries = [(entity, entity_points(message))] if message.header.frame_id: entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{message.header.frame_id}"))) From 6d5170d41f8032455229a08a7abc98faa849369f Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 20 Oct 2026 05:42:03 +0000 Subject: [PATCH 2/2] [autofix.ci] apply automated fixes --- dimos/models/embedding/clip.py | 2 +- dimos/models/embedding/dino.py | 2 +- dimos/models/embedding/siglip.py | 2 +- dimos/models/embedding/siglip2.py | 2 +- .../models/embedding/test_generated_images.py | 37 +++++++++++-------- dimos/models/embedding/treid.py | 2 +- 6 files changed, 26 insertions(+), 21 deletions(-) diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index ad5e3e0cc6c..f429dc62a93 100644 --- a/dimos/models/embedding/clip.py +++ b/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 diff --git a/dimos/models/embedding/dino.py b/dimos/models/embedding/dino.py index 5b0d24fd137..022a442ebf7 100644 --- a/dimos/models/embedding/dino.py +++ b/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 diff --git a/dimos/models/embedding/siglip.py b/dimos/models/embedding/siglip.py index bb0ed762943..8cabb1afdf4 100644 --- a/dimos/models/embedding/siglip.py +++ b/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 diff --git a/dimos/models/embedding/siglip2.py b/dimos/models/embedding/siglip2.py index f8f1fe8861b..c85932f9176 100644 --- a/dimos/models/embedding/siglip2.py +++ b/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 diff --git a/dimos/models/embedding/test_generated_images.py b/dimos/models/embedding/test_generated_images.py index e3c1245ab07..a51f9778ba4 100644 --- a/dimos/models/embedding/test_generated_images.py +++ b/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]) diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index ce36b5d004b..3ce7331eec3 100644 --- a/dimos/models/embedding/treid.py +++ b/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