From cccf0213ec7c7d3569910bb2a858b7d79eb5f48b Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 15 Jul 2025 20:06:59 -0700 Subject: [PATCH 1/7] simpleplanner tests --- .devcontainer/devcontainer.json | 2 +- dimos/msgs/geometry_msgs/PoseStamped.py | 6 ++ dimos/robot/local_planner/simple.py | 89 ++++++++++++++----- .../multiprocess/unitree_go2.py | 9 +- dimos/robot/unitree_webrtc/type/lidar.py | 8 -- 5 files changed, 76 insertions(+), 38 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index fe96015340..dfa6228492 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,6 +1,6 @@ { "name": "dimos-dev", - "image": "ghcr.io/dimensionalos/ros-dev:dev", + "image": "ghcr.io/dimensionalos/dev:dev", "customizations": { "vscode": { "extensions": [ diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py index 3871072d32..e0936964d8 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -74,3 +74,9 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped: lcm_msg.pose.orientation.w, ], # noqa: E501, ) + + def __str__(self) -> str: + return ( + f"PoseStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " + f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}])" + ) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index 8eaf20ba6c..ffa9a72867 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -14,8 +14,8 @@ import math import time -from dataclasses import dataclass -from typing import Any, Callable, Optional +import traceback +from typing import Callable, Optional import reactivex as rx from plum import dispatch @@ -23,20 +23,18 @@ from dimos.core import In, Module, Out, rpc from dimos.msgs.geometry_msgs import PoseStamped, Vector3 -from dimos.robot.unitree_webrtc.type.odometry import Odometry # from dimos.robot.local_planner.local_planner import LocalPlanner from dimos.types.costmap import Costmap from dimos.types.path import Path -from dimos.types.pose import Pose from dimos.types.vector import Vector, VectorLike, to_vector from dimos.utils.logging_config import setup_logger from dimos.utils.threadpool import get_scheduler -logger = setup_logger("dimos.robot.unitree.global_planner") +logger = setup_logger("dimos.robot.unitree.local_planner") -def transform_to_robot_frame(global_vector: Vector, robot_position: Pose) -> Vector: +def transform_to_robot_frame(global_vector: Vector3, robot_position: PoseStamped) -> Vector: """Transform a global coordinate vector to robot-relative coordinates. Args: @@ -47,13 +45,14 @@ def transform_to_robot_frame(global_vector: Vector, robot_position: Pose) -> Vec Vector in robot coordinates where X is forward/backward, Y is left/right """ # Get the robot's yaw angle (rotation around Z-axis) - robot_yaw = robot_position.rot.z + robot_yaw = robot_position.yaw # Create rotation matrix to transform from global to robot frame # We need to rotate the coordinate system by -robot_yaw to get robot-relative coordinates cos_yaw = math.cos(-robot_yaw) sin_yaw = math.sin(-robot_yaw) + print(cos_yaw, sin_yaw, global_vector) # Apply 2D rotation transformation # This transforms a global direction vector into the robot's coordinate frame # In robot frame: X=forward/backward, Y=left/right @@ -61,7 +60,7 @@ def transform_to_robot_frame(global_vector: Vector, robot_position: Pose) -> Vec robot_x = global_vector.x * cos_yaw - global_vector.y * sin_yaw # Forward/backward robot_y = global_vector.x * sin_yaw + global_vector.y * cos_yaw # Left/right - return Vector(-robot_x, robot_y, 0) + return Vector3(-robot_x, robot_y, 0) class SimplePlanner(Module): @@ -71,7 +70,7 @@ class SimplePlanner(Module): get_costmap: Callable[[], Costmap] - latest_odom: PoseStamped = None + latest_odom: Optional[PoseStamped] = None goal: Optional[Vector] = None speed: float = 0.3 @@ -83,24 +82,59 @@ def __init__( Module.__init__(self) self.get_costmap = get_costmap - def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: + def get_move_stream(self, frequency: float = 20.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( # do we have a goal? ops.filter(lambda _: self.goal is not None), - # For testing: make robot move left/right instead of rotating - ops.map(lambda _: self._test_translational_movement()), - self.frequency_spy("movement_test"), + # do we have odometry data? + ops.filter(lambda _: self.latest_odom is not None), + # transform goal to robot frame with error handling + ops.map(lambda _: self._safe_transform_goal()), + # filter out None results (errors) + ops.filter(lambda result: result is not None), + ops.map(self._calculate_rotation_to_target), + # filter out None results from calc_move + ops.filter(lambda result: result is not None), ) + def _safe_transform_goal(self) -> Optional[Vector]: + """Safely transform goal to robot frame with error handling.""" + try: + if self.goal is None or self.latest_odom is None: + return None + + # Convert PoseStamped to Pose for transform function + # Pose constructor takes (pos, rot) where pos and rot are VectorLike + + return transform_to_robot_frame(self.goal, self.latest_odom) + except Exception as e: + logger.error(f"Error transforming goal to robot frame: {e}") + print(traceback.format_exc()) + return None + @rpc def start(self): + print(self.path.connection, self.path.transport) self.path.subscribe(self.set_goal) + self.movecmd.publish(Vector3(1, 2, 3)) + self.movecmd.publish(Vector3(1, 2, 3)) + self.movecmd.publish(Vector3(1, 2, 3)) + self.movecmd.publish(Vector3(1, 2, 3)) - def setodom(odom: Odometry): + def setodom(odom: PoseStamped): self.latest_odom = odom + def pubmove(move: Vector3): + print("PUBMOVE", move, "\n\n") + # print(self.movecmd) + try: + self.movecmd.publish(move) + except Exception as e: + print(traceback.format_exc()) + print(e) + self.odom.subscribe(setodom) - self.get_move_stream(frequency=20.0).subscribe(self.movecmd.publish) + self.get_move_stream(frequency=20.0).subscribe(pubmove) @dispatch def set_goal(self, goal: Path, stop_event=None, goal_theta=None) -> bool: @@ -114,14 +148,14 @@ def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: logger.info(f"Setting goal: {self.goal}") return True - def calc_move(self, direction: Vector) -> Vector: + def calc_move(self, direction: Vector) -> Optional[Vector]: """Calculate the movement vector based on the direction to the goal. Args: direction: Direction vector towards the goal Returns: - Movement vector scaled by speed + Movement vector scaled by speed, or None if error occurs """ try: # Normalize the direction vector and scale by speed @@ -130,7 +164,8 @@ def calc_move(self, direction: Vector) -> Vector: print("CALC MOVE", direction, normalized_direction, move_vector) return move_vector except Exception as e: - print("Error calculating move vector:", e) + logger.error(f"Error calculating move vector: {e}") + return None def spy(self, name: str): def spyfun(x): @@ -184,11 +219,11 @@ def _test_translational_movement(self) -> Vector: if phase < 0.5: # First half: move LEFT (positive X according to our documentation) - movement = Vector3(0.2, 0, 0) # Move left at 0.2 m/s + movement = Vector(0.2, 0, 0) # Move left at 0.2 m/s direction = "LEFT (positive X)" else: # Second half: move RIGHT (negative X according to our documentation) - movement = Vector3(-0.2, 0, 0) # Move right at 0.2 m/s + movement = Vector(-0.2, 0, 0) # Move right at 0.2 m/s direction = "RIGHT (negative X)" print("=== LEFT-RIGHT MOVEMENT TEST ===") @@ -207,11 +242,15 @@ def _calculate_rotation_to_target(self, direction_to_goal: Vector) -> Vector: Returns: Vector with (x=0, y=0, z=angular_velocity) for rotation only """ + if self.latest_odom is None: + logger.warning("No odometry data available for rotation calculation") + return Vector(0, 0, 0) + # Calculate the desired yaw angle to face the target desired_yaw = math.atan2(direction_to_goal.y, direction_to_goal.x) # Get current robot yaw - current_yaw = self.latest_odom.orientation.z + current_yaw = self.latest_odom.yaw # Calculate the yaw error using a more robust method to avoid oscillation yaw_error = math.atan2( @@ -247,13 +286,17 @@ def _calculate_rotation_to_target(self, direction_to_goal: Vector) -> Vector: # Return movement command: no translation (x=0, y=0), only rotation (z=angular_velocity) # Try flipping the sign in case the rotation convention is opposite - return Vector(0, 0, -angular_velocity) + return Vector3(0, 0, -angular_velocity) def _debug_direction(self, name: str, direction: Vector) -> Vector: """Debug helper to log direction information""" robot_pos = self.latest_odom + if robot_pos is None: + print(f"DEBUG {name}: direction={direction}, robot_pos=None, goal={self.goal}") + return direction + print( - f"DEBUG {name}: direction={direction}, robot_pos={robot_pos.position.to_2d()}, robot_yaw={math.degrees(robot_pos.rot.z):.1f}°, goal={self.goal}" + f"DEBUG {name}: direction={direction}, robot_pos={robot_pos.position.to_2d()}, robot_yaw={math.degrees(robot_pos.orientation.z):.1f}°, goal={self.goal}" ) return direction diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index 3d2127a871..a845247a0a 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -84,7 +84,7 @@ class RealRTC(WebRTCRobot): ... # inherit RealRTC instead of FakeRTC to run the real robot -class ConnectionModule(FakeRTC, Module): +class ConnectionModule(RealRTC, Module): movecmd: In[Vector3] = None odom: Out[Vector3] = None lidar: Out[LidarMessage] = None @@ -146,7 +146,6 @@ async def run(ip): # This enables LCM transport # Ensures system multicast, udp sizes are auto-adjusted if needed - # TODO: this doesn't seem to work atm and LCMTransport instantiation can fail pubsub.lcm.autoconf() connection.lidar.transport = core.LCMTransport("/lidar", LidarMessage) @@ -171,11 +170,10 @@ async def run(ip): global_planner.path.transport = core.pLCMTransport("/global_path") local_planner.path.connect(global_planner.path) - local_planner.odom.connect(connection.odom) local_planner.movecmd.transport = core.LCMTransport("/move", Vector3) - local_planner.movecmd.connect(connection.movecmd) + connection.movecmd.connect(local_planner.movecmd) ctrl = dimos.deploy(ControlModule) @@ -207,13 +205,12 @@ async def run(ip): print(colors.green("starting foxglove bridge")) foxglove_bridge.start() - # uncomment to move the bot print(colors.green("starting ctrl")) ctrl.start() print(colors.red("READY")) - await asyncio.sleep(10) + await asyncio.sleep(2) print("querying system") print(mapper.costmap()) while True: diff --git a/dimos/robot/unitree_webrtc/type/lidar.py b/dimos/robot/unitree_webrtc/type/lidar.py index f45cb8dfe7..bd5ccf9c0b 100644 --- a/dimos/robot/unitree_webrtc/type/lidar.py +++ b/dimos/robot/unitree_webrtc/type/lidar.py @@ -86,14 +86,6 @@ def from_msg(cls: "LidarMessage", raw_message: RawLidarMsg) -> "LidarMessage": raw_msg=raw_message, ) - def to_pointcloud2(self) -> PointCloud2: - """Convert to PointCloud2 message format.""" - return PointCloud2( - pointcloud=self.pointcloud, - frame_id=self.frame_id, - ts=self.ts, - ) - def __repr__(self): return f"LidarMessage(ts={to_human_readable(self.ts)}, origin={self.origin}, resolution={self.resolution}, {self.pointcloud})" From 7f1117cd71d694e596f72f02aec1c52470c7c54d Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 13:49:59 -0700 Subject: [PATCH 2/7] posestamped test --- dimos/msgs/geometry_msgs/test_PoseStamped.py | 55 ++++++++++++++++++++ flake.nix | 2 +- 2 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 dimos/msgs/geometry_msgs/test_PoseStamped.py diff --git a/dimos/msgs/geometry_msgs/test_PoseStamped.py b/dimos/msgs/geometry_msgs/test_PoseStamped.py new file mode 100644 index 0000000000..86dbf72bdc --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -0,0 +1,55 @@ +# Copyright 2025 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 pickle +import time + +from dimos.msgs.geometry_msgs import PoseStamped + + +def test_lcm_encode_decode(): + """Test encoding and decoding of Pose to/from binary LCM format.""" + + pose_source = PoseStamped( + ts=time.time(), + position=(1.0, 2.0, 3.0), + orientation=(0.1, 0.2, 0.3, 0.9), + ) + binary_msg = pose_source.lcm_encode() + pose_dest = PoseStamped.lcm_decode(binary_msg) + + assert isinstance(pose_dest, PoseStamped) + assert pose_dest is not pose_source + + print(pose_source.position) + print(pose_source.orientation) + + print(pose_dest.position) + print(pose_dest.orientation) + assert pose_dest == pose_source + + +def test_pickle_encode_decode(): + """Test encoding and decoding of PoseStamped to/from binary LCM format.""" + + pose_source = PoseStamped( + ts=time.time(), + position=(1.0, 2.0, 3.0), + orientation=(0.1, 0.2, 0.3, 0.9), + ) + binary_msg = pickle.dumps(pose_source) + pose_dest = pickle.loads(binary_msg) + assert isinstance(pose_dest, PoseStamped) + assert pose_dest is not pose_source + assert pose_dest == pose_source diff --git a/flake.nix b/flake.nix index 60751b156f..fe6749797e 100644 --- a/flake.nix +++ b/flake.nix @@ -21,7 +21,7 @@ ### Python + static analysis python312 python312Packages.pip python312Packages.setuptools - python312Packages.virtualenv ruff mypy pre-commit + python312Packages.virtualenv pre-commit ### Runtime deps python312Packages.pyaudio portaudio ffmpeg_6 ffmpeg_6.dev From 668e131a5550897b7fbdba6f7c03a9ced161abb0 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 14:08:13 -0700 Subject: [PATCH 3/7] type cleanup, path tests, path vector3 compat --- dimos/robot/global_planner/algo.py | 10 +- .../multiprocess/unitree_go2.py | 2 +- dimos/robot/unitree_webrtc/type/test_lidar.py | 18 -- dimos/types/path.py | 121 ++++----- dimos/types/test_path.py | 242 ++++++++++++++++++ 5 files changed, 312 insertions(+), 81 deletions(-) create mode 100644 dimos/types/test_path.py diff --git a/dimos/robot/global_planner/algo.py b/dimos/robot/global_planner/algo.py index 236725ce05..fad3cf13f5 100644 --- a/dimos/robot/global_planner/algo.py +++ b/dimos/robot/global_planner/algo.py @@ -12,13 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math import heapq -from typing import Optional, Tuple +import math from collections import deque -from dimos.types.path import Path -from dimos.types.vector import VectorLike, Vector +from typing import Optional, Tuple + +from dimos.msgs.geometry_msgs import Vector3 as Vector +from dimos.msgs.geometry_msgs.Vector3 import VectorLike from dimos.types.costmap import Costmap +from dimos.types.path import Path def find_nearest_free_cell( diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index a845247a0a..bf20449e62 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -84,7 +84,7 @@ class RealRTC(WebRTCRobot): ... # inherit RealRTC instead of FakeRTC to run the real robot -class ConnectionModule(RealRTC, Module): +class ConnectionModule(FakeRTC, Module): movecmd: In[Vector3] = None odom: Out[Vector3] = None lidar: Out[LidarMessage] = None diff --git a/dimos/robot/unitree_webrtc/type/test_lidar.py b/dimos/robot/unitree_webrtc/type/test_lidar.py index 912740a71a..75ceec88f8 100644 --- a/dimos/robot/unitree_webrtc/type/test_lidar.py +++ b/dimos/robot/unitree_webrtc/type/test_lidar.py @@ -31,21 +31,3 @@ def test_init(): assert isinstance(raw_frame, dict) frame = LidarMessage.from_msg(raw_frame) assert isinstance(frame, LidarMessage) - data = frame.to_pointcloud2().lcm_encode() - assert len(data) > 0 - assert isinstance(data, bytes) - - -@pytest.mark.tool -def test_publish(): - lcm = LCM() - lcm.start() - - topic = Topic(topic="/lidar", lcm_type=PointCloud2) - lidar = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) - - while True: - for frame in lidar.iterate(): - print(frame) - lcm.publish(topic, frame.to_pointcloud2()) - time.sleep(0.1) diff --git a/dimos/types/path.py b/dimos/types/path.py index c87658182f..dd1bf3603e 100644 --- a/dimos/types/path.py +++ b/dimos/types/path.py @@ -11,9 +11,14 @@ # 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 __future__ import annotations + +from typing import Iterator, List, Tuple, TypeVar, Union import numpy as np -from typing import List, Union, Tuple, Iterator, TypeVar +from dimos_lcm.nav_msgs import Path as LCMPPath + +from dimos.msgs.geometry_msgs import Vector3 from dimos.types.vector import Vector T = TypeVar("T", bound="Path") @@ -22,18 +27,30 @@ class Path: """A class representing a path as a sequence of points.""" + msg_name = "nav_msgs.Path" + + def lcm_encode(self) -> bytes: + """Encode the path to a bytes representation for LCM.""" + for point in self: + print(point) + + lcmpath = LCMPPath() + lcmpath.header.frame_id = "map" + lcmpath.header.stamp = 0 # Placeholder for timestamp + # Convert points to LCM format + def __init__( self, - points: Union[List[Vector], List[np.ndarray], List[Tuple], np.ndarray, None] = None, + points: Union[List[Vector3], List[np.ndarray], List[Tuple], np.ndarray, None] = None, ): """Initialize a path from a list of points. Args: - points: List of Vector objects, numpy arrays, tuples, or a 2D numpy array where each row is a point. + points: List of Vector3 objects, numpy arrays, tuples, or a 2D numpy array where each row is a point. If None, creates an empty path. Examples: - Path([Vector(1, 2), Vector(3, 4)]) # from Vector objects + Path([Vector3(1, 2), Vector(3, 4)]) # from Vector objects Path([(1, 2), (3, 4)]) # from tuples Path(np.array([[1, 2], [3, 4]])) # from 2D numpy array """ @@ -48,14 +65,14 @@ def __init__( # Convert various input types to numpy array converted = [] for p in points: - if isinstance(p, Vector): + if isinstance(p, Vector3) or isinstance(p, Vector): converted.append(p.data) else: converted.append(p) self._points = np.array(converted, dtype=float) - def serialize(self) -> Tuple: - """Serialize the vector to a tuple.""" + def serialize(self) -> dict: + """Serialize the path to a dictionary.""" return { "type": "path", "points": self._points.tolist(), @@ -66,17 +83,17 @@ def points(self) -> np.ndarray: """Get the path points as a numpy array.""" return self._points - def as_vectors(self) -> List[Vector]: - """Get the path points as Vector objects.""" - return [Vector(p) for p in self._points] + def as_vectors(self) -> List[Vector3]: + """Get the path points as Vector3 objects.""" + return [Vector3(p) for p in self._points] - def append(self, point: Union[Vector, np.ndarray, Tuple]) -> None: + def append(self, point: Union[Vector3, np.ndarray, Tuple]) -> None: """Append a point to the path. Args: - point: A Vector, numpy array, or tuple representing a point + point: A Vector3, numpy array, or tuple representing a point """ - if isinstance(point, Vector): + if isinstance(point, Vector3): point_data = point.data else: point_data = np.array(point, dtype=float) @@ -87,7 +104,7 @@ def append(self, point: Union[Vector, np.ndarray, Tuple]) -> None: else: self._points = np.vstack((self._points, point_data)) - def extend(self, points: Union[List[Vector], List[np.ndarray], List[Tuple], "Path"]) -> None: + def extend(self, points: Union[List[Vector3], List[np.ndarray], List[Tuple], "Path"]) -> None: """Extend the path with more points. Args: @@ -102,14 +119,14 @@ def extend(self, points: Union[List[Vector], List[np.ndarray], List[Tuple], "Pat for point in points: self.append(point) - def insert(self, index: int, point: Union[Vector, np.ndarray, Tuple]) -> None: + def insert(self, index: int, point: Union[Vector3, np.ndarray, Tuple]) -> None: """Insert a point at a specific index. Args: index: The index at which to insert the point - point: A Vector, numpy array, or tuple representing a point + point: A Vector3, numpy array, or tuple representing a point """ - if isinstance(point, Vector): + if isinstance(point, Vector3): point_data = point.data else: point_data = np.array(point, dtype=float) @@ -295,7 +312,7 @@ def smooth(self: T, weight: float = 0.5, iterations: int = 1) -> T: return self.__class__(smoothed_points) - def nearest_point_index(self, point: Union[Vector, np.ndarray, Tuple]) -> int: + def nearest_point_index(self, point: Union[Vector3, np.ndarray, Tuple]) -> int: """Find the index of the closest point on the path to the given point. Args: @@ -307,7 +324,7 @@ def nearest_point_index(self, point: Union[Vector, np.ndarray, Tuple]) -> int: if len(self._points) == 0: raise ValueError("Cannot find nearest point in an empty path") - if isinstance(point, Vector): + if isinstance(point, Vector3): point_data = point.data else: point_data = np.array(point, dtype=float) @@ -331,36 +348,41 @@ def __len__(self) -> int: """Return the number of points in the path.""" return len(self._points) - def __getitem__(self, idx) -> Union[np.ndarray, "Path"]: + def __getitem__(self, idx) -> Union[np.ndarray, Path]: """Get a point or slice of points from the path.""" if isinstance(idx, slice): return self.__class__(self._points[idx]) return self._points[idx].copy() - def get_vector(self, idx: int) -> Vector: - """Get a point at the given index as a Vector object.""" - return Vector(self._points[idx]) + def get_vector(self, idx: int) -> Vector3: + """Get a point at the given index as a Vector3 object.""" + return Vector3(self._points[idx]) - def last(self) -> Vector: - """Get the first point in the path as a Vector object.""" + def last(self) -> Vector3: + """Get the last point in the path as a Vector3 object.""" if len(self._points) == 0: return None - return Vector(self._points[-1]) + return Vector3(self._points[-1]) - def head(self) -> Vector: - """Get the first point in the path as a Vector object.""" + def head(self) -> Vector3: + """Get the first point in the path as a Vector3 object.""" if len(self._points) == 0: return None - return Vector(self._points[0]) + return Vector3(self._points[0]) - def tail(self) -> "Path": + def tail(self) -> Path: """Get all points except the first point as a new Path object.""" if len(self._points) <= 1: return None return self.__class__(self._points[1:].copy()) - def __iter__(self) -> Iterator[np.ndarray]: + def vectors(self) -> Iterator[Vector3]: """Iterate over the points in the path.""" + for point in self._points: + yield Vector3(*point) + + def __iter__(self) -> Iterator[np.ndarray]: + """Iterate over the points in the path as numpy arrays.""" for point in self._points: yield point.copy() @@ -368,9 +390,9 @@ def __repr__(self) -> str: """String representation of the path.""" return f"↶ Path ({len(self._points)} Points)" - def ipush(self, point: Union[Vector, np.ndarray, Tuple]) -> "Path": + def ipush(self, point: Union[Vector3, np.ndarray, Tuple]) -> "Path": """Return a new Path with `point` appended.""" - if isinstance(point, Vector): + if isinstance(point, Vector3): p = point.data else: p = np.asarray(point, dtype=float) @@ -387,28 +409,11 @@ def iclip_tail(self, max_len: int) -> "Path": raise ValueError("max_len must be ≥ 0") return self.__class__(self._points[-max_len:]) - def __add__(self, point): - """path + vec -> path.pushed(vec)""" - return self.pushed(point) - - -if __name__ == "__main__": - # Test vectors in various directions - print( - Path( - [ - Vector(1, 0), # Right - Vector(1, 1), # Up-Right - Vector(0, 1), # Up - Vector(-1, 1), # Up-Left - Vector(-1, 0), # Left - Vector(-1, -1), # Down-Left - Vector(0, -1), # Down - Vector(1, -1), # Down-Right - Vector(0.5, 0.5), # Up-Right (shorter) - Vector(-3, 4), # Up-Left (longer) - ] - ) - ) - - print(Path()) + def __add__(self, other): + """path + point -> path.ipush(point) or path + path -> path.extend(path)""" + if isinstance(other, Path): + new_path = Path(self._points.copy()) + new_path.extend(other) + return new_path + else: + return self.ipush(other) diff --git a/dimos/types/test_path.py b/dimos/types/test_path.py new file mode 100644 index 0000000000..36ae1563b6 --- /dev/null +++ b/dimos/types/test_path.py @@ -0,0 +1,242 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.types.path import Path +from dimos.types.vector import Vector, Vector3 + + +@pytest.fixture +def path(): + return Path([(1, 2, 3), (4, 5, 6), (7, 8, 9)]) + + +@pytest.fixture +def empty_path(): + return Path() + + +def test_init(path): + assert path.length() == 10.392304845413264 + assert len(path) == 3 + assert np.array_equal(path[1], [4.0, 5.0, 6.0]) + + +def test_init_empty(): + empty = Path() + assert len(empty) == 0 + assert empty.length() == 0.0 + + +def test_init_Vector3(): + points = map((lambda p: Vector3(p)), [[1, 2], [3, 4], [5, 6]]) + path = Path(points) + print(path) + + +def test_init_numpy_array(): + points = np.array([[1, 2], [3, 4], [5, 6]]) + path = Path(points) + assert len(path) == 3 + assert np.array_equal(path[0], [1.0, 2.0]) + + +def test_add_path(path): + path2 = Path([(10, 11, 12)]) + result = path + path2 + assert len(result) == 4 + assert np.array_equal(result[3], [10.0, 11.0, 12.0]) + + +def test_add_point(path): + result = path + (10, 11, 12) + assert len(result) == 4 + assert np.array_equal(result[3], [10.0, 11.0, 12.0]) + + +def test_append(path): + original_len = len(path) + path.append((10, 11, 12)) + assert len(path) == original_len + 1 + assert np.array_equal(path[-1], [10.0, 11.0, 12.0]) + + +def test_extend(path): + path2 = Path([(10, 11, 12), (13, 14, 15)]) + original_len = len(path) + path.extend(path2) + assert len(path) == original_len + 2 + assert np.array_equal(path[-1], [13.0, 14.0, 15.0]) + + +def test_insert(path): + path.insert(1, (10, 11, 12)) + assert len(path) == 4 + assert np.array_equal(path[1], [10.0, 11.0, 12.0]) + assert np.array_equal(path[2], [4.0, 5.0, 6.0]) # Original point shifted + + +def test_remove(path): + removed = path.remove(1) + assert len(path) == 2 + assert np.array_equal(removed, [4.0, 5.0, 6.0]) + assert np.array_equal(path[1], [7.0, 8.0, 9.0]) # Next pointhey ca shifted down + + +def test_clear(path): + path.clear() + assert len(path) == 0 + + +def test_resample(path): + resampled = path.resample(2.0) + assert len(resampled) >= 2 + # Resampling can create more points than original * 2 for complex paths + assert len(resampled) > 0 + + +def test_simplify(path): + simplified = path.simplify(0.1) + assert len(simplified) <= len(path) + assert len(simplified) >= 2 + + +def test_smooth(path): + smoothed = path.smooth(0.5, 1) + assert len(smoothed) == len(path) + assert np.array_equal(smoothed[0], path[0]) # First point unchanged + assert np.array_equal(smoothed[-1], path[-1]) # Last point unchanged + + +def test_nearest_point_index(path): + idx = path.nearest_point_index((4, 5, 6)) + assert idx == 1 + + idx = path.nearest_point_index((1, 2, 3)) + assert idx == 0 + + +def test_nearest_point_index_empty(): + empty = Path() + with pytest.raises(ValueError): + empty.nearest_point_index((1, 2, 3)) + + +def test_reverse(path): + reversed_path = path.reverse() + assert len(reversed_path) == len(path) + assert np.array_equal(reversed_path[0], path[-1]) + assert np.array_equal(reversed_path[-1], path[0]) + + +def test_getitem_slice(path): + slice_path = path[1:3] + assert isinstance(slice_path, Path) + assert len(slice_path) == 2 + assert np.array_equal(slice_path[0], [4.0, 5.0, 6.0]) + + +def test_get_vector(path): + vector = path.get_vector(1) + assert isinstance(vector, Vector3) + assert vector == Vector3([4.0, 5.0, 6.0]) + + +def test_head_tail_last(path): + head = path.head() + assert isinstance(head, Vector3) + assert head == Vector3([1.0, 2.0, 3.0]) + + last = path.last() + assert isinstance(last, Vector3) + assert last == Vector3([7.0, 8.0, 9.0]) + + tail = path.tail() + assert isinstance(tail, Path) + assert len(tail) == 2 + assert np.array_equal(tail[0], [4.0, 5.0, 6.0]) + + +def test_head_tail_last_empty(): + empty = Path() + assert empty.head() is None + assert empty.last() is None + assert empty.tail() is None + + +def test_iter(path): + arrays = list(path) + assert len(arrays) == 3 + assert all(isinstance(arr, np.ndarray) for arr in arrays) + assert np.array_equal(arrays[0], [1.0, 2.0, 3.0]) + + +def test_vectors(path): + vectors = list(path.vectors()) + assert len(vectors) == 3 + assert all(isinstance(v, Vector3) for v in vectors) + assert vectors[0] == Vector3([1.0, 2.0, 3.0]) + + +def test_repr(path): + repr_str = repr(path) + assert "Path" in repr_str + assert "3 Points" in repr_str + + +def test_ipush(path): + new_path = path.ipush((10, 11, 12)) + assert len(new_path) == 4 + assert len(path) == 3 # Original unchanged + assert np.array_equal(new_path[-1], [10.0, 11.0, 12.0]) + + +def test_iclip_tail(path): + clipped = path.iclip_tail(2) + assert len(clipped) == 2 + assert np.array_equal(clipped[0], [4.0, 5.0, 6.0]) + assert np.array_equal(clipped[1], [7.0, 8.0, 9.0]) + + +def test_iclip_tail_negative(): + path = Path([(1, 2, 3)]) + with pytest.raises(ValueError): + path.iclip_tail(-1) + + +def test_serialize(path): + serialized = path.serialize() + assert isinstance(serialized, dict) + assert serialized["type"] == "path" + assert serialized["points"] == [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]] + + +def test_as_vectors(path): + vectors = path.as_vectors() + assert len(vectors) == 3 + assert all(isinstance(v, Vector3) for v in vectors) + assert vectors[0] == Vector3([1.0, 2.0, 3.0]) + + +def test_points_property(path): + points = path.points + assert isinstance(points, np.ndarray) + assert points.shape == (3, 3) + assert np.array_equal(points[0], [1.0, 2.0, 3.0]) + + +# def test_lcm_encode_decode(path): +# print(path.lcm_encode()) From 569a9b5ac9922ece85f5646d483e6d4a807193bf Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 14:25:41 -0700 Subject: [PATCH 4/7] ros polyfil switched to LCM --- dimos/types/ros_polyfill.py | 86 +++---------------------------------- 1 file changed, 5 insertions(+), 81 deletions(-) diff --git a/dimos/types/ros_polyfill.py b/dimos/types/ros_polyfill.py index b5c2bc1d64..1bb4ece7fb 100644 --- a/dimos/types/ros_polyfill.py +++ b/dimos/types/ros_polyfill.py @@ -15,89 +15,13 @@ try: from geometry_msgs.msg import Vector3 except ImportError: - - class Vector3: - def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0): - self.x = float(x) - self.y = float(y) - self.z = float(z) - - def __repr__(self) -> str: - return f"Vector3(x={self.x}, y={self.y}, z={self.z})" - + from dimos.msgs.geometry_msgs import Vector3 # type: ignore[import] try: + from geometry_msgs.msg import Point, Pose, Quaternion, Twist from nav_msgs.msg import OccupancyGrid, Odometry - from geometry_msgs.msg import Pose, Point, Quaternion, Twist from std_msgs.msg import Header except ImportError: - - class Header: - def __init__(self): - self.stamp = None - self.frame_id = "" - - class Point: - def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0): - self.x = float(x) - self.y = float(y) - self.z = float(z) - - def __repr__(self) -> str: - return f"Point(x={self.x}, y={self.y}, z={self.z})" - - class Quaternion: - def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, w: float = 1.0): - self.x = float(x) - self.y = float(y) - self.z = float(z) - self.w = float(w) - - def __repr__(self) -> str: - return f"Quaternion(x={self.x}, y={self.y}, z={self.z}, w={self.w})" - - class Pose: - def __init__(self): - self.position = Point() - self.orientation = Quaternion() - - def __repr__(self) -> str: - return f"Pose(position={self.position}, orientation={self.orientation})" - - class MapMetaData: - def __init__(self): - self.map_load_time = None - self.resolution = 0.05 - self.width = 0 - self.height = 0 - self.origin = Pose() - - def __repr__(self) -> str: - return f"MapMetaData(resolution={self.resolution}, width={self.width}, height={self.height}, origin={self.origin})" - - class Twist: - def __init__(self): - self.linear = Vector3() - self.angular = Vector3() - - def __repr__(self) -> str: - return f"Twist(linear={self.linear}, angular={self.angular})" - - class OccupancyGrid: - def __init__(self): - self.header = Header() - self.info = MapMetaData() - self.data = [] - - def __repr__(self) -> str: - return f"OccupancyGrid(info={self.info}, data_length={len(self.data)})" - - class Odometry: - def __init__(self): - self.header = Header() - self.child_frame_id = "" - self.pose = Pose() - self.twist = Twist() - - def __repr__(self) -> str: - return f"Odometry(pose={self.pose}, twist={self.twist})" + from dimos_lcm.geometry_msgs import Point, Pose, Quaternion, Twist + from dimos_lcm.nav_msgs import OccupancyGrid, Odometry + from dimos_lcm.std_msgs import Header From a2a20d52b22b146b512c55e9ae3e339ac4b91679 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 17:01:33 -0700 Subject: [PATCH 5/7] disable pytest deprecation warnings by default --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index afe184a759..b06e9139b0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -182,6 +182,7 @@ files = [ "dimos/msgs/**/*.py", "dimos/protocol/**/*.py" ] +addopts = "--disable-warnings" [tool.pytest.ini_options] testpaths = ["dimos"] From 01edd91c2d8389982e110230f88a1386b28c4c64 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 17:03:19 -0700 Subject: [PATCH 6/7] ros polyfil for path vector3 --- dimos/types/path.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/types/path.py b/dimos/types/path.py index dd1bf3603e..f39248ebf1 100644 --- a/dimos/types/path.py +++ b/dimos/types/path.py @@ -18,7 +18,7 @@ import numpy as np from dimos_lcm.nav_msgs import Path as LCMPPath -from dimos.msgs.geometry_msgs import Vector3 +from dimos.types.ros_polyfill import Vector3 from dimos.types.vector import Vector T = TypeVar("T", bound="Path") From 3c9c09091b5f4a2b6d55e365721aebc3592dd64b Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 16 Jul 2025 17:07:05 -0700 Subject: [PATCH 7/7] fixed path ros dep --- dimos/types/path.py | 2 +- dimos/types/test_path.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/types/path.py b/dimos/types/path.py index f39248ebf1..dd1bf3603e 100644 --- a/dimos/types/path.py +++ b/dimos/types/path.py @@ -18,7 +18,7 @@ import numpy as np from dimos_lcm.nav_msgs import Path as LCMPPath -from dimos.types.ros_polyfill import Vector3 +from dimos.msgs.geometry_msgs import Vector3 from dimos.types.vector import Vector T = TypeVar("T", bound="Path") diff --git a/dimos/types/test_path.py b/dimos/types/test_path.py index 36ae1563b6..3f69002963 100644 --- a/dimos/types/test_path.py +++ b/dimos/types/test_path.py @@ -15,8 +15,8 @@ import numpy as np import pytest +from dimos.msgs.geometry_msgs import Vector3 from dimos.types.path import Path -from dimos.types.vector import Vector, Vector3 @pytest.fixture