From 1ccba98ba27f7ad874f4ed080f1f60512099f453 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 23 Oct 2026 02:11:21 -0700 Subject: [PATCH 1/3] fix(tf): isolate exact timestamp retention for generated transforms --- dimos/protocol/tf/buffer.py | 83 +++++++++++++++++++++++++++++ dimos/protocol/tf/test_buffer.py | 91 ++++++++++++++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 dimos/protocol/tf/buffer.py create mode 100644 dimos/protocol/tf/test_buffer.py diff --git a/dimos/protocol/tf/buffer.py b/dimos/protocol/tf/buffer.py new file mode 100644 index 00000000000..9852a5d1a79 --- /dev/null +++ b/dimos/protocol/tf/buffer.py @@ -0,0 +1,83 @@ +# 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. + + +"""Bounded timestamp index for generated transforms; frame search stays in TF.""" + +from copy import deepcopy +import math +from typing import cast + +from sortedcontainers import SortedDict # type: ignore[import-untyped] + +from dimos.message_utils.time import time_from_seconds, to_nanoseconds, to_seconds +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.types.timestamped import to_human_readable + + +class TBuffer: + """A bounded time index of generated transforms, keyed by integer nanoseconds.""" + + def __init__(self, buffer_size: float = 10.0) -> None: + if math.isnan(buffer_size) or buffer_size <= 0: + raise ValueError("buffer_size must be positive") + self.buffer_size = buffer_size + self._entries: SortedDict = SortedDict() + + def __len__(self) -> int: + return len(self._entries) + + def add(self, transform: TransformStamped) -> None: + stamp = to_nanoseconds(transform.header.stamp) + self._entries[stamp] = deepcopy(transform) + if math.isfinite(self.buffer_size): + oldest = self._entries.peekitem(-1)[0] - round(self.buffer_size * 1_000_000_000) + while self._entries and self._entries.peekitem(0)[0] < oldest: + self._entries.popitem(0) + + def first(self) -> TransformStamped | None: + return deepcopy(self._entries.peekitem(0)[1]) if self._entries else None + + def last(self) -> TransformStamped | None: + return deepcopy(self._entries.peekitem(-1)[1]) if self._entries else None + + def get( + self, time_point: float | None = None, time_tolerance: float = 1.0 + ) -> TransformStamped | None: + if time_point is None: + return self.last() + stamp = to_nanoseconds(time_from_seconds(time_point)) + if math.isnan(time_tolerance) or time_tolerance < 0: + raise ValueError("time_tolerance must be nonnegative") + index = self._entries.bisect_left(stamp) + candidates = [ + self._entries.peekitem(i) for i in (index - 1, index) if 0 <= i < len(self._entries) + ] + if not candidates: + return None + nearest, result = min(candidates, key=lambda item: (abs(item[0] - stamp), -item[0])) + if abs(nearest - stamp) / 1_000_000_000 > time_tolerance: + return None + return deepcopy(cast("TransformStamped", result)) + + def __str__(self) -> str: + first, last = self.first(), self.last() + if first is None or last is None: + return "TBuffer(empty)" + start, end = to_seconds(first.header.stamp), to_seconds(last.header.stamp) + return ( + f"TBuffer({first.header.frame_id} -> {first.child_frame_id}, " + f"{len(self)} msgs, {end - start:.2f}s " + f"[{to_human_readable(start)} - {to_human_readable(end)}])" + ) diff --git a/dimos/protocol/tf/test_buffer.py b/dimos/protocol/tf/test_buffer.py new file mode 100644 index 00000000000..224df3b9556 --- /dev/null +++ b/dimos/protocol/tf/test_buffer.py @@ -0,0 +1,91 @@ +# 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. + + +"""Timestamp retention and ownership, independently of transport/graph migration.""" + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.protocol.tf.buffer import TBuffer + + +def test_buffer_retains_distinct_nanoseconds_and_owns_its_snapshots(): + buffer = TBuffer() + first = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456789)), + child_frame_id="b", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + second = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456790)), + child_frame_id="b", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + buffer.add(first) + buffer.add(second) + first.header.stamp.nanosec = 0 + assert len(buffer) == 2 + stored_first = buffer.first() + assert stored_first is not None + assert stored_first.header.stamp.nanosec == 123456789 + stored_first.header.stamp.nanosec = 1 + unchanged = buffer.first() + assert unchanged is not None + assert unchanged.header.stamp.nanosec == 123456789 + latest = buffer.get() + assert latest is not None + assert latest.header.stamp.nanosec == 123456790 + + +def test_buffer_drops_out_of_order_data_older_than_retained_window(): + buffer = TBuffer(buffer_size=1) + for sec in (10, 11, 1): + buffer.add( + TransformStamped( + header=Header(stamp=Time(sec=sec, nanosec=0), frame_id=""), + child_frame_id="", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + ) + assert len(buffer) == 2 + first = buffer.first() + assert first is not None + assert first.header.stamp.sec == 10 + + +def test_buffer_preserves_negative_stamp(): + buffer = TBuffer() + buffer.add( + TransformStamped( + header=Header(stamp=Time(sec=-1, nanosec=500000000), frame_id=""), + child_frame_id="", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + ) + result = buffer.get(time_point=-0.5, time_tolerance=0) + assert result is not None + assert result.header.stamp == Time(sec=-1, nanosec=500000000) From efdebb4e287bd81dbf5c7875b7cb322f568e07ba Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 23 Oct 2026 02:15:46 -0700 Subject: [PATCH 2/3] ci(cdr): run this adapter layer tests before SDK checks --- .github/workflows/cdr-native.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/cdr-native.yml b/.github/workflows/cdr-native.yml index 21b5b1eed64..bf99ece4e7b 100644 --- a/.github/workflows/cdr-native.yml +++ b/.github/workflows/cdr-native.yml @@ -40,7 +40,8 @@ jobs: dimos/message_utils/test_time.py \ dimos/message_utils/test_geometry.py \ dimos/message_utils/test_pointcloud.py \ - dimos/message_utils/test_pointcloud_colors.py + dimos/message_utils/test_pointcloud_colors.py \ + dimos/protocol/tf/test_buffer.py - name: Test native SDK run: | cargo test --locked --manifest-path native/rust/dimos-module/Cargo.toml From 03c2c5114dc845d66ceb5d56b8a1d2e312e27117 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Mon, 26 Oct 2026 19:11:11 +0000 Subject: [PATCH 3/3] [autofix.ci] apply automated fixes --- dimos/protocol/tf/buffer.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/protocol/tf/buffer.py b/dimos/protocol/tf/buffer.py index f94b1a58d0c..51c00d95adb 100644 --- a/dimos/protocol/tf/buffer.py +++ b/dimos/protocol/tf/buffer.py @@ -75,7 +75,10 @@ def __str__(self) -> str: first, last = self.first(), self.last() if first is None or last is None: return "TBuffer(empty)" - start, end = message_time.to_seconds(first.header.stamp), message_time.to_seconds(last.header.stamp) + start, end = ( + message_time.to_seconds(first.header.stamp), + message_time.to_seconds(last.header.stamp), + ) return ( f"TBuffer({first.header.frame_id} -> {first.child_frame_id}, " f"{len(self)} msgs, {end - start:.2f}s "