From 2015f7b0e3b2b19a5002d2e21603d9b3850c5b54 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 19 Oct 2026 23:22:14 -0700 Subject: [PATCH 1/2] refactor: follow generated CDR trajectories with Python control laws --- .github/workflows/message-codegen.yml | 15 ++ dimos/navigation/local_planner/profile.py | 9 +- .../navigation/local_planner/test_profile.py | 75 +++++++--- dimos/navigation/spec.py | 9 +- .../trajectory_follower/basic/module.py | 30 ++-- .../trajectory_follower/basic/test_module.py | 137 +++++++----------- .../trajectory_follower/fancy/controller.py | 9 +- .../trajectory_follower/fancy/laws/hinted.py | 31 ++-- .../trajectory_follower/fancy/laws/seed.py | 21 +-- .../trajectory_follower/fancy/module.py | 34 +++-- .../fancy/test_controller.py | 33 +++-- .../trajectory_follower/fancy/test_module.py | 65 ++++++--- .../fancy/test_rust_parity.py | 45 +++--- .../test_generated_ports.py | 121 ++++++++++++++++ 14 files changed, 416 insertions(+), 218 deletions(-) create mode 100644 dimos/navigation/trajectory_follower/test_generated_ports.py diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index 6f1c79a495a..16b4404637a 100644 --- a/.github/workflows/message-codegen.yml +++ b/.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 diff --git a/dimos/navigation/local_planner/profile.py b/dimos/navigation/local_planner/profile.py index e2a2a0a442c..7e7733d57e2 100644 --- a/dimos/navigation/local_planner/profile.py +++ b/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 diff --git a/dimos/navigation/local_planner/test_profile.py b/dimos/navigation/local_planner/test_profile.py index a3c19e354eb..2268ffa63fa 100644 --- a/dimos/navigation/local_planner/test_profile.py +++ b/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) diff --git a/dimos/navigation/spec.py b/dimos/navigation/spec.py index 6f16a7c5964..c7eeac92773 100644 --- a/dimos/navigation/spec.py +++ b/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): diff --git a/dimos/navigation/trajectory_follower/basic/module.py b/dimos/navigation/trajectory_follower/basic/module.py index 9b8ed5dd5c0..d58b0f32cc6 100644 --- a/dimos/navigation/trajectory_follower/basic/module.py +++ b/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,9 +120,11 @@ 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 @@ -130,7 +132,7 @@ 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( diff --git a/dimos/navigation/trajectory_follower/basic/test_module.py b/dimos/navigation/trajectory_follower/basic/test_module.py index 9cf403aba33..56c37d094df 100644 --- a/dimos/navigation/trajectory_follower/basic/test_module.py +++ b/dimos/navigation/trajectory_follower/basic/test_module.py @@ -12,103 +12,78 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +import pytest + +from dimos.core.cdr_transport import CdrTransport +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.time import time_from_seconds from dimos.navigation.trajectory_follower.basic.module import BasicPathFollower, lookahead_distance -from dimos.protocol.tf.tf import MultiTBuffer +from dimos.protocol.rpc.pubsubrpc import LCMRPC MOUNT_Z = 0.163 -class FakeTF(MultiTBuffer): - """In-memory tf with the dispose() hook and call counter the module tests need.""" - - def __init__(self) -> None: - super().__init__() - self.gets = 0 - - def get( - self, - parent_frame: str, - child_frame: str, - time_point: float | None = None, - time_tolerance: float | None = None, - *, - forward_tolerance: float = 0.0, - ) -> Transform | None: - self.gets += 1 - return super().get( - parent_frame, - child_frame, - time_point, - time_tolerance, - forward_tolerance=forward_tolerance, - ) - - def dispose(self) -> None: - pass - - -def _mount() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, MOUNT_Z), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", +class MemoryRPC(LCMRPC): + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + +@pytest.fixture +def module_and_tf(): + module = BasicPathFollower(rpc_transport=MemoryRPC) + wire = CdrTransport("basic_tf", module.tf.type, backend="lcm", url="memq://") + module.tf.transport = wire + try: + yield module, module.tfbuffer + finally: + module.stop() + wire.stop() + + +def _mount(): + return TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(1.0)), child_frame_id="mid360_link", - ts=1.0, + transform=Transform( + translation=Vector3(0.0, 0.0, MOUNT_Z), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), ) -def _odom_edge() -> Transform: - return Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="odom", +def _odom_edge(): + return TransformStamped( + header=Header(frame_id="odom", stamp=time_from_seconds(1.0)), child_frame_id="mid360_link", - ts=1.0, + transform=Transform( + translation=Vector3(1.0, 2.0, 3.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), ) -def test_lookup_pose_steers_from_the_tf_base_pose() -> None: - tf = FakeTF() - tf.receive_transform(_mount()) - tf.receive_transform(_odom_edge()) - module = BasicPathFollower() - module._tf = tf - try: - pose = module._lookup_pose() - assert pose is not None - assert abs(pose.position.z - (3.0 - MOUNT_Z)) < 1e-9 - finally: - module.stop() +def test_lookup_pose_steers_from_the_tf_base_pose(module_and_tf): + module, tf = module_and_tf + tf.receive_transform(_mount(), _odom_edge()) + pose = module._lookup_pose() + assert pose is not None + assert abs(pose.pose.position.z - (3.0 - MOUNT_Z)) < 1e-9 -def test_lookup_pose_is_none_without_the_mount_tf() -> None: - tf = FakeTF() +def test_lookup_pose_is_none_without_the_mount_tf(module_and_tf): + module, tf = module_and_tf tf.receive_transform(_odom_edge()) - module = BasicPathFollower() - module._tf = tf - try: - assert module._lookup_pose() is None - finally: - module.stop() - - -def test_lookup_retries_are_throttled_during_an_outage() -> None: - tf = FakeTF() - module = BasicPathFollower() - module._tf = tf - try: - assert module._lookup_pose() is None - assert module._lookup_pose() is None - assert tf.gets == 1 - tf.receive_transform(_mount()) - tf.receive_transform(_odom_edge()) - module._next_lookup = 0.0 - assert module._lookup_pose() is not None - finally: - module.stop() + assert module._lookup_pose() is None + + +def test_lookup_retries_are_throttled_during_an_outage(module_and_tf, mocker): + module, tf = module_and_tf + lookup = mocker.spy(tf, "get_pose") + assert module._lookup_pose() is None + assert module._lookup_pose() is None + assert lookup.call_count == 1 + tf.receive_transform(_mount(), _odom_edge()) + module._next_lookup = 0.0 + assert module._lookup_pose() is not None def test_lookahead_floor_at_low_speed() -> None: diff --git a/dimos/navigation/trajectory_follower/fancy/controller.py b/dimos/navigation/trajectory_follower/fancy/controller.py index eef34feb27f..7519b9628b4 100644 --- a/dimos/navigation/trajectory_follower/fancy/controller.py +++ b/dimos/navigation/trajectory_follower/fancy/controller.py @@ -30,9 +30,9 @@ import numpy as np from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path +from dimos.messages.geometry_msgs.msg import PoseStamped, Twist +from dimos.messages.nav_msgs.msg import Path +from dimos.msgs.geometry import yaw from dimos.protocol.service.spec import BaseConfig @@ -87,6 +87,7 @@ def path_xy_yaw(path: Path) -> NDArray[np.float64]: """The plan as a contiguous (N, 3) float64 array, the rust marshalling.""" return np.ascontiguousarray( np.array( - [[p.position.x, p.position.y, p.yaw] for p in path.poses], dtype=np.float64 + [[p.pose.position.x, p.pose.position.y, yaw(p.pose.orientation)] for p in path.poses], + dtype=np.float64, ).reshape(-1, 3) ) diff --git a/dimos/navigation/trajectory_follower/fancy/laws/hinted.py b/dimos/navigation/trajectory_follower/fancy/laws/hinted.py index f7d02853bcf..80ee6bfbe77 100644 --- a/dimos/navigation/trajectory_follower/fancy/laws/hinted.py +++ b/dimos/navigation/trajectory_follower/fancy/laws/hinted.py @@ -25,10 +25,10 @@ import numpy as np from numpy.typing import NDArray -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.messages.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos.messages.nav_msgs.msg import Path +from dimos.msgs.geometry import yaw +from dimos.msgs.time import to_nanoseconds from dimos.navigation.embodiment.base import Embodiment from dimos.navigation.embodiment.go2 import GO2 from dimos.navigation.local_planner.profile import decode_ceilings @@ -76,12 +76,12 @@ def update( self, pose: PoseStamped, path: Path, t: float, clearance: NDArray[np.float64] | None = None ) -> Twist: cfg, emb = self.config, self.emb - if len(path) < 2: + if len(path.poses) < 2: # empty path or single-pose veto stub: the planner says stop return Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) - xy = np.array([[p.position.x, p.position.y] for p in path.poses]) - yaws = np.array([p.yaw for p in path.poses]) - px, py, pyaw = pose.position.x, pose.position.y, pose.yaw + xy = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]) + yaws = np.array([yaw(p.pose.orientation) for p in path.poses]) + px, py, pyaw = pose.pose.position.x, pose.pose.position.y, yaw(pose.pose.orientation) n = len(xy) seg = np.linalg.norm(np.diff(xy, axis=0), axis=1) if n > 1 else np.zeros(1) @@ -198,10 +198,21 @@ def update( clr = None if clearance is None else np.ascontiguousarray(clearance, dtype=np.float64) # the law reads only stamp deltas: a precision profile, not a schedule ts = np.ascontiguousarray( - np.array([p.ts for p in path.poses], dtype=np.float64).reshape(-1) + np.array( + [ + to_nanoseconds(p.header.stamp) - to_nanoseconds(path.poses[0].header.stamp) + for p in path.poses + ], + dtype=np.float64, + ).reshape(-1) + / 1_000_000_000 ) vx, vy, wz = self._mod.update_hinted( - (float(pose.position.x), float(pose.position.y), float(pose.yaw)), + ( + float(pose.pose.position.x), + float(pose.pose.position.y), + float(yaw(pose.pose.orientation)), + ), path_xy_yaw(path), clr, ts, diff --git a/dimos/navigation/trajectory_follower/fancy/laws/seed.py b/dimos/navigation/trajectory_follower/fancy/laws/seed.py index 14072fe6311..068bbb3d200 100644 --- a/dimos/navigation/trajectory_follower/fancy/laws/seed.py +++ b/dimos/navigation/trajectory_follower/fancy/laws/seed.py @@ -25,10 +25,9 @@ import numpy as np from numpy.typing import NDArray -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.messages.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos.messages.nav_msgs.msg import Path +from dimos.msgs.geometry import yaw from dimos.navigation.embodiment.base import Embodiment from dimos.navigation.embodiment.go2 import GO2 from dimos.navigation.trajectory_follower.fancy.controller import ( @@ -67,12 +66,12 @@ def update( self, pose: PoseStamped, path: Path, t: float, clearance: NDArray[np.float64] | None = None ) -> Twist: cfg, emb = self.config, self.emb - if len(path) < 2: + if len(path.poses) < 2: # empty path or a single-pose veto stub: hold return Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) - xy = np.array([[p.position.x, p.position.y] for p in path.poses]) - yaws = np.array([p.yaw for p in path.poses]) - px, py, pyaw = pose.position.x, pose.position.y, pose.yaw + xy = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]) + yaws = np.array([yaw(p.pose.orientation) for p in path.poses]) + px, py, pyaw = pose.pose.position.x, pose.pose.position.y, yaw(pose.pose.orientation) seg = np.linalg.norm(np.diff(xy, axis=0), axis=1) if len(xy) > 1 else np.zeros(1) arcs = np.concatenate([[0.0], np.cumsum(seg)]) @@ -141,7 +140,11 @@ def update( ) -> Twist: clr = None if clearance is None else np.ascontiguousarray(clearance, dtype=np.float64) vx, vy, wz = self._mod.update_seed( - (float(pose.position.x), float(pose.position.y), float(pose.yaw)), + ( + float(pose.pose.position.x), + float(pose.pose.position.y), + float(yaw(pose.pose.orientation)), + ), path_xy_yaw(path), clr, self._emb, diff --git a/dimos/navigation/trajectory_follower/fancy/module.py b/dimos/navigation/trajectory_follower/fancy/module.py index 43b78873249..c83cf0f0e01 100644 --- a/dimos/navigation/trajectory_follower/fancy/module.py +++ b/dimos/navigation/trajectory_follower/fancy/module.py @@ -33,14 +33,14 @@ 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.nav_msgs.Path import Path -from dimos.msgs.std_msgs.Bool import Bool -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.messages.geometry_msgs.msg import PoseStamped, Twist +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.navigation import spec from dimos.navigation.embodiment.base import Embodiment from dimos.navigation.embodiment.go2 import GO2 +from dimos.navigation.go2.replanning_a_star.controllers import _velocity from dimos.navigation.tf_pose import TfPose from dimos.navigation.trajectory_follower.fancy.controller import TrajectoryController from dimos.navigation.trajectory_follower.fancy.laws import hinted @@ -84,7 +84,7 @@ class TrajectoryFollowerConfig(ModuleConfig): goal_tolerance: float = 0.20 # planar distance that counts as arrival (m) # The planner's own body: the law decodes the path stamps with its governor band. embodiment: Embodiment = GO2 - # The pose is the `path.frame_id -> base_frame` edge on tf, read each tick. + # The pose is the `path.header.frame_id -> base_frame` edge on tf, read each tick. base_frame: str = "base_link" # Deadman: zero the twist once the held path is this old, measured from arrival; must clear the ~1 Hz replan cadence. max_path_age_s: float = 2.5 @@ -129,7 +129,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 _on_path(self, msg: Path) -> None: @@ -144,10 +144,12 @@ def _on_path(self, msg: Path) -> None: self._path_at = time.monotonic() if len(msg.poses) >= 2: # a single-pose stub is a refusal, not an arrival target - self._latch.set_goal((msg.poses[-1].position.x, msg.poses[-1].position.y)) + self._latch.set_goal( + (msg.poses[-1].pose.position.x, msg.poses[-1].pose.position.y) + ) if not msg.poses: # now, not a control period later - self.nav_cmd_vel.publish(Twist()) + self.nav_cmd_vel.publish(_velocity()) def _control_loop(self) -> None: period = 1.0 / self.config.control_frequency @@ -165,29 +167,29 @@ def tick(self) -> None: age = None if self._path_at is None else now - self._path_at pose = None if path is not None and self._pose_src is not None: - pose = self._pose_src.get(path.frame_id) + pose = self._pose_src.get(path.header.frame_id) if path is not None and pose is not None: assert age is not None self.step(pose, path, age) elif path is not None: # a plan with no live pose under it: the deadman on the pose - self.nav_cmd_vel.publish(Twist()) + self.nav_cmd_vel.publish(_velocity()) def step(self, pose: PoseStamped, path: Path, age: float) -> None: """One control tick against a path that arrived `age` seconds ago.""" # the deadman outranks arrival: a goal reached against an unrefreshed plan is a coincidence if age > self.config.max_path_age_s: - self.nav_cmd_vel.publish(Twist()) + self.nav_cmd_vel.publish(_velocity()) return - xy = (pose.position.x, pose.position.y) + xy = (pose.pose.position.x, pose.pose.position.y) with self._lock: arrived, reached = self._latch.arrive(xy), self._latch.reached if arrived: - self.nav_cmd_vel.publish(Twist()) - self.goal_reached.publish(Bool(True)) + self.nav_cmd_vel.publish(_velocity()) + self.goal_reached.publish(Bool(data=True)) logger.info("goal reached", x=round(xy[0], 2), y=round(xy[1], 2)) return if reached: - self.nav_cmd_vel.publish(Twist()) + self.nav_cmd_vel.publish(_velocity()) return self.nav_cmd_vel.publish(self._controller.update(pose, path, time.monotonic())) diff --git a/dimos/navigation/trajectory_follower/fancy/test_controller.py b/dimos/navigation/trajectory_follower/fancy/test_controller.py index 167029c2c8f..97d64d0750b 100644 --- a/dimos/navigation/trajectory_follower/fancy/test_controller.py +++ b/dimos/navigation/trajectory_follower/fancy/test_controller.py @@ -19,10 +19,11 @@ from pydantic import ValidationError import pytest -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_seconds from dimos.navigation.embodiment.go2 import GO2 from dimos.navigation.trajectory_follower.fancy.controller import ControllerConfig from dimos.navigation.trajectory_follower.fancy.laws.seed import PursuitController @@ -30,21 +31,23 @@ def _pose(x: float, y: float, yaw: float = 0.0) -> PoseStamped: return PoseStamped( - frame_id="world", - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0, 0, yaw)), + header=Header(frame_id="world", stamp=time_from_seconds(0.0)), + pose=Pose(position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0, 0, yaw)), ) def _straight_path(n: int = 40, step: float = 0.1) -> Path: - return Path(frame_id="world", poses=[_pose(i * step, 0.0) for i in range(n)]) + return Path( + header=Header(frame_id="world", stamp=time_from_seconds(0.0)), + poses=[_pose(i * step, 0.0) for i in range(n)], + ) def _fan_path() -> Path: """Rotate in place at the origin: yaw steps with zero displacement.""" yaws = [0.0, 0.3, 0.6, 0.9, 1.2, 1.5] poses = [_pose(0.0, 0.0, y) for y in yaws] + [_pose(0.1, 0.0, 1.5), _pose(1.0, 0.1, 1.5)] - return Path(frame_id="world", poses=poses) + return Path(header=Header(frame_id="world", stamp=time_from_seconds(0.0)), poses=poses) def test_on_path_drives_forward() -> None: @@ -84,7 +87,11 @@ def test_fan_done_resumes_translation() -> None: def test_empty_path_stops() -> None: - tw = PursuitController().update(_pose(0.0, 0.0), Path(frame_id="world", poses=[]), 0.0) + tw = PursuitController().update( + _pose(0.0, 0.0), + Path(header=Header(frame_id="world", stamp=time_from_seconds(0.0)), poses=[]), + 0.0, + ) assert tw.linear.x == 0.0 and tw.angular.z == 0.0 @@ -105,7 +112,7 @@ def test_config_rejects_unknown_fields() -> None: def test_governor_creeps_in_tight_room() -> None: path = _straight_path() - tight = np.full(len(path), 0.06) # barely above the precision floor + tight = np.full(len(path.poses), 0.06) # barely above the precision floor tw = PursuitController().update(_pose(0.0, 0.0), path, 0.0, clearance=tight) # within the bottom twentieth of the governor's band, whatever the ceiling is creep = GO2.min_speed + 0.05 * (GO2.max_speed - GO2.min_speed) @@ -114,7 +121,7 @@ def test_governor_creeps_in_tight_room() -> None: def test_governor_full_speed_in_open_room() -> None: path = _straight_path() - wide = np.full(len(path), 1.0) + wide = np.full(len(path.poses), 1.0) tw_open = PursuitController().update(_pose(-1.0, 0.0), path, 0.0, clearance=wide) tw_bare = PursuitController().update(_pose(-1.0, 0.0), path, 0.0) assert math.hypot(tw_open.linear.x, tw_open.linear.y) == pytest.approx( @@ -124,7 +131,7 @@ def test_governor_full_speed_in_open_room() -> None: def test_governor_reads_room_ahead_not_behind() -> None: path = _straight_path() # 4 m of path - clear = np.full(len(path), 1.0) + clear = np.full(len(path.poses), 1.0) clear[:5] = 0.06 # tight patch already behind the lookahead window tw = PursuitController().update(_pose(1.5, 0.0), path, 0.0, clearance=clear) assert math.hypot(tw.linear.x, tw.linear.y) > GO2.min_speed + 0.1 diff --git a/dimos/navigation/trajectory_follower/fancy/test_module.py b/dimos/navigation/trajectory_follower/fancy/test_module.py index de5f2f45bb2..7d55e343b2b 100644 --- a/dimos/navigation/trajectory_follower/fancy/test_module.py +++ b/dimos/navigation/trajectory_follower/fancy/test_module.py @@ -16,11 +16,18 @@ import pytest from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -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, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.nav_msgs.msg import Path +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.time import time_from_seconds from dimos.navigation.tf_pose import TfPose from dimos.navigation.trajectory_follower.fancy.laws import seed from dimos.navigation.trajectory_follower.fancy.module import ( @@ -29,8 +36,15 @@ TrajectoryFollowerConfig, ) from dimos.navigation.trajectory_follower.fancy.native import TrajectoryFollowerNativeConfig +from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.protocol.tf.tf import MultiTBuffer + +class MemoryRPC(LCMRPC): + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + # The real constructor stands up an LCM RPC transport per instance; the fixture stops them. _BUILT: list[TrajectoryFollower] = [] @@ -62,25 +76,24 @@ def test_goal_latch_ignores_sub_tolerance_goal_moves(): def _follower(**config): - follower = TrajectoryFollower(**config) + follower = TrajectoryFollower(rpc_transport=MemoryRPC, **config) _BUILT.append(follower) return follower def _base_at(z: float) -> PoseStamped: """The tf-resolved base pose; the ground sits emb.base_height under it.""" - return PoseStamped(ts=0.0, frame_id="odom", position=Vector3(0.0, 0.0, z)) + return PoseStamped( + header=Header(frame_id="odom", stamp=time_from_seconds(0.0)), + pose=Pose(position=Point(x=0.0, y=0.0, z=z), orientation=Quaternion(0.0, 0.0, 0.0, 1.0)), + ) def _straight_path(end_x: float = 0.5) -> Path: - return Path( - ts=0.0, - frame_id="odom", - poses=[ - PoseStamped(ts=0.0, frame_id="odom", position=Vector3(0.0, 0.0, 0.0)), - PoseStamped(ts=0.0, frame_id="odom", position=Vector3(end_x, 0.0, 0.0)), - ], - ) + first = _base_at(0.0) + last = _base_at(0.0) + last.pose.position.x = end_x + return Path(header=Header(frame_id="odom", stamp=time_from_seconds(0.0)), poses=[first, last]) class _Heard(list): @@ -132,7 +145,8 @@ def test_a_stale_path_outranks_an_arrival(): out, reached = _driven(follower), _reached(follower) path = _straight_path(0.5) follower._on_path(path) - at_goal = PoseStamped(ts=0.0, frame_id="odom", position=Vector3(0.5, 0.0, 0.01)) + at_goal = _base_at(0.01) + at_goal.pose.position.x = 0.5 follower.step(at_goal, path, age=9.0) assert out[-1].linear.x == 0.0 assert not reached @@ -153,12 +167,12 @@ def _on_tf(follower: TrajectoryFollower, frame: str = "odom") -> tuple[MultiTBuf tf, clock = MultiTBuffer(), _Clock() follower._pose_src = TfPose(tf, "base_link", follower.config.max_path_age_s, clock=clock) tf.receive_transform( - Transform( - translation=Vector3(0.0, 0.0, 0.01), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=frame, + TransformStamped( + header=Header(frame_id=frame, stamp=time_from_seconds(5.0)), child_frame_id="base_link", - ts=5.0, + transform=Transform( + translation=Vector3(0.0, 0.0, 0.01), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), ) ) return tf, clock @@ -171,7 +185,12 @@ def test_the_pose_comes_off_tf_in_the_path_frame(heard): follower._on_path(_straight_path(5.0)) # an odom plan; the edge on tf is map -> base_link follower.tick() assert (out[-1].linear.x, out[-1].linear.y, out[-1].angular.z) == (0.0, 0.0, 0.0) - follower._on_path(Path(ts=0.0, frame_id="map", poses=_straight_path(5.0).poses)) + follower._on_path( + Path( + header=Header(frame_id="map", stamp=time_from_seconds(0.0)), + poses=_straight_path(5.0).poses, + ) + ) clock.t += TfPose.RETRY_PERIOD_S # the miss parked lookups for a period follower.tick() assert out[-1].linear.x > 0.0 @@ -196,7 +215,7 @@ def test_an_empty_path_drops_the_plan_and_zeroes_the_twist(): follower._on_path(path) follower.step(pose, path, age=0.0) assert out[-1].linear.x > 0.0 - follower._on_path(Path(ts=0.0, frame_id=path.frame_id, poses=[])) + follower._on_path(Path(header=path.header, poses=[])) assert (out[-1].linear.x, out[-1].linear.y, out[-1].angular.z) == (0.0, 0.0, 0.0) assert follower._path is None diff --git a/dimos/navigation/trajectory_follower/fancy/test_rust_parity.py b/dimos/navigation/trajectory_follower/fancy/test_rust_parity.py index f31922ee309..c99bc88dd6b 100644 --- a/dimos/navigation/trajectory_follower/fancy/test_rust_parity.py +++ b/dimos/navigation/trajectory_follower/fancy/test_rust_parity.py @@ -27,10 +27,11 @@ pytest.importorskip("dimos_trajectory_follower") -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_seconds from dimos.navigation.embodiment.go2 import GO2 from dimos.navigation.trajectory_follower.fancy.controller import ( ControllerConfig, @@ -51,14 +52,16 @@ def _pose(x: float, y: float, yaw: float = 0.0) -> PoseStamped: return PoseStamped( - frame_id="world", - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0, 0, yaw)), + header=Header(frame_id="world", stamp=time_from_seconds(0.0)), + pose=Pose(position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0, 0, yaw)), ) def _path(states: list[tuple[float, float, float]]) -> Path: - return Path(frame_id="world", poses=[_pose(*s) for s in states]) + return Path( + header=Header(frame_id="world", stamp=time_from_seconds(0.0)), + poses=[_pose(*s) for s in states], + ) def _straight(rng: np.random.Generator) -> list[tuple[float, float, float]]: @@ -142,10 +145,12 @@ def _cases(seed: int = 20260802, n: int = CASES): # type: ignore[no-untyped-def path_xy_yaw(path), enc, float(srng.uniform(0.0, 1e9)), GOVERNOR ) for q, t in zip(path.poses, ts, strict=True): - q.ts = t + q.header.stamp = time_from_seconds(t) if k % 8 == 3: for q in path.poses: - q.ts = 5.0 # flat: not the dialect, both must ignore it + q.header.stamp = time_from_seconds( + 5.0 + ) # flat: not the dialect, both must ignore it # non-default gains and plant every fifth case: the params tuple order is load-bearing emb = GO2 @@ -153,14 +158,14 @@ def _cases(seed: int = 20260802, n: int = CASES): # type: ignore[no-untyped-def emb = replace( GO2, control=GO2.control.model_copy( - update=dict( - lookahead=float(rng.uniform(0.1, 1.2)), - k_pos=float(rng.uniform(0.5, 4.0)), - k_yaw=float(rng.uniform(0.5, 4.0)), - fan_yaw_per_m=float(rng.uniform(1.0, 6.0)), - fan_yaw_done=float(rng.uniform(0.05, 0.6)), - speed_lookahead=float(rng.uniform(0.5, 4.0)), - ) + update={ + "lookahead": float(rng.uniform(0.1, 1.2)), + "k_pos": float(rng.uniform(0.5, 4.0)), + "k_yaw": float(rng.uniform(0.5, 4.0)), + "fan_yaw_per_m": float(rng.uniform(1.0, 6.0)), + "fan_yaw_done": float(rng.uniform(0.05, 0.6)), + "speed_lookahead": float(rng.uniform(0.5, 4.0)), + } ), max_speed=float(rng.uniform(0.2, 1.5)), min_speed=float(rng.uniform(0.05, 0.3)), @@ -203,9 +208,9 @@ def test_rust_matches_python(law: str) -> None: seen["held"] += 1 if abs(a[2]) >= emb.max_yaw_rate - 1e-12 or math.hypot(a[0], a[1]) >= top - 1e-12: seen["clamped"] += 1 - if case[3] is not None and len(case[3]) == len(case[2]): + if case[3] is not None and len(case[3]) == len(case[2].poses): seen["governed"] += 1 - if len(case[2]) > 2 and math.hypot(a[0], a[1]) < 1e-3 and abs(a[2]) > 1e-3: + if len(case[2].poses) > 2 and math.hypot(a[0], a[1]) < 1e-3 and abs(a[2]) > 1e-3: seen["fan"] += 1 # the sweep is only worth its tolerance if it actually reached the branches assert all(v > 0 for v in seen.values()), f"unexercised branches: {seen}" diff --git a/dimos/navigation/trajectory_follower/test_generated_ports.py b/dimos/navigation/trajectory_follower/test_generated_ports.py new file mode 100644 index 00000000000..eb5ff68e7c0 --- /dev/null +++ b/dimos/navigation/trajectory_follower/test_generated_ports.py @@ -0,0 +1,121 @@ +# 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. + +from contextlib import ExitStack +from copy import deepcopy +from threading import Event + +import pytest + +from dimos.core.cdr_transport import CdrTransport +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.nav_msgs.msg import Path +from dimos.messages.std_msgs.msg import Bool, Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.time import time_from_nanoseconds +from dimos.navigation.trajectory_follower.basic.module import BasicPathFollower +from dimos.navigation.trajectory_follower.fancy.module import TrajectoryFollower +from dimos.protocol.rpc.pubsubrpc import LCMRPC + + +class MemoryRPC(LCMRPC): + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + +def route(header, end): + return Path( + header=deepcopy(header), + poses=[ + PoseStamped( + header=deepcopy(header), + pose=Pose( + position=Point(x=x, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + for x in [end * index / 16 for index in range(17)] + ], + ) + + +def base_transform(header, x): + return TFMessage( + transforms=[ + TransformStamped( + header=deepcopy(header), + child_frame_id="base_link", + transform=Transform( + translation=Vector3(x=x, y=0, z=0.01), rotation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + ] + ) + + +@pytest.mark.parametrize("follower_type", [BasicPathFollower, TrajectoryFollower]) +def test_actual_generated_follower_ports_drive_arrive_and_preempt_over_cdr(follower_type): + with ExitStack() as cleanup: + follower = follower_type(rpc_transport=MemoryRPC) + wires = {} + for name in ["tf", "path", "nav_cmd_vel", "goal_reached"]: + port = getattr(follower, name) + wire = CdrTransport("follower_" + name, port.type, backend="lcm", url="memq://") + port.transport = wire + wires[name] = wire + cleanup.callback(wire.stop) + cleanup.callback(follower.stop) + commands = [] + goals = [] + moving = Event() + arrived = Event() + zeroed = Event() + + def receive_command(value): + commands.append(value) + if value.linear.x > 0: + moving.set() + elif value.linear.x == 0 and value.linear.y == 0 and value.angular.z == 0: + zeroed.set() + + def receive_goal(value): + goals.append(value) + if value.data: + arrived.set() + + cleanup.callback(wires["nav_cmd_vel"].subscribe(receive_command)) + cleanup.callback(wires["goal_reached"].subscribe(receive_goal)) + follower.start() + header = Header(frame_id="odom", stamp=time_from_nanoseconds(1700000000123456789)) + wires["tf"].publish(base_transform(header, 0.0)) + wires["path"].publish(route(header, 2.0)) + assert moving.wait(5) + header.stamp = time_from_nanoseconds(1700000000123456790) + wires["tf"].publish(base_transform(header, 2.0)) + assert arrived.wait(5) + assert goals == [Bool(data=True)] + moving.clear() + wires["path"].publish(route(header, 4.0)) + assert moving.wait(5) + zeroed.clear() + wires["path"].publish(Path(header=deepcopy(header), poses=[])) + assert zeroed.wait(5) + assert any(c.linear.x > 0 for c in commands) From 7a544c743bb80fd4e2504d4b9728ea4338457998 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 20 Oct 2026 06:23:33 +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