diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index 567c681a633..e912c677b33 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -262,3 +262,8 @@ jobs: run: | .native-review/bin/python -I -c 'from dimos.mapping.occupancy import path_map; from importlib import import_module; astar = import_module("dimos.navigation.go2.replanning_a_star.min_cost_astar"); import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (path_map, astar)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/navigation/go2/replanning_a_star/test_generated_astar.py" MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/occupancy/path_map.py dimos/navigation/go2/replanning_a_star/min_cost_astar.py + - name: Verify generated path resampling and exact source metadata + if: runner.os == 'Linux' + run: | + .native-review/bin/python -I -c 'from dimos.mapping.occupancy import path_resampling; import pytest; import sys; assert "site-packages" in str(path_resampling.__file__); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/occupancy/test_generated_path_resampling.py" + MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/occupancy/path_resampling.py diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index 1f50fad7cdf..292273b438d 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -13,23 +13,75 @@ # limitations under the License. +from copy import deepcopy import math +from typing import TypeVar, cast import numpy as np from scipy.ndimage import uniform_filter1d +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose as CdrPose, + PoseStamped as CdrPoseStamped, + Quaternion as CdrQuaternion, +) +from dimos.messages.nav_msgs.msg import Path as CdrPath from dimos.msgs.geometry_msgs.Pose import Pose 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.utils.logging_config import setup_logger -from dimos.utils.transform_utils import euler_to_quaternion logger = setup_logger() -def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: +_PathT = TypeVar("_PathT", Path, CdrPath) +_Stamped = PoseStamped | CdrPoseStamped +_Quaternion = Quaternion | CdrQuaternion + + +def _pose(stamped: _Stamped) -> PoseStamped | CdrPose: + return stamped.pose if isinstance(stamped, CdrPoseStamped) else stamped + + +def _new_pose( + path: Path | CdrPath, position: tuple[float, float, float], orientation: _Quaternion +) -> _Stamped: + if isinstance(path, CdrPath): + x, y, z = position + return CdrPoseStamped( + header=deepcopy(path.header), + pose=CdrPose( + position=Point(x=float(x), y=float(y), z=float(z)), + orientation=CdrQuaternion( + x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w + ), + ), + ) + return PoseStamped( + frame_id=path.frame_id, + position=position, + orientation=Quaternion(orientation.x, orientation.y, orientation.z, orientation.w), + ) + + +def _new_path(path: _PathT, poses: list[_Stamped]) -> _PathT: + if isinstance(path, CdrPath): + return CdrPath(header=deepcopy(path.header), poses=cast("list[CdrPoseStamped]", poses)) + return Path(frame_id=path.frame_id, poses=cast("list[PoseStamped]", poses)) + + +def _set_orientation(stamped: _Stamped, orientation: _Quaternion) -> None: + if isinstance(stamped, CdrPoseStamped): + stamped.pose.orientation = CdrQuaternion( + x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w + ) + else: + stamped.orientation = Quaternion(orientation.x, orientation.y, orientation.z, orientation.w) + + +def _add_orientations_to_path(path: Path | CdrPath, goal_orientation: _Quaternion) -> None: """Add orientations to path poses based on direction of movement. Args: @@ -44,8 +96,8 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: # Calculate orientations for all poses except the last one for i in range(len(path.poses) - 1): - current_pose = path.poses[i] - next_pose = path.poses[i + 1] + current_pose = _pose(path.poses[i]) + next_pose = _pose(path.poses[i + 1]) # Calculate direction to next point dx = next_pose.position.x - current_pose.position.x @@ -55,24 +107,29 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: yaw = math.atan2(dy, dx) # Convert to quaternion (roll=0, pitch=0, yaw) - orientation = euler_to_quaternion(Vector3(0, 0, yaw)) - current_pose.orientation = orientation + orientation = Quaternion(0, 0, math.sin(yaw / 2), math.cos(yaw / 2)) + _set_orientation(path.poses[i], orientation) # Set last pose orientation identity_quat = Quaternion(0, 0, 0, 1) - if goal_orientation != identity_quat: + if (goal_orientation.x, goal_orientation.y, goal_orientation.z, goal_orientation.w) != ( + 0, + 0, + 0, + 1, + ): # Use the provided goal orientation if it's not the identity - path.poses[-1].orientation = goal_orientation + _set_orientation(path.poses[-1], goal_orientation) elif len(path.poses) > 1: # Use the previous pose's orientation - path.poses[-1].orientation = path.poses[-2].orientation + _set_orientation(path.poses[-1], _pose(path.poses[-2]).orientation) else: # Single pose with identity goal orientation - path.poses[-1].orientation = identity_quat + _set_orientation(path.poses[-1], identity_quat) # TODO: replace goal_pose with just goal_orientation -def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: +def simple_resample_path(path: _PathT, goal_pose: Pose | CdrPose, spacing: float) -> _PathT: """Resample a path to have approximately uniform spacing between poses. Args: @@ -82,21 +139,21 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: Returns: A new Path with resampled poses """ - if len(path) < 2 or spacing <= 0: + if len(path.poses) < 2 or spacing <= 0: return path - resampled = [] - resampled.append(path.poses[0]) + resampled: list[_Stamped] = [] + resampled.append(deepcopy(path.poses[0])) accumulated_distance = 0.0 for i in range(1, len(path.poses)): current = path.poses[i] - prev = path.poses[i - 1] + prev: _Stamped = path.poses[i - 1] # Calculate segment distance - dx = current.x - prev.x - dy = current.y - prev.y + dx = _pose(current).position.x - _pose(prev).position.x + dy = _pose(current).position.y - _pose(prev).position.y segment_length = (dx**2 + dy**2) ** 0.5 if segment_length < 1e-10: @@ -114,13 +171,9 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: break # Create new pose - new_x = prev.x + dir_x * dist_along - new_y = prev.y + dir_y * dist_along - new_pose = PoseStamped( - frame_id=path.frame_id, - position=[new_x, new_y, 0.0], - orientation=prev.orientation, # Keep same orientation - ) + new_x = _pose(prev).position.x + dir_x * dist_along + new_y = _pose(prev).position.y + dir_y * dist_along + new_pose = _new_pose(path, (new_x, new_y, 0.0), _pose(prev).orientation) resampled.append(new_pose) # Update for next iteration @@ -133,10 +186,13 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: # Add last pose if not already there if len(path.poses) > 1: last = path.poses[-1] - if not resampled or (resampled[-1].x != last.x or resampled[-1].y != last.y): - resampled.append(last) + if not resampled or ( + _pose(resampled[-1]).position.x != _pose(last).position.x + or _pose(resampled[-1]).position.y != _pose(last).position.y + ): + resampled.append(deepcopy(last)) - ret = Path(frame_id=path.frame_id, poses=resampled) + ret = _new_path(path, resampled) _add_orientations_to_path(ret, goal_pose.orientation) @@ -144,8 +200,8 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: def smooth_resample_path( - path: Path, goal_pose: Pose, spacing: float, smoothing_window: int = 100 -) -> Path: + path: _PathT, goal_pose: Pose | CdrPose, spacing: float, smoothing_window: int = 100 +) -> _PathT: """Resample a path with smoothing to reduce jagged corners and abrupt turns. This produces smoother paths than simple_resample_path by: @@ -165,21 +221,17 @@ def smooth_resample_path( """ if len(path.poses) == 1: - p = path.poses[0].position + p = _pose(path.poses[0]).position o = goal_pose.orientation - new_pose = PoseStamped( - frame_id=path.frame_id, - position=[p.x, p.y, p.z], - orientation=[o.x, o.y, o.z, o.w], - ) - return Path(frame_id=path.frame_id, poses=[new_pose]) + new_pose = _new_pose(path, (p.x, p.y, p.z), o) + return _new_path(path, [new_pose]) - if len(path) < 2 or spacing <= 0: + if len(path.poses) < 2 or spacing <= 0: return path # Extract x, y coordinates from path - xs = np.array([p.x for p in path.poses]) - ys = np.array([p.y for p in path.poses]) + xs = np.array([_pose(p).position.x for p in path.poses]) + ys = np.array([_pose(p).position.y for p in path.poses]) # Remove duplicate consecutive points diffs = np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2) @@ -243,16 +295,14 @@ def smooth_resample_path( sampled_y = np.interp(sample_distances, arc_length_smooth, ys_smooth) # Create resampled poses - resampled = [] + resampled: list[_Stamped] = [] for i in range(len(sampled_x)): - new_pose = PoseStamped( - frame_id=path.frame_id, - position=[float(sampled_x[i]), float(sampled_y[i]), 0.0], - orientation=Quaternion(0, 0, 0, 1), + new_pose = _new_pose( + path, (float(sampled_x[i]), float(sampled_y[i]), 0.0), Quaternion(0, 0, 0, 1) ) resampled.append(new_pose) - ret = Path(frame_id=path.frame_id, poses=resampled) + ret = _new_path(path, resampled) _add_orientations_to_path(ret, goal_pose.orientation) diff --git a/dimos/mapping/occupancy/test_generated_path_resampling.py b/dimos/mapping/occupancy/test_generated_path_resampling.py new file mode 100644 index 00000000000..0c4f9c89fb7 --- /dev/null +++ b/dimos/mapping/occupancy/test_generated_path_resampling.py @@ -0,0 +1,178 @@ +# 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. + + +from copy import deepcopy +import math + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.mapping.occupancy.path_resampling import simple_resample_path, smooth_resample_path +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos.messages.nav_msgs.msg import Path +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry_msgs.Pose import Pose as LegacyPose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as LegacyStamped +from dimos.msgs.nav_msgs.Path import Path as LegacyPath +from dimos.msgs.time import time_from_nanoseconds + + +def make_path(points, stamp): + header = Header(frame_id="map", stamp=time_from_nanoseconds(stamp)) + return Path( + header=deepcopy(header), + poses=[ + PoseStamped( + header=deepcopy(header), + pose=Pose( + position=Point(x=x, y=y, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + for x, y in points + ], + ) + + +def values(path): + return np.array( + [ + [ + p.pose.position.x, + p.pose.position.y, + p.pose.position.z, + p.pose.orientation.x, + p.pose.orientation.y, + p.pose.orientation.z, + p.pose.orientation.w, + ] + for p in path.poses + ] + ) + + +@pytest.mark.parametrize("method", [simple_resample_path, smooth_resample_path]) +@pytest.mark.parametrize("stamp", [0, -1, 1700000000123456789]) +@pytest.mark.parametrize("goal_yaw", [0, math.pi / 2]) +def test_generated_resampling_keeps_exact_metadata_and_matches_existing_numerics( + method, stamp, goal_yaw +): + points = [(0.0, 0.0), (0.4, 0.0), (0.4, 0.4), (0.8, 0.4)] + source = decode(encode(make_path(points, stamp)), Path) + before = deepcopy(source) + quaternion = Quaternion(x=0, y=0, z=math.sin(goal_yaw / 2), w=math.cos(goal_yaw / 2)) + goal = Pose(position=Point(x=0.8, y=0.4, z=0), orientation=quaternion) + legacy = LegacyPath( + frame_id="map", + poses=[ + LegacyStamped(frame_id="map", position=[x, y, 0], orientation=[0, 0, 0, 1]) + for x, y in points + ], + ) + legacy_goal = LegacyPose(0.8, 0.4, 0, 0, 0, quaternion.z, quaternion.w) + + result = method(source, goal, 0.1) + expected = method(legacy, legacy_goal, 0.1) + + assert type(result) is Path + assert result.header == source.header + assert all(p.header == source.header for p in result.poses) + np.testing.assert_allclose( + values(result), + [ + [ + p.position.x, + p.position.y, + p.position.z, + p.orientation.x, + p.orientation.y, + p.orientation.z, + p.orientation.w, + ] + for p in expected.poses + ], + rtol=0, + atol=1e-14, + ) + np.testing.assert_allclose(values(result)[[0, -1], :3], [[0, 0, 0], [0.8, 0.4, 0]]) + assert decode(encode(result), Path) == result + assert source == before + result.poses[0].pose.position.x = 100 + result.poses[0].header.frame_id = "changed" + result.header.frame_id = "changed" + assert source == before + assert result.poses[-1].header == source.header + + +@pytest.mark.parametrize("method", [simple_resample_path, smooth_resample_path]) +@pytest.mark.parametrize( + "points,spacing", + [ + ([], 0.1), + ([(1.0, 2.0)], 0.1), + ([(0.0, 0.0), (1.0, 1.0)], 0), + ([(0.0, 0.0), (1.0, 1.0)], -1), + ], +) +def test_generated_resampling_handles_empty_single_repeated_and_disabled_paths( + method, points, spacing +): + path = make_path(points, 0) + goal = Pose(position=Point(x=1, y=2, z=0), orientation=Quaternion(x=0, y=0, z=1, w=0)) + + result = method(path, goal, spacing) + + assert result.header == path.header + assert len(result.poses) == len(points) + assert [(p.pose.position.x, p.pose.position.y, p.pose.position.z) for p in result.poses] == [ + (x, y, 0) for x, y in points + ] + + +@pytest.mark.parametrize("method,count", [(simple_resample_path, 1), (smooth_resample_path, 2)]) +def test_generated_repeated_coordinates_retain_existing_method_behavior(method, count): + path = make_path([(1.0, 2.0), (1.0, 2.0)], 0) + goal = Pose(position=Point(x=1, y=2, z=0), orientation=Quaternion(x=0, y=0, z=1, w=0)) + result = method(path, goal, 0.1) + assert len(result.poses) == count + assert [(p.pose.position.x, p.pose.position.y) for p in result.poses] == [(1.0, 2.0)] * count + + +def test_generated_single_smooth_pose_uses_goal_orientation_and_copies_metadata(): + path = make_path([(1.0, 2.0)], -1) + before = deepcopy(path) + goal = Pose(position=Point(x=1, y=2, z=0), orientation=Quaternion(x=0, y=0, z=1, w=0)) + result = smooth_resample_path(path, goal, 0.1) + assert result.poses[0].pose.orientation == goal.orientation + assert result.header == path.header and result.poses[0].header == path.header + assert path == before + + +def test_simple_resampling_preserves_existing_endpoint_source_headers_without_aliasing(): + path = make_path([(0.0, 0.0), (0.25, 0.0)], -1) + path.poses[0].header.stamp = time_from_nanoseconds(0) + path.poses[-1].header.stamp = time_from_nanoseconds(17) + before = deepcopy(path) + goal = Pose(position=Point(x=0.25, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)) + + result = simple_resample_path(path, goal, 0.1) + + assert [p.header.stamp for p in result.poses] == [ + path.poses[0].header.stamp, + path.header.stamp, + path.header.stamp, + path.poses[-1].header.stamp, + ] + assert path == before diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index e240a6529d1..b65977283c0 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -20,7 +20,6 @@ import tempfile from typing import TYPE_CHECKING, Any, Protocol, TypedDict -from dimos.messages.sensor_msgs.msg import Image from hydra.utils import instantiate import numpy as np from numpy.typing import NDArray @@ -28,6 +27,7 @@ from PIL import Image as PILImage import torch +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import default_torch_device from dimos.msgs.image import image_to_bgr, image_to_jpeg from dimos.perception.detection.detectors.base import Detector diff --git a/dimos/models/segmentation/test_yoloe.py b/dimos/models/segmentation/test_yoloe.py index 345fc79ac1c..5d2efaffa50 100644 --- a/dimos/models/segmentation/test_yoloe.py +++ b/dimos/models/segmentation/test_yoloe.py @@ -16,11 +16,11 @@ from typing import Any from unittest.mock import MagicMock -from dimos.messages.sensor_msgs.msg import Image -from dimos.messages.std_msgs.msg import Header import numpy as np from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.models.segmentation.yoloe import YoloeBoxSegmenter from dimos.msgs.image import image_from_array, image_to_bgr from dimos.msgs.time import time_from_seconds, to_seconds diff --git a/dimos/models/segmentation/yoloe.py b/dimos/models/segmentation/yoloe.py index d19ecc15701..fd075227d27 100644 --- a/dimos/models/segmentation/yoloe.py +++ b/dimos/models/segmentation/yoloe.py @@ -19,11 +19,11 @@ from dataclasses import replace from typing import Protocol -from dimos.messages.sensor_msgs.msg import Image import numpy as np from numpy.typing import NDArray from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.messages.sensor_msgs.msg import Image from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox