diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 54c61d2feb..c7eb4dfca7 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -36,6 +36,7 @@ jobs: - docker/python/** - requirements*.txt - requirements.txt + - pyproject.toml dev: - docker/dev/** diff --git a/bin/foxglove-bridge b/bin/foxglove-bridge new file mode 100755 index 0000000000..8d80ac52cd --- /dev/null +++ b/bin/foxglove-bridge @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +# current script dir + ..dimos + + +script_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +python $script_dir/../dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py "$@" diff --git a/bin/lcmspy b/bin/lcmspy index 0efa14fce3..64387aad98 100755 --- a/bin/lcmspy +++ b/bin/lcmspy @@ -4,4 +4,4 @@ script_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -python $script_dir/../dimos/utils/cli/lcmspy_cli.py "$@" +python $script_dir/../dimos/utils/cli/lcmspy/run_lcmspy.py "$@" diff --git a/dimos/msgs/foxglove_msgs/Arrow.py b/dimos/msgs/foxglove_msgs/Arrow.py deleted file mode 100644 index c2087071b1..0000000000 --- a/dimos/msgs/foxglove_msgs/Arrow.py +++ /dev/null @@ -1,111 +0,0 @@ -# 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. - -from __future__ import annotations - -from dataclasses import asdict, dataclass -from typing import TYPE_CHECKING, Optional, TypedDict - -import numpy as np -from dimos_lcm.foxglove_msgs import ArrowPrimitive, Color, LinePrimitive -from dimos_lcm.geometry_msgs import Point, Vector3 - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.geometry_msgs.Twist import Twist - - -class ArrowConfig(TypedDict, total=False): - shaft_diameter: float - head_diameter: float - head_length_ratio: float - head_length: Optional[float] - color: tuple[float, float, float, float] - - -default_config: ArrowConfig = { - "shaft_diameter": 0.02, - "head_diameter": 2.0, - "head_length_ratio": 1.0, - "head_length": None, - "color": (1.0, 0.0, 0.0, 1.0), -} - - -class Arrow(ArrowPrimitive): - @classmethod - def from_transform( - cls, - transform: "Transform", - pose: "Pose | PoseStamped", - arrow_config: ArrowConfig = {}, - ): - config = {**default_config, **arrow_config} - - # Apply transform to pose to get end position - transformed_pose = pose @ transform - - # Calculate arrow vector using Vector3 operations - arrow_vec = transformed_pose.position - pose.position - length = arrow_vec.length() - - # Calculate head length - head_length = config["head_length"] - if head_length is None: - head_length = config["head_diameter"] * config["head_length_ratio"] - - # Create arrow geometry - arrow = cls() - - # Set the pose (start position and orientation) - arrow.pose = pose - - # Set arrow properties using the actual ArrowPrimitive fields - arrow.shaft_length = length - arrow.shaft_diameter = config["shaft_diameter"] - arrow.head_length = head_length - arrow.head_diameter = config["head_diameter"] - - # Set color - arrow.color = Color( - r=config["color"][0], g=config["color"][1], b=config["color"][2], a=config["color"][3] - ) - - return arrow - - def lcm_encode(self) -> bytes: - """Encode Arrow to LCM binary format.""" - return self.encode() - - @classmethod - def lcm_decode(cls, data: bytes): - """Decode Arrow from LCM binary format. - - Note: This returns an Arrow instance, not ArrowPrimitive. - """ - # First decode as ArrowPrimitive - arrow_primitive = ArrowPrimitive.decode(data) - - # Create a new Arrow instance and copy all fields - arrow = cls() - arrow.pose = arrow_primitive.pose - arrow.shaft_length = arrow_primitive.shaft_length - arrow.shaft_diameter = arrow_primitive.shaft_diameter - arrow.head_length = arrow_primitive.head_length - arrow.head_diameter = arrow_primitive.head_diameter - arrow.color = arrow_primitive.color - - return arrow diff --git a/dimos/msgs/foxglove_msgs/__init__.py b/dimos/msgs/foxglove_msgs/__init__.py deleted file mode 100644 index 638a3480f2..0000000000 --- a/dimos/msgs/foxglove_msgs/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from dimos.msgs.foxglove_msgs.Arrow import Arrow diff --git a/dimos/msgs/foxglove_msgs/test_Arrow.py b/dimos/msgs/foxglove_msgs/test_Arrow.py deleted file mode 100644 index 511ce5a01c..0000000000 --- a/dimos/msgs/foxglove_msgs/test_Arrow.py +++ /dev/null @@ -1,252 +0,0 @@ -# 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. - -from typing import Optional, TypedDict - -import numpy as np - -from dimos.msgs.foxglove_msgs.Arrow import Arrow -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 - - -class ArrowConfigDict(TypedDict, total=False): - shaft_diameter: float - head_diameter: float - head_length_ratio: float - head_length: Optional[float] - color: tuple[float, float, float, float] - - -def test_arrow_from_transform_basic(): - """Test basic arrow creation from pose and transform.""" - # Create a pose at origin - pose = Pose(1.0, 2.0, 3.0) - - # Create a transform that moves 2 units in x direction - transform = Transform(translation=Vector3(2.0, 0.0, 0.0)) - - # Create arrow - arrow = Arrow.from_transform(transform, pose) - - # Check that arrow pose matches input pose - assert arrow.pose.position.x == 1.0 - assert arrow.pose.position.y == 2.0 - assert arrow.pose.position.z == 3.0 - - # Check that shaft length matches the transform magnitude - expected_length = 2.0 # magnitude of Vector3(2.0, 0.0, 0.0) - assert np.isclose(arrow.shaft_length, expected_length, atol=1e-10) - - # Check default configuration values - assert arrow.shaft_diameter == 0.02 - assert arrow.head_diameter == 2.0 - assert arrow.color.r == 1.0 - assert arrow.color.g == 0.0 - assert arrow.color.b == 0.0 - assert arrow.color.a == 1.0 - - -def test_arrow_from_transform_with_config(): - """Test arrow creation with custom configuration.""" - pose = Pose(0.0, 0.0, 0.0) - transform = Transform(translation=Vector3(1.0, 1.0, 0.0)) - - # Custom configuration - config = { - "shaft_diameter": 0.05, - "head_diameter": 1.5, - "color": (0.0, 1.0, 0.0, 0.8), # Green with transparency - } - - arrow = Arrow.from_transform(transform, pose, config) - - # Check custom values were applied - assert arrow.shaft_diameter == 0.05 - assert arrow.head_diameter == 1.5 - assert arrow.color.r == 0.0 - assert arrow.color.g == 1.0 - assert arrow.color.b == 0.0 - assert arrow.color.a == 0.8 - - # Check shaft length matches transform magnitude - expected_length = np.sqrt(2.0) # magnitude of Vector3(1.0, 1.0, 0.0) - assert np.isclose(arrow.shaft_length, expected_length, atol=1e-10) - - -def test_arrow_from_transform_zero_length(): - """Test arrow creation with zero-length transform.""" - pose = Pose(5.0, 5.0, 5.0) - - # Zero transform (no movement) - identity transform - transform = Transform() - - arrow = Arrow.from_transform( - transform, - pose, - ) - - # Arrow should have zero length - assert arrow.shaft_length == 0.0 - - # Pose should be preserved - assert arrow.pose.position.x == 5.0 - assert arrow.pose.position.y == 5.0 - assert arrow.pose.position.z == 5.0 - - -def test_arrow_head_length_calculation(): - """Test head length calculation with and without explicit setting.""" - pose = Pose() - transform = Transform(translation=Vector3(1.0, 0.0, 0.0)) - - # Test with default head length (should be head_diameter * head_length_ratio) - arrow1 = Arrow.from_transform( - transform, - pose, - ) - expected_head_length = 2.0 * 1.0 # head_diameter * head_length_ratio - assert arrow1.head_length == expected_head_length - - # Test with explicit head length - config = {"head_length": 0.5} - arrow2 = Arrow.from_transform(transform, pose, config) - assert arrow2.head_length == 0.5 - - # Test with custom ratio - config = {"head_length_ratio": 2.0} - arrow3 = Arrow.from_transform(transform, pose, config) - expected_head_length = 2.0 * 2.0 # head_diameter * custom_ratio - assert arrow3.head_length == expected_head_length - - -def test_arrow_3d_transform(): - """Test arrow with 3D translation vector.""" - pose = Pose(1.0, 1.0, 1.0) - transform = Transform(translation=Vector3(2.0, 3.0, 6.0)) # magnitude = 7.0 - - arrow = Arrow.from_transform(transform, pose) - - expected_length = 7.0 # sqrt(2^2 + 3^2 + 6^2) - assert np.isclose(arrow.shaft_length, expected_length, atol=1e-10) - - # Verify the arrow starts at the original pose - assert arrow.pose.position.x == 1.0 - assert arrow.pose.position.y == 1.0 - assert arrow.pose.position.z == 1.0 - - -def test_arrow_lcm_encode_decode(): - """Test LCM encoding and decoding of Arrow.""" - # Create an arrow using from_transform - pose = Pose(1.0, 2.0, 3.0, 0.0, 0.0, 0.707107, 0.707107) # 90 deg around Z - transform = Transform(translation=Vector3(2.0, 1.0, 0.5)) - config = { - "shaft_diameter": 0.03, - "head_diameter": 1.8, - "head_length": 0.4, - "color": (0.2, 0.8, 0.3, 0.9), - } - - arrow_source = Arrow.from_transform(transform, pose, config) - - # Encode to binary - binary_msg = arrow_source.lcm_encode() - - # Decode from binary - arrow_dest = Arrow.lcm_decode(binary_msg) - - # Verify it's a new instance of Arrow (not ArrowPrimitive) - assert isinstance(arrow_dest, Arrow) - assert arrow_dest is not arrow_source - - # Verify all fields match - assert np.isclose(arrow_dest.pose.position.x, arrow_source.pose.position.x, atol=1e-10) - assert np.isclose(arrow_dest.pose.position.y, arrow_source.pose.position.y, atol=1e-10) - assert np.isclose(arrow_dest.pose.position.z, arrow_source.pose.position.z, atol=1e-10) - assert np.isclose(arrow_dest.pose.orientation.x, arrow_source.pose.orientation.x, atol=1e-10) - assert np.isclose(arrow_dest.pose.orientation.y, arrow_source.pose.orientation.y, atol=1e-10) - assert np.isclose(arrow_dest.pose.orientation.z, arrow_source.pose.orientation.z, atol=1e-10) - assert np.isclose(arrow_dest.pose.orientation.w, arrow_source.pose.orientation.w, atol=1e-10) - - assert np.isclose(arrow_dest.shaft_length, arrow_source.shaft_length, atol=1e-10) - assert np.isclose(arrow_dest.shaft_diameter, arrow_source.shaft_diameter, atol=1e-10) - assert np.isclose(arrow_dest.head_length, arrow_source.head_length, atol=1e-10) - assert np.isclose(arrow_dest.head_diameter, arrow_source.head_diameter, atol=1e-10) - - assert np.isclose(arrow_dest.color.r, arrow_source.color.r, atol=1e-10) - assert np.isclose(arrow_dest.color.g, arrow_source.color.g, atol=1e-10) - assert np.isclose(arrow_dest.color.b, arrow_source.color.b, atol=1e-10) - assert np.isclose(arrow_dest.color.a, arrow_source.color.a, atol=1e-10) - - -def test_arrow_from_transform_with_posestamped(): - """Test arrow creation from PoseStamped and transform.""" - # Create a PoseStamped - pose_stamped = PoseStamped( - ts=1234567890.123, - frame_id="base_link", - position=Vector3(2.0, 3.0, 4.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - # Create a transform - transform = Transform( - translation=Vector3(3.0, 0.0, 0.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) - ) - - # Create arrow - arrow = Arrow.from_transform(transform, pose_stamped) - - # Check that arrow pose matches input pose_stamped - assert arrow.pose.position.x == 2.0 - assert arrow.pose.position.y == 3.0 - assert arrow.pose.position.z == 4.0 - - # Check that shaft length matches the transform magnitude - expected_length = 3.0 # magnitude of Vector3(3.0, 0.0, 0.0) - assert np.isclose(arrow.shaft_length, expected_length, atol=1e-10) - - # Verify the arrow properties are set correctly - assert arrow.shaft_diameter == 0.02 - assert arrow.head_diameter == 2.0 - assert arrow.color.r == 1.0 - assert arrow.color.g == 0.0 - assert arrow.color.b == 0.0 - assert arrow.color.a == 1.0 - - -def test_arrow_complex_transform_with_rotation(): - """Test arrow with transform that includes both translation and rotation.""" - # Create a pose at origin facing forward - pose = Pose(0.0, 0.0, 0.0) - - # Create a transform with translation and 45 degree rotation around Z - angle = np.pi / 4 # 45 degrees - transform = Transform( - translation=Vector3(2.0, 2.0, 0.0), - rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), - ) - - # Create arrow - arrow = Arrow.from_transform(transform, pose) - - # The arrow vector should be from pose position to transformed position - # Since pose is at origin, the transformed position is just the translation - expected_length = np.sqrt(8.0) # magnitude of Vector3(2.0, 2.0, 0.0) - assert np.isclose(arrow.shaft_length, expected_length, atol=1e-10) - - # Arrow should start at the original pose - assert arrow.pose.position.x == 0.0 - assert arrow.pose.position.y == 0.0 - assert arrow.pose.position.z == 0.0 diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index 0a32210be0..09d196059b 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -41,22 +41,6 @@ class Pose(LCMPose): orientation: Quaternion msg_name = "geometry_msgs.Pose" - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - traceback.print_exc() - raise ValueError("Decode error") - return cls._lcm_decode_one(data) - - @classmethod - def _lcm_decode_one(cls, buf): - return cls(Vector3._decode_one(buf), Quaternion._decode_one(buf)) - - def lcm_encode(self) -> bytes: - return super().encode() - @dispatch def __init__(self) -> None: """Initialize a pose at origin with identity orientation.""" @@ -172,7 +156,7 @@ def __eq__(self, other) -> bool: def __matmul__(self, transform: LCMTransform | Transform) -> Pose: return self + transform - def find_transform(self, other: PoseConvertable) -> LCMTransform: + def find_transform(self, other: PoseConvertable) -> Transform: other_pose = to_pose(other) if not isinstance(other, Pose) else other inv_orientation = self.orientation.conjugate() @@ -183,11 +167,10 @@ def find_transform(self, other: PoseConvertable) -> LCMTransform: relative_rotation = inv_orientation * other_pose.orientation - transform = LCMTransform() - transform.translation = local_translation - transform.rotation = relative_rotation - - return transform + return Transform( + translation=local_translation, + rotation=relative_rotation, + ) def __add__(self, other: "Pose" | PoseConvertable | LCMTransform | Transform) -> "Pose": """Compose two poses or apply a transform (transform composition). diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py index e0936964d8..bc41a40844 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -56,13 +56,13 @@ def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None: def lcm_encode(self) -> bytes: lcm_mgs = LCMPoseStamped() lcm_mgs.pose = self - [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.sec] = sec_nsec(self.ts) + [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.nsec] = sec_nsec(self.ts) lcm_mgs.header.frame_id = self.frame_id - return lcm_mgs.encode() + return lcm_mgs.lcm_encode() @classmethod def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped: - lcm_msg = LCMPoseStamped.decode(data) + lcm_msg = LCMPoseStamped.lcm_decode(data) return cls( ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), frame_id=lcm_msg.header.frame_id, diff --git a/dimos/msgs/geometry_msgs/Quaternion.py b/dimos/msgs/geometry_msgs/Quaternion.py index 2a7e398379..9879e1e263 100644 --- a/dimos/msgs/geometry_msgs/Quaternion.py +++ b/dimos/msgs/geometry_msgs/Quaternion.py @@ -48,9 +48,6 @@ def lcm_decode(cls, data: bytes | BinaryIO): def _lcm_decode_one(cls, buf): return cls(struct.unpack(">dddd", buf.read(32))) - def lcm_encode(self): - return super().encode() - @dispatch def __init__(self) -> None: ... diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index a4a75993fb..267575dcb5 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -15,83 +15,42 @@ from __future__ import annotations import struct +import time from io import BytesIO from typing import BinaryIO from dimos_lcm.geometry_msgs import Transform as LCMTransform +from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped from plum import dispatch -from .Quaternion import Quaternion -from .Vector3 import Vector3 +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs import Header +from dimos.types.timestamped import Timestamped -class Transform(LCMTransform): +class Transform(Timestamped): translation: Vector3 rotation: Quaternion - msg_name = "geometry_msgs.Transform" - - @dispatch - def __init__(self) -> None: - """Initialize an identity transform.""" - self.translation = Vector3() - self.rotation = Quaternion() - - @dispatch - def __init__(self, translation: Vector3) -> None: - """Initialize a transform with only translation (identity rotation).""" - self.translation = translation - self.rotation = Quaternion() - - @dispatch - def __init__(self, rotation: Quaternion) -> None: - """Initialize a transform with only rotation (zero translation).""" - self.translation = Vector3() - self.rotation = rotation - - @dispatch - def __init__(self, translation: Vector3, rotation: Quaternion) -> None: - """Initialize a transform from translation and rotation.""" - self.translation = translation - self.rotation = rotation - - @dispatch - def __init__(self, transform: "Transform") -> None: - """Initialize from another Transform (copy constructor).""" - self.translation = Vector3(transform.translation) - self.rotation = Quaternion(transform.rotation) - - @dispatch - def __init__(self, lcm_transform: LCMTransform) -> None: - """Initialize from an LCM Transform.""" - self.translation = Vector3(lcm_transform.translation) - self.rotation = Quaternion(lcm_transform.rotation) - - @dispatch - def __init__(self, **kwargs): - """Handle keyword arguments for LCM compatibility.""" - # Get values with defaults and let dispatch handle type conversion - translation = kwargs.get("translation", Vector3()) - rotation = kwargs.get("rotation", Quaternion()) - - # Call the appropriate positional init - dispatch will handle the types - self.__init__(translation, rotation) - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._lcm_decode_one(data) - - @classmethod - def _lcm_decode_one(cls, buf): - translation = Vector3._lcm_decode_one(buf) - rotation = Quaternion._lcm_decode_one(buf) - return cls(translation=translation, rotation=rotation) - - def lcm_encode(self) -> bytes: - return super().encode() + ts: float + frame_id: str + child_frame_id: str + msg_name = "tf2_msgs.TFMessage" + + def __init__( + self, + translation: Vector3 | None = None, + rotation: Quaternion | None = None, + frame_id: str = "world", + child_frame_id: str = "base_link", + ts: float = 0.0, + **kwargs, + ) -> None: + self.frame_id = frame_id + self.child_frame_id = child_frame_id + self.ts = ts if ts != 0.0 else time.time() + self.translation = translation if translation is not None else Vector3() + self.rotation = rotation if rotation is not None else Quaternion() def __repr__(self) -> str: return f"Transform(translation={self.translation!r}, rotation={self.rotation!r})" @@ -109,3 +68,55 @@ def __eq__(self, other) -> bool: def identity(cls) -> Transform: """Create an identity transform.""" return cls() + + def lcm_transform(self) -> LCMTransformStamped: + return LCMTransformStamped( + child_frame_id=self.child_frame_id, + header=Header(self.ts, self.frame_id), + transform=LCMTransform( + translation=self.translation, + rotation=self.rotation, + ), + ) + + def lcm_encode(self) -> bytes: + # we get a circular import otherwise + from dimos.msgs.tf2_msgs.TFMessage import TFMessage + + return TFMessage(self).lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes | BinaryIO) -> Transform: + """Decode from LCM TFMessage bytes.""" + from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage + + lcm_msg = LCMTFMessage.lcm_decode(data) + + if not lcm_msg.transforms: + raise ValueError("No transforms found in LCM message") + + # Get the first transform from the message + lcm_transform_stamped = lcm_msg.transforms[0] + + # Extract timestamp from header + ts = lcm_transform_stamped.header.stamp.sec + ( + lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 + ) + + # Create and return Transform instance + return cls( + translation=Vector3( + lcm_transform_stamped.transform.translation.x, + lcm_transform_stamped.transform.translation.y, + lcm_transform_stamped.transform.translation.z, + ), + rotation=Quaternion( + lcm_transform_stamped.transform.rotation.x, + lcm_transform_stamped.transform.rotation.y, + lcm_transform_stamped.transform.rotation.z, + lcm_transform_stamped.transform.rotation.w, + ), + frame_id=lcm_transform_stamped.header.frame_id, + child_frame_id=lcm_transform_stamped.child_frame_id, + ts=ts, + ) diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py index 2d67d8c765..10e07eaeb5 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -69,23 +69,6 @@ def __init__(self, **kwargs): self.__init__(linear, angular) - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._lcm_decode_one(data) - - @classmethod - def _lcm_decode_one(cls, buf): - linear = Vector3._lcm_decode_one(buf) - angular = Vector3._lcm_decode_one(buf) - return cls(linear=linear, angular=angular) - - def lcm_encode(self) -> bytes: - return super().encode() - def __repr__(self) -> str: return f"Twist(linear={self.linear!r}, angular={self.angular!r})" diff --git a/dimos/msgs/geometry_msgs/Vector3.py b/dimos/msgs/geometry_msgs/Vector3.py index 7f839f2773..4b255d082c 100644 --- a/dimos/msgs/geometry_msgs/Vector3.py +++ b/dimos/msgs/geometry_msgs/Vector3.py @@ -47,21 +47,6 @@ class Vector3(LCMVector3): z: float = 0.0 msg_name = "geometry_msgs.Vector3" - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._lcm_decode_one(data) - - @classmethod - def _lcm_decode_one(cls, buf): - return cls(struct.unpack(">ddd", buf.read(24))) - - def lcm_encode(self) -> bytes: - return super().encode() - @dispatch def __init__(self) -> None: """Initialize a zero 3D vector.""" diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index ad103e8255..2524f12faa 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -556,6 +556,9 @@ def encodepass(): assert isinstance(pose_dest, Pose) assert pose_dest is not pose_source assert pose_dest == pose_source + # Verify we get our custom types back + assert isinstance(pose_dest.position, Vector3) + assert isinstance(pose_dest.orientation, Quaternion) import timeit diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index 64f8c6f8b1..3ffa8ce234 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import time + import numpy as np import pytest from dimos_lcm.geometry_msgs import Transform as LCMTransform +from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped from dimos.msgs.geometry_msgs import Pose, Quaternion, Transform, Vector3 @@ -33,41 +36,19 @@ def test_transform_initialization(): # Test initialization with Vector3 and Quaternion trans = Vector3(1.0, 2.0, 3.0) rot = Quaternion(0.0, 0.0, 0.707107, 0.707107) # 90 degrees around Z - tf2 = Transform(trans, rot) + tf2 = Transform(translation=trans, rotation=rot) assert tf2.translation == trans assert tf2.rotation == rot - # Test copy constructor - tf3 = Transform(tf2) - assert tf3.translation == tf2.translation - assert tf3.rotation == tf2.rotation - assert tf3 == tf2 - # Ensure it's a deep copy - tf3.translation.x = 10.0 - assert tf2.translation.x == 1.0 - - # Test initialization from LCM Transform - lcm_tf = LCMTransform() - lcm_tf.translation = Vector3(4.0, 5.0, 6.0) - lcm_tf.rotation = Quaternion(0.0, 0.707107, 0.0, 0.707107) # 90 degrees around Y - tf4 = Transform(lcm_tf) - assert tf4.translation.x == 4.0 - assert tf4.translation.y == 5.0 - assert tf4.translation.z == 6.0 - assert np.isclose(tf4.rotation.x, 0.0) - assert np.isclose(tf4.rotation.y, 0.707107) - assert np.isclose(tf4.rotation.z, 0.0) - assert np.isclose(tf4.rotation.w, 0.707107) - # Test initialization with only translation - tf5 = Transform(Vector3(7.0, 8.0, 9.0)) + tf5 = Transform(translation=Vector3(7.0, 8.0, 9.0)) assert tf5.translation.x == 7.0 assert tf5.translation.y == 8.0 assert tf5.translation.z == 9.0 assert tf5.rotation.w == 1.0 # Identity rotation # Test initialization with only rotation - tf6 = Transform(Quaternion(0.0, 0.0, 0.0, 1.0)) + tf6 = Transform(rotation=Quaternion(0.0, 0.0, 0.0, 1.0)) assert tf6.translation.is_zero() # Zero translation assert tf6.rotation.w == 1.0 @@ -101,10 +82,12 @@ def test_transform_identity(): def test_transform_equality(): - tf1 = Transform(Vector3(1, 2, 3), Quaternion(0, 0, 0, 1)) - tf2 = Transform(Vector3(1, 2, 3), Quaternion(0, 0, 0, 1)) - tf3 = Transform(Vector3(1, 2, 4), Quaternion(0, 0, 0, 1)) # Different z - tf4 = Transform(Vector3(1, 2, 3), Quaternion(0, 0, 1, 0)) # Different rotation + tf1 = Transform(translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1)) + tf2 = Transform(translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1)) + tf3 = Transform(translation=Vector3(1, 2, 4), rotation=Quaternion(0, 0, 0, 1)) # Different z + tf4 = Transform( + translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 1, 0) + ) # Different rotation assert tf1 == tf2 assert tf1 != tf3 @@ -113,7 +96,9 @@ def test_transform_equality(): def test_transform_string_representations(): - tf = Transform(Vector3(1.5, -2.0, 3.14), Quaternion(0, 0, 0.707107, 0.707107)) + tf = Transform( + translation=Vector3(1.5, -2.0, 3.14), rotation=Quaternion(0, 0, 0.707107, 0.707107) + ) # Test repr repr_str = repr(tf) @@ -180,7 +165,7 @@ def test_pose_add_transform_with_rotation(): translation=Vector3(1.0, 0.0, 0.0), rotation=Quaternion( 0.0, 0.0, np.sin(rotation_angle / 2), np.cos(rotation_angle / 2) - ), # 45° around Z + ), # 45� around Z ) transform2 = Transform( @@ -192,13 +177,13 @@ def test_pose_add_transform_with_rotation(): transformed_pose2 = initial_pose @ transform1 @ transform2 # Test transformed_pose1: initial_pose + transform1 - # Since the pose is rotated 90° (facing +Y), moving forward (local X) + # Since the pose is rotated 90� (facing +Y), moving forward (local X) # means moving in the +Y direction in world frame assert np.isclose(transformed_pose1.position.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose1.position.y, 1.0, atol=1e-10) assert np.isclose(transformed_pose1.position.z, 0.0, atol=1e-10) - # Orientation should be 90° + 45° = 135° around Z + # Orientation should be 90� + 45� = 135� around Z total_angle1 = angle + rotation_angle # 135 degrees assert np.isclose(transformed_pose1.orientation.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose1.orientation.y, 0.0, atol=1e-10) @@ -206,24 +191,24 @@ def test_pose_add_transform_with_rotation(): assert np.isclose(transformed_pose1.orientation.w, np.cos(total_angle1 / 2), atol=1e-10) # Test transformed_pose2: initial_pose + transform1 + transform2 - # Starting from (0, 0, 0) facing 90°: + # Starting from (0, 0, 0) facing 90�: # - # - Apply transform1: move 1 forward (along +Y) → (0, 1, 0), now facing 135° + # - Apply transform1: move 1 forward (along +Y) � (0, 1, 0), now facing 135� # # - Apply transform2: move 1 in local Y and 1 up - # At 135°, local Y points at 225° (135° + 90°) + # At 135�, local Y points at 225� (135� + 90�) # - # x += cos(225°) = -√2/2, y += sin(225°) = -√2/2 + # x += cos(225�) = -2/2, y += sin(225�) = -2/2 sqrt2_2 = np.sqrt(2) / 2 - expected_x = 0.0 - sqrt2_2 # 0 - √2/2 ≈ -0.707 - expected_y = 1.0 - sqrt2_2 # 1 - √2/2 ≈ 0.293 + expected_x = 0.0 - sqrt2_2 # 0 - 2/2 H -0.707 + expected_y = 1.0 - sqrt2_2 # 1 - 2/2 H 0.293 expected_z = 1.0 # 0 + 1 assert np.isclose(transformed_pose2.position.x, expected_x, atol=1e-10) assert np.isclose(transformed_pose2.position.y, expected_y, atol=1e-10) assert np.isclose(transformed_pose2.position.z, expected_z, atol=1e-10) - # Orientation should be 135° (only transform1 has rotation) + # Orientation should be 135� (only transform1 has rotation) total_angle2 = total_angle1 # 135 degrees (transform2 has no rotation) assert np.isclose(transformed_pose2.orientation.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose2.orientation.y, 0.0, atol=1e-10) @@ -231,40 +216,15 @@ def test_pose_add_transform_with_rotation(): assert np.isclose(transformed_pose2.orientation.w, np.cos(total_angle2 / 2), atol=1e-10) -def test_pose_add_lcm_transform(): - """Test that Pose works with both LCMTransform and Transform types.""" - initial_pose = Pose(1.0, 0.0, 0.0) - - # Create an LCMTransform - lcm_transform = LCMTransform() - lcm_transform.translation = Vector3(2.0, 1.0, 0.0) - angle = np.pi / 2 # 90 degrees - lcm_transform.rotation = Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)) - - # Create a regular Transform with same values +def test_lcm_encode_decode(): + angle = np.pi / 2 transform = Transform( translation=Vector3(2.0, 1.0, 0.0), rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), ) - # Test both + and @ operators with LCMTransform - pose_lcm_add = initial_pose + lcm_transform - pose_lcm_matmul = initial_pose @ lcm_transform - - # Test both + and @ operators with Transform - pose_tf_add = initial_pose + transform - pose_tf_matmul = initial_pose @ transform - - # All results should be the same - assert np.isclose(pose_lcm_add.position.x, 3.0, atol=1e-10) - assert np.isclose(pose_lcm_add.position.y, 1.0, atol=1e-10) - assert np.isclose(pose_lcm_add.position.z, 0.0, atol=1e-10) + data = transform.lcm_encode() - # Check that all methods produce the same result - assert pose_lcm_add.position == pose_lcm_matmul.position - assert pose_lcm_add.position == pose_tf_add.position - assert pose_lcm_add.position == pose_tf_matmul.position + decoded_transform = Transform.lcm_decode(data) - assert pose_lcm_add.orientation == pose_lcm_matmul.orientation - assert pose_lcm_add.orientation == pose_tf_add.orientation - assert pose_lcm_add.orientation == pose_tf_matmul.orientation + assert decoded_transform == transform diff --git a/dimos/msgs/geometry_msgs/test_Twist.py b/dimos/msgs/geometry_msgs/test_Twist.py index c5df6d5597..2e57523826 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -183,6 +183,8 @@ def test_twist_lcm_encoding(): decoded = Twist.lcm_decode(encoded) assert decoded.linear == tw.linear assert decoded.angular == tw.angular + + assert isinstance(decoded.linear, Vector3) assert decoded == tw diff --git a/dimos/msgs/sensor_msgs/Image.py b/dimos/msgs/sensor_msgs/Image.py index 2ac53a2fd7..7c1400536d 100644 --- a/dimos/msgs/sensor_msgs/Image.py +++ b/dimos/msgs/sensor_msgs/Image.py @@ -293,13 +293,13 @@ def lcm_encode(self, frame_id: Optional[str] = None) -> LCMImage: msg.data_length = len(image_bytes) msg.data = image_bytes - return msg.encode() + return msg.lcm_encode() @classmethod def lcm_decode(cls, data: bytes, **kwargs) -> "Image": """Create Image from LCM Image message.""" # Parse encoding to determine format and data type - msg = LCMImage.decode(data) + msg = LCMImage.lcm_decode(data) format_info = cls._parse_encoding(msg.encoding) # Convert bytes back to numpy array diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 4c4455a473..2238b31025 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -75,7 +75,7 @@ def lcm_encode(self, frame_id: Optional[str] = None) -> bytes: msg.is_bigendian = False msg.fields_length = 4 # x, y, z, intensity msg.fields = self._create_xyz_field() - return msg.encode() + return msg.lcm_encode() # Point cloud dimensions msg.height = 1 # Unorganized point cloud @@ -105,11 +105,11 @@ def lcm_encode(self, frame_id: Optional[str] = None) -> bytes: msg.is_dense = True # No invalid points msg.is_bigendian = False # Little endian - return msg.encode() + return msg.lcm_encode() @classmethod def lcm_decode(cls, data: bytes) -> "PointCloud2": - msg = LCMPointCloud2.decode(data) + msg = LCMPointCloud2.lcm_decode(data) if msg.width == 0 or msg.height == 0: # Empty point cloud diff --git a/dimos/msgs/std_msgs/Header.py b/dimos/msgs/std_msgs/Header.py new file mode 100644 index 0000000000..642b3d36c0 --- /dev/null +++ b/dimos/msgs/std_msgs/Header.py @@ -0,0 +1,87 @@ +# 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. + +from __future__ import annotations + +import time +from datetime import datetime +from typing import Optional, Union + +from dimos_lcm.std_msgs import Header as LCMHeader +from dimos_lcm.std_msgs import Time as LCMTime +from plum import dispatch + + +class Header(LCMHeader): + msg_name = "std_msgs.Header" + + @dispatch + def __init__(self) -> None: + """Initialize a Header with current time and empty frame_id.""" + super().__init__(seq=0, stamp=LCMTime(), frame_id="") + + @dispatch + def __init__(self, frame_id: str) -> None: + """Initialize a Header with current time and specified frame_id.""" + ts = time.time() + sec = int(ts) + nsec = int((ts - sec) * 1_000_000_000) + super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) + + @dispatch + def __init__(self, timestamp: float, frame_id: str = "", seq: int = 1) -> None: + """Initialize a Header with Unix timestamp, frame_id, and optional seq.""" + sec = int(timestamp) + nsec = int((timestamp - sec) * 1_000_000_000) + super().__init__(seq=seq, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) + + @dispatch + def __init__(self, timestamp: datetime, frame_id: str = "") -> None: + """Initialize a Header with datetime object and frame_id.""" + ts = timestamp.timestamp() + sec = int(ts) + nsec = int((ts - sec) * 1_000_000_000) + super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) + + @dispatch + def __init__(self, seq: int, stamp: LCMTime, frame_id: str) -> None: + """Initialize with explicit seq, stamp, and frame_id (LCM compatibility).""" + super().__init__(seq=seq, stamp=stamp, frame_id=frame_id) + + @dispatch + def __init__(self, header: LCMHeader) -> None: + """Initialize from another Header (copy constructor).""" + super().__init__(seq=header.seq, stamp=header.stamp, frame_id=header.frame_id) + + @classmethod + def now(cls, frame_id: str = "", seq: int = 1) -> Header: + """Create a Header with current timestamp.""" + ts = time.time() + return cls(ts, frame_id, seq) + + @property + def timestamp(self) -> float: + """Get timestamp as Unix time (float).""" + return self.stamp.sec + (self.stamp.nsec / 1_000_000_000) + + @property + def datetime(self) -> datetime: + """Get timestamp as datetime object.""" + return datetime.fromtimestamp(self.timestamp) + + def __str__(self) -> str: + return f"Header(seq={self.seq}, time={self.timestamp:.6f}, frame_id='{self.frame_id}')" + + def __repr__(self) -> str: + return f"Header(seq={self.seq}, stamp=Time(sec={self.stamp.sec}, nsec={self.stamp.nsec}), frame_id='{self.frame_id}')" diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py new file mode 100644 index 0000000000..a58b74a2f2 --- /dev/null +++ b/dimos/msgs/std_msgs/__init__.py @@ -0,0 +1,17 @@ +# 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. + +from .Header import Header + +__all__ = ["Header"] diff --git a/dimos/msgs/std_msgs/test_header.py b/dimos/msgs/std_msgs/test_header.py new file mode 100644 index 0000000000..85ffa0b8c6 --- /dev/null +++ b/dimos/msgs/std_msgs/test_header.py @@ -0,0 +1,100 @@ +# 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 time +from datetime import datetime + +import pytest + +from dimos.msgs.std_msgs import Header + + +def test_header_initialization_methods(): + """Test various ways to initialize a Header.""" + + # Method 1: With timestamp and frame_id + header1 = Header(123.456, "world") + assert header1.seq == 1 + assert header1.stamp.sec == 123 + assert header1.stamp.nsec == 456000000 + assert header1.frame_id == "world" + + # Method 2: With just frame_id (uses current time) + header2 = Header("base_link") + assert header2.seq == 1 + assert header2.frame_id == "base_link" + # Timestamp should be close to current time + assert abs(header2.timestamp - time.time()) < 0.1 + + # Method 3: Empty header (current time, empty frame_id) + header3 = Header() + assert header3.seq == 0 + assert header3.frame_id == "" + + # Method 4: With datetime object + dt = datetime(2025, 1, 18, 12, 30, 45, 500000) # 500ms + header4 = Header(dt, "sensor") + assert header4.seq == 1 + assert header4.frame_id == "sensor" + expected_timestamp = dt.timestamp() + assert abs(header4.timestamp - expected_timestamp) < 1e-6 + + # Method 5: With custom seq number + header5 = Header(999.123, "custom", seq=42) + assert header5.seq == 42 + assert header5.stamp.sec == 999 + assert header5.stamp.nsec == 123000000 + assert header5.frame_id == "custom" + + # Method 6: Using now() class method + header6 = Header.now("camera") + assert header6.seq == 1 + assert header6.frame_id == "camera" + assert abs(header6.timestamp - time.time()) < 0.1 + + # Method 7: now() with custom seq + header7 = Header.now("lidar", seq=99) + assert header7.seq == 99 + assert header7.frame_id == "lidar" + + +def test_header_properties(): + """Test Header property accessors.""" + header = Header(1234567890.123456789, "test") + + # Test timestamp property + assert abs(header.timestamp - 1234567890.123456789) < 1e-6 + + # Test datetime property + dt = header.datetime + assert isinstance(dt, datetime) + assert abs(dt.timestamp() - 1234567890.123456789) < 1e-6 + + +def test_header_string_representation(): + """Test Header string representations.""" + header = Header(100.5, "map", seq=10) + + # Test __str__ + str_repr = str(header) + assert "seq=10" in str_repr + assert "time=100.5" in str_repr + assert "frame_id='map'" in str_repr + + # Test __repr__ + repr_str = repr(header) + assert "Header(" in repr_str + assert "seq=10" in repr_str + assert "Time(sec=100, nsec=500000000)" in repr_str + assert "frame_id='map'" in repr_str diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py new file mode 100644 index 0000000000..ddd9ee2b29 --- /dev/null +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -0,0 +1,127 @@ +# 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. + +# 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. + +from __future__ import annotations + +from typing import BinaryIO + +from dimos_lcm.geometry_msgs import Transform as LCMTransform +from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped +from dimos_lcm.std_msgs import Header as LCMHeader +from dimos_lcm.std_msgs import Time as LCMTime +from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage + +from dimos.msgs.geometry_msgs.Transform import Transform + + +class TFMessage: + """TFMessage that accepts Transform objects and encodes to LCM format.""" + + transforms: list[Transform] + msg_name = "tf2_msgs.TFMessage" + + def __init__(self, *transforms: Transform) -> None: + self.transforms = list(transforms) + + def add_transform(self, transform: Transform, child_frame_id: str = "base_link") -> None: + """Add a transform to the message.""" + self.transforms.append(transform) + self.transforms_length = len(self.transforms) + + def lcm_encode(self) -> bytes: + """Encode as LCM TFMessage. + + Args: + child_frame_ids: Optional list of child frame IDs for each transform. + If not provided, defaults to "base_link" for all. + """ + + print("WILL MAP", self.transforms) + res = list(map(lambda t: t.lcm_transform(), self.transforms)) + print("RES IS", res) + + print("HEADER", res[0].header) + lcm_msg = LCMTFMessage( + transforms_length=len(self.transforms), + transforms=res, + ) + + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes | BinaryIO) -> TFMessage: + """Decode from LCM TFMessage bytes.""" + lcm_msg = LCMTFMessage.lcm_decode(data) + + # Convert LCM TransformStamped objects to Transform objects + transforms = [] + for lcm_transform_stamped in lcm_msg.transforms: + # Extract timestamp + ts = lcm_transform_stamped.header.stamp.sec + ( + lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 + ) + + print( + lcm_transform_stamped.transform.translation, + lcm_transform_stamped.transform.rotation, + lcm_transform_stamped.header.frame_id, + ts, + ) + + print(Transform) + + # Create Transform + transform = Transform( + translation=lcm_transform_stamped.transform.translation, + rotation=lcm_transform_stamped.transform.rotation, + frame_id=lcm_transform_stamped.header.frame_id, + ts=ts, + ) + transforms.append(transform) + + return cls(*transforms) + + def __len__(self) -> int: + """Return number of transforms.""" + return len(self.transforms) + + def __getitem__(self, index: int) -> Transform: + """Get transform by index.""" + return self.transforms[index] + + def __iter__(self): + """Iterate over transforms.""" + return iter(self.transforms) + + def __repr__(self) -> str: + return f"TFMessage({len(self.transforms)} transforms)" + + def __str__(self) -> str: + lines = [f"TFMessage with {len(self.transforms)} transforms:"] + for i, transform in enumerate(self.transforms): + lines.append(f" [{i}] {transform.frame_id} @ {transform.ts:.3f}") + return "\n".join(lines) diff --git a/dimos/msgs/tf2_msgs/__init__.py b/dimos/msgs/tf2_msgs/__init__.py new file mode 100644 index 0000000000..683e4ec61b --- /dev/null +++ b/dimos/msgs/tf2_msgs/__init__.py @@ -0,0 +1,17 @@ +# 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. + +from dimos.msgs.tf2_msgs.TFMessage import TFMessage + +__all__ = ["TFMessage"] diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py new file mode 100644 index 0000000000..6eec4cbdcc --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -0,0 +1,109 @@ +# 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 pytest +from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage + +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.tf2_msgs import TFMessage + + +def test_tfmessage_initialization(): + """Test TFMessage initialization with Transform objects.""" + # Create some transforms + tf1 = Transform( + translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1), frame_id="world", ts=100.0 + ) + tf2 = Transform( + translation=Vector3(4, 5, 6), + rotation=Quaternion(0, 0, 0.707, 0.707), + frame_id="map", + ts=101.0, + ) + + # Create TFMessage with transforms + msg = TFMessage(tf1, tf2) + + assert len(msg) == 2 + assert msg[0] == tf1 + assert msg[1] == tf2 + + # Test iteration + transforms = list(msg) + assert transforms == [tf1, tf2] + + +def test_tfmessage_empty(): + """Test empty TFMessage.""" + msg = TFMessage() + assert len(msg) == 0 + assert list(msg) == [] + + +def test_tfmessage_add_transform(): + """Test adding transforms to TFMessage.""" + msg = TFMessage() + + tf = Transform(translation=Vector3(1, 2, 3), frame_id="base", ts=200.0) + + msg.add_transform(tf) + assert len(msg) == 1 + assert msg[0] == tf + + +def test_tfmessage_lcm_encode_decode(): + """Test encoding TFMessage to LCM bytes.""" + # Create transforms + tf1 = Transform( + translation=Vector3(1.0, 2.0, 3.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + child_frame_id="robot", + frame_id="world", + ts=123.456, + ) + tf2 = Transform( + translation=Vector3(4.0, 5.0, 6.0), + rotation=Quaternion(0.0, 0.0, 0.707, 0.707), + frame_id="robot", + child_frame_id="target", + ts=124.567, + ) + + # Create TFMessage + msg = TFMessage(tf1, tf2) + + # Encode with custom child_frame_ids + encoded = msg.lcm_encode() + + # Decode using LCM to verify + lcm_msg = LCMTFMessage.lcm_decode(encoded) + + assert lcm_msg.transforms_length == 2 + + # Check first transform + ts1 = lcm_msg.transforms[0] + assert ts1.header.frame_id == "world" + assert ts1.child_frame_id == "robot" + assert ts1.header.stamp.sec == 123 + assert ts1.header.stamp.nsec == 456000000 + assert ts1.transform.translation.x == 1.0 + assert ts1.transform.translation.y == 2.0 + assert ts1.transform.translation.z == 3.0 + + # Check second transform + ts2 = lcm_msg.transforms[1] + assert ts2.header.frame_id == "robot" + assert ts2.child_frame_id == "target" + assert ts2.transform.rotation.z == 0.707 + assert ts2.transform.rotation.w == 0.707 diff --git a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py new file mode 100644 index 0000000000..8eefdb3aee --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -0,0 +1,71 @@ +# 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 time +from dataclasses import dataclass + +import numpy as np +import pytest + +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.tf2_msgs import TFMessage +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic + + +# Publishes a series of transforms representing a robot kinematic chain +# to actual LCM messages, foxglove running in parallel should render this +@pytest.mark.tofix +def test_publish_transforms(): + import tf_lcm_py + from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage + + lcm = LCM(autoconf=True) + lcm.start() + + topic = Topic(topic="/tf", lcm_type=LCMTFMessage) + + # Create a robot kinematic chain using our new types + current_time = time.time() + + # 1. World to base_link transform (robot at position) + world_to_base = Transform( + translation=Vector3(4.0, 3.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z + frame_id="world", + child_frame_id="base_link", + ts=current_time, + ) + + # 2. Base to arm transform (arm lifted up) + base_to_arm = Transform( + translation=Vector3(0.2, 0.0, 1.5), + rotation=Quaternion(0.0, 0.258819, 0.0, 0.965926), # 30 degrees around Y + frame_id="base_link", + child_frame_id="arm_link", + ts=current_time, + ) + + lcm.publish(topic, TFMessage(world_to_base, base_to_arm)) + + time.sleep(0.05) + # 3. Arm to gripper transform (gripper extended) + arm_to_gripper = Transform( + translation=Vector3(0.5, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation + frame_id="arm_link", + child_frame_id="gripper_link", + ts=current_time, + ) + + lcm.publish(topic, TFMessage(world_to_base, arm_to_gripper)) diff --git a/dimos/protocol/service/lcmservice.py b/dimos/protocol/service/lcmservice.py index 5f8c747864..a8fac1f495 100644 --- a/dimos/protocol/service/lcmservice.py +++ b/dimos/protocol/service/lcmservice.py @@ -14,17 +14,16 @@ from __future__ import annotations +import os import subprocess import sys import threading import traceback -import os -from functools import cache from dataclasses import dataclass +from functools import cache from typing import Any, Callable, Optional, Protocol, runtime_checkable import lcm - from dimos.protocol.service.spec import Service @@ -137,6 +136,7 @@ class LCMConfig: ttl: int = 0 url: str | None = None autoconf: bool = False + lcm: Optional[lcm.LCM] = None @runtime_checkable @@ -172,7 +172,13 @@ class LCMService(Service[LCMConfig]): def __init__(self, **kwargs) -> None: super().__init__(**kwargs) - self.l = lcm.LCM(self.config.url) if self.config.url else lcm.LCM() + + # we support passing an existing LCM instance + if self.config.lcm: + self.l = self.config.lcm + else: + self.l = lcm.LCM(self.config.url) if self.config.url else lcm.LCM() + self._stop_event = threading.Event() self._thread = None diff --git a/dimos/robot/foxglove_bridge.py b/dimos/robot/foxglove_bridge.py index a0374fc251..fdd65281c8 100644 --- a/dimos/robot/foxglove_bridge.py +++ b/dimos/robot/foxglove_bridge.py @@ -16,7 +16,7 @@ import threading # this is missing, I'm just trying to import lcm_foxglove_bridge.py from dimos_lcm -import dimos_lcm.lcm_foxglove_bridge as bridge +from dimos_lcm.foxglove_bridge import FoxgloveBridge as LCMFoxgloveBridge from dimos.core import Module, rpc @@ -35,7 +35,8 @@ def run_bridge(): self._loop = asyncio.new_event_loop() asyncio.set_event_loop(self._loop) try: - self._loop.run_until_complete(bridge.main()) + bridge = LCMFoxgloveBridge(host="0.0.0.0", port=8765, debug=False, num_threads=4) + self._loop.run_until_complete(bridge.run()) except Exception as e: print(f"Foxglove bridge error: {e}") diff --git a/dimos/robot/global_planner/planner.py b/dimos/robot/global_planner/planner.py index e1484e2bc9..3b08c783e0 100644 --- a/dimos/robot/global_planner/planner.py +++ b/dimos/robot/global_planner/planner.py @@ -59,7 +59,7 @@ class AstarPlanner(Planner): get_costmap: Callable[[], Costmap] get_robot_pos: Callable[[], Vector3] - set_local_nav: Callable[[Path, Optional[threading.Event], Optional[float]], bool] + set_local_nav: Callable[[Path, Optional[threading.Event], Optional[float]], bool] = None conservativism: int = 8 @@ -95,7 +95,7 @@ def plan(self, goallike: PoseLike) -> Path: path = path.resample(0.1) self.vis("a*", path) self.path.publish(path) - if hasattr(self, "set_local_nav"): + if hasattr(self, "set_local_nav") and self.set_local_nav: self.set_local_nav(path) return path logger.warning("No path found to the goal.") diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index e9c4dfe545..4b771f3124 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -22,18 +22,19 @@ from reactivex import operators as ops from dimos.core import In, Module, Out, rpc -from dimos.msgs.foxglove_msgs import Arrow # from dimos.robot.local_planner.local_planner import LocalPlanner from dimos.msgs.geometry_msgs import ( Pose, PoseLike, PoseStamped, + Transform, Twist, Vector3, VectorLike, to_pose, ) +from dimos.msgs.tf2_msgs import TFMessage from dimos.types.costmap import Costmap from dimos.types.path import Path from dimos.utils.logging_config import setup_logger @@ -59,7 +60,7 @@ class SimplePlanner(Module): odom: In[PoseStamped] = None movecmd: Out[Twist] = None - arrow: Out[Arrow] = None + tf: Out[Transform] = None get_costmap: Callable[[], Costmap] @@ -75,12 +76,25 @@ def __init__( Module.__init__(self) self.get_costmap = get_costmap + def _odom_to_tf(self, odom: PoseStamped) -> Transform: + """Convert Odometry to Transform.""" + return Transform( + translation=odom.position, + rotation=odom.orientation, + frame_id="world", + child_frame_id="base_link", + ts=odom.ts, + ) + def transform_to_robot_frame( self, global_target: Vector3, global_robot_position: PoseStamped ) -> Vector3: - transform = global_robot_position.find_transform(global_target) - self.arrow.publish(Arrow.from_transform(transform, global_robot_position)) - return transform.translation + tf1 = self._odom_to_tf(global_robot_position) + tf2 = global_robot_position.find_transform(global_target) + tf2.child_frame_id = "target" + tf2.frame_id = "base_link" + self.tf.publish(TFMessage(tf1, tf2)) + return tf2.translation def get_move_stream(self, frequency: float = 20.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( @@ -95,7 +109,6 @@ def get_move_stream(self, frequency: float = 20.0) -> rx.Observable: ops.map(self._calculate_rotation_to_target), # filter out None results from calc_move ops.filter(lambda result: result is not None), - ops.map(vector_to_twist), ) def _safe_transform_goal(self) -> Optional[Vector3]: diff --git a/dimos/robot/module/test_tf.py b/dimos/robot/module/test_tf.py new file mode 100644 index 0000000000..7b71a33a28 --- /dev/null +++ b/dimos/robot/module/test_tf.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +# 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 time + +import pytest + +import lcm +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 + + +@pytest.mark.tool +def test_tf_broadcast_and_query(): + """Test TF broadcasting and querying between two TF instances. + If you run foxglove-bridge this will show up in the UI""" + from dimos.robot.module.tf import TF, TFConfig + + broadcaster = TF() + querier = TF() + + # Create a transform from world to robot + current_time = time.time() + + world_to_robot = Transform( + translation=Vector3(1.0, 2.0, 3.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation + frame_id="world", + child_frame_id="robot", + ts=current_time, + ) + + # Broadcast the transform + broadcaster.send(world_to_robot) + + # Give time for the message to propagate + time.sleep(0.05) + + # Query should now be able to find the transform + assert querier.can_transform("world", "robot", current_time) + + # Verify frames are available + frames = querier.get_frames() + assert "world" in frames + assert "robot" in frames + + # Add another transform in the chain + robot_to_sensor = Transform( + translation=Vector3(0.5, 0.0, 0.2), + rotation=Quaternion(0.0, 0.0, 0.707107, 0.707107), # 90 degrees around Z + frame_id="robot", + child_frame_id="sensor", + ts=current_time, + ) + + random_object_in_view = Pose( + position=Vector3(1.0, 0.0, 0.0), + ) + + broadcaster.send(robot_to_sensor) + time.sleep(0.05) + + # Should be able to query the full chain + assert querier.can_transform("world", "sensor", current_time) + + t = querier.lookup("world", "sensor") + print("FOUND T", t) + + # random_object_in_view.find_transform() + + # Stop services + broadcaster.stop() + querier.stop() diff --git a/dimos/robot/module/tf.py b/dimos/robot/module/tf.py new file mode 100644 index 0000000000..71aee4c8c2 --- /dev/null +++ b/dimos/robot/module/tf.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 + +# 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 time +from abc import abstractmethod +from dataclasses import dataclass +from datetime import datetime +from typing import Optional, TypeVar + +import dimos_lcm +import numpy as np +import pytest + +import lcm +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.tf2_msgs import TFMessage +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic +from dimos.protocol.pubsub.spec import PubSub +from dimos.protocol.service.lcmservice import LCMConfig, LCMService, Service + +CONFIG = TypeVar("CONFIG") + + +class TFSpec(Service[CONFIG]): + @abstractmethod + def send(self, *args: Transform) -> None: ... + + @abstractmethod + def send_static(self, *args: Transform) -> None: ... + + @abstractmethod + def lookup( + self, + parent_frame: str, + child_frame: str, + time_point: Optional[float] = None, + time_tolerance: Optional[float] = None, + ): ... + + +@dataclass +class TFConfig(LCMConfig): + topic: str = "/tf" + buffer_size: float = 10.0 # seconds + rate_limit: float = 10.0 # Hz + autostart: bool = True + + +@dataclass +class GenericTFConfig(TFConfig): + pubsub: PubSub = None + + +class GenericTF(TFSpec[TFConfig]): + def __init__(self, **kwargs): + super().__init__(**kwargs) + if self.config.autostart: + self.start() + + def start(): + self.pubsub.subscribe(self.topic, self.receive_transform) + + def receive_transform(self, msg: TFMessage, topic: Topic) -> None: ... + + def send(self, *args: Transform) -> None: ... + + def send_static(self, *args: Transform) -> None: ... + + def lookup( + self, + parent_frame: str, + child_frame: str, + time_point: Optional[float] = None, + time_tolerance: Optional[float] = None, + ): ... + + def stop(): ... + + +# this doesn't work due to tf_lcm_py package +class TFLCM(LCMService, TFSpec[TFConfig]): + """A service for managing and broadcasting transforms using LCM. + This is not a separete module, You can include this in your module + if you need to access transforms. + + Ideally we would have a generic pubsub for transforms so we are + transport agnostic (TODO) + + For now we are not doing this because we want to use cpp buffer/lcm + implementation. We also don't want to manually hook up tf stream + for each module. + """ + + default_config = TFConfig + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + + import tf_lcm_py as tf + + self.l = tf.LCM() + self.buffer = tf.Buffer(self.config.buffer_size) + self.listener = tf.TransformListener(self.l, self.buffer) + self.broadcaster = tf.TransformBroadcaster() + self.static_broadcaster = tf.StaticTransformBroadcaster() + + # will call the underlying LCMService.start + self.start() + + def send(self, *args: Transform) -> None: + for t in args: + self.broadcaster.send_transform(t.lcm_transform()) + + def send_static(self, *args: Transform) -> None: + for t in args: + self.static_broadcaster.send_static_transform(t) + + def lookup( + self, + parent_frame: str, + child_frame: str, + time_point: Optional[float] = None, + time_tolerance: Optional[float] = None, + ): + return self.buffer.lookup_transform( + parent_frame, + child_frame, + datetime.now(), + lcm_module=dimos_lcm, + ) + + def can_transform( + self, parent_frame: str, child_frame: str, time_point: Optional[float | datetime] = None + ) -> bool: + if not time_point: + time_point = datetime.now() + + if isinstance(time_point, float): + time_point = datetime.fromtimestamp(time_point) + + return self.buffer.can_transform(parent_frame, child_frame, time_point) + + def get_frames(self) -> list[str]: + return self.buffer.get_all_frame_names() + + def start(self): + super().start() + ... + + def stop(self): ... diff --git a/dimos/robot/unitree_webrtc/multiprocess/example_usage.py b/dimos/robot/unitree_webrtc/multiprocess/example_usage.py index f8a64b4b0b..0e3c10b3a5 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/example_usage.py +++ b/dimos/robot/unitree_webrtc/multiprocess/example_usage.py @@ -43,15 +43,15 @@ async def run_light_robot(): await robot.start() - pose = robot.get_pose() - print(f"Robot position: {pose['position']}") - print(f"Robot rotation: {pose['rotation']}") + # pose = robot.get_pose() + # print(f"Robot position: {pose['position']}") + # print(f"Robot rotation: {pose['rotation']}") - from dimos.msgs.geometry_msgs import Vector3 + # from dimos.msgs.geometry_msgs import Vector3 # robot.move(Vector3(0.5, 0, 0), duration=2.0) - robot.explore() + # robot.explore() while True: await asyncio.sleep(1) diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index aa66291e68..4825d5dbaf 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -27,7 +27,6 @@ import dimos.core.colors as colors from dimos import core from dimos.core import In, Module, Out, rpc -from dimos.msgs.foxglove_msgs import Arrow from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist, Vector3 from dimos.msgs.sensor_msgs import Image from dimos.protocol import pubsub diff --git a/dimos/robot/unitree_webrtc/type/lidar.py b/dimos/robot/unitree_webrtc/type/lidar.py index bd5ccf9c0b..8481647d18 100644 --- a/dimos/robot/unitree_webrtc/type/lidar.py +++ b/dimos/robot/unitree_webrtc/type/lidar.py @@ -59,7 +59,7 @@ def __init__(self, **kwargs): super().__init__( pointcloud=kwargs.get("pointcloud"), ts=kwargs.get("ts"), - frame_id="lidar", + frame_id="world", ) self.origin = kwargs.get("origin") diff --git a/dimos/robot/unitree_webrtc/type/odometry.py b/dimos/robot/unitree_webrtc/type/odometry.py index 76def232e4..a9c03309da 100644 --- a/dimos/robot/unitree_webrtc/type/odometry.py +++ b/dimos/robot/unitree_webrtc/type/odometry.py @@ -83,6 +83,9 @@ class RawOdometryMessage(TypedDict): class Odometry(PoseStamped, Timestamped): name = "geometry_msgs.PoseStamped" + def __init__(self, frame_id: str = "base_link", *args, **kwargs) -> None: + super().__init__(frame_id=frame_id, *args, **kwargs) + @classmethod def from_msg(cls, msg: RawOdometryMessage) -> "Odometry": pose = msg["data"]["pose"] diff --git a/dimos/types/test_timestamped.py b/dimos/types/test_timestamped.py index bf7962371e..4fa2ebca7e 100644 --- a/dimos/types/test_timestamped.py +++ b/dimos/types/test_timestamped.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from datetime import datetime +from datetime import datetime, timezone -from dimos.types.timestamped import Timestamped +from dimos.types.timestamped import Timestamped, to_ros_stamp, to_datetime def test_timestamped_dt_method(): @@ -24,3 +24,73 @@ def test_timestamped_dt_method(): assert isinstance(dt, datetime) assert abs(dt.timestamp() - ts) < 1e-6 assert dt.tzinfo is not None, "datetime should be timezone-aware" + + +def test_to_ros_stamp(): + """Test the to_ros_stamp function with different input types.""" + + # Test with float timestamp + ts_float = 1234567890.123456789 + result = to_ros_stamp(ts_float) + assert result["sec"] == 1234567890 + # Float precision limitation - check within reasonable range + assert abs(result["nanosec"] - 123456789) < 1000 + + # Test with integer timestamp + ts_int = 1234567890 + result = to_ros_stamp(ts_int) + assert result["sec"] == 1234567890 + assert result["nanosec"] == 0 + + # Test with datetime object + dt = datetime(2009, 2, 13, 23, 31, 30, 123456, tzinfo=timezone.utc) + result = to_ros_stamp(dt) + assert result["sec"] == 1234567890 + assert abs(result["nanosec"] - 123456000) < 1000 # Allow small rounding error + + # Test with RosStamp (passthrough) + ros_stamp = {"sec": 1234567890, "nanosec": 123456789} + result = to_ros_stamp(ros_stamp) + assert result is ros_stamp # Should be the exact same object + + +def test_to_datetime(): + """Test the to_datetime function with different input types.""" + + # Test with float timestamp + ts_float = 1234567890.123456 + dt = to_datetime(ts_float) + assert isinstance(dt, datetime) + assert dt.tzinfo is not None # Should have timezone + assert abs(dt.timestamp() - ts_float) < 1e-6 + + # Test with integer timestamp + ts_int = 1234567890 + dt = to_datetime(ts_int) + assert isinstance(dt, datetime) + assert dt.tzinfo is not None + assert dt.timestamp() == ts_int + + # Test with RosStamp + ros_stamp = {"sec": 1234567890, "nanosec": 123456000} + dt = to_datetime(ros_stamp) + assert isinstance(dt, datetime) + assert dt.tzinfo is not None + expected_ts = 1234567890.123456 + assert abs(dt.timestamp() - expected_ts) < 1e-6 + + # Test with datetime (already has timezone) + dt_input = datetime(2009, 2, 13, 23, 31, 30, tzinfo=timezone.utc) + dt_result = to_datetime(dt_input) + assert dt_result.tzinfo is not None + # Should convert to local timezone by default + + # Test with naive datetime (no timezone) + dt_naive = datetime(2009, 2, 13, 23, 31, 30) + dt_result = to_datetime(dt_naive) + assert dt_result.tzinfo is not None + + # Test with specific timezone + dt_utc = to_datetime(ts_float, tz=timezone.utc) + assert dt_utc.tzinfo == timezone.utc + assert abs(dt_utc.timestamp() - ts_float) < 1e-6 diff --git a/dimos/types/timestamped.py b/dimos/types/timestamped.py index 189bf7eaec..ee27aad759 100644 --- a/dimos/types/timestamped.py +++ b/dimos/types/timestamped.py @@ -25,11 +25,11 @@ class RosStamp(TypedDict): nanosec: int -EpochLike = Union[int, float, datetime, RosStamp] +TimeLike = Union[int, float, datetime, RosStamp] -def to_timestamp(ts: EpochLike) -> float: - """Convert EpochLike to a timestamp in seconds.""" +def to_timestamp(ts: TimeLike) -> float: + """Convert TimeLike to a timestamp in seconds.""" if isinstance(ts, datetime): return ts.timestamp() if isinstance(ts, (int, float)): @@ -39,6 +39,37 @@ def to_timestamp(ts: EpochLike) -> float: raise TypeError("unsupported timestamp type") +def to_ros_stamp(ts: TimeLike) -> RosStamp: + """Convert TimeLike to a ROS-style timestamp dictionary.""" + if isinstance(ts, dict) and "sec" in ts and "nanosec" in ts: + return ts + + timestamp = to_timestamp(ts) + sec = int(timestamp) + nanosec = int((timestamp - sec) * 1_000_000_000) + return {"sec": sec, "nanosec": nanosec} + + +def to_datetime(ts: TimeLike, tz=None) -> datetime: + if isinstance(ts, datetime): + if ts.tzinfo is None: + # Assume UTC for naive datetime + ts = ts.replace(tzinfo=timezone.utc) + if tz is not None: + return ts.astimezone(tz) + return ts.astimezone() # Convert to local tz + + # Convert to timestamp first + timestamp = to_timestamp(ts) + + # Create datetime from timestamp + if tz is not None: + return datetime.fromtimestamp(timestamp, tz=tz) + else: + # Use local timezone by default + return datetime.fromtimestamp(timestamp).astimezone() + + class Timestamped: ts: float diff --git a/dimos/utils/cli/__init__.py b/dimos/utils/cli/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/utils/run_foxglove_bridge.py b/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py similarity index 80% rename from dimos/utils/run_foxglove_bridge.py rename to dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py index dadb7c2529..bbcb70faee 100644 --- a/dimos/utils/run_foxglove_bridge.py +++ b/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py @@ -18,8 +18,14 @@ """ import asyncio +import os import threading -import dimos_lcm.lcm_foxglove_bridge as bridge + +import dimos_lcm +from dimos_lcm.foxglove_bridge import FoxgloveBridge + +dimos_lcm_path = os.path.dirname(os.path.abspath(dimos_lcm.__file__)) +print(f"Using dimos_lcm from: {dimos_lcm_path}") def run_bridge_example(): @@ -30,11 +36,9 @@ def bridge_thread(): loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) try: - bridge_runner = bridge.LcmFoxgloveBridgeRunner( - host="0.0.0.0", port=8765, debug=True, num_threads=4 - ) + bridge_instance = FoxgloveBridge(host="0.0.0.0", port=8765, debug=True, num_threads=4) - loop.run_until_complete(bridge_runner.run()) + loop.run_until_complete(bridge_instance.run()) except Exception as e: print(f"Bridge error: {e}") finally: diff --git a/dimos/utils/cli/lcmspy.py b/dimos/utils/cli/lcmspy/lcmspy.py similarity index 99% rename from dimos/utils/cli/lcmspy.py rename to dimos/utils/cli/lcmspy/lcmspy.py index aa5b48b297..134051302c 100755 --- a/dimos/utils/cli/lcmspy.py +++ b/dimos/utils/cli/lcmspy/lcmspy.py @@ -137,7 +137,7 @@ def __init__(self, **kwargs): def start(self): super().start() - self.l.subscribe("/.*", self.msg) + self.l.subscribe(".*", self.msg) def stop(self): """Stop the LCM spy and clean up resources""" diff --git a/dimos/utils/cli/lcmspy_cli.py b/dimos/utils/cli/lcmspy/run_lcmspy.py similarity index 96% rename from dimos/utils/cli/lcmspy_cli.py rename to dimos/utils/cli/lcmspy/run_lcmspy.py index 47bf2b8a56..17a9d0bbc6 100644 --- a/dimos/utils/cli/lcmspy_cli.py +++ b/dimos/utils/cli/lcmspy/run_lcmspy.py @@ -28,8 +28,8 @@ from textual.renderables.sparkline import Sparkline as SparklineRenderable from textual.widgets import DataTable, Footer, Header, Label, Sparkline -from dimos.utils.cli.lcmspy import GraphLCMSpy -from dimos.utils.cli.lcmspy import GraphTopic as SpyTopic +from dimos.utils.cli.lcmspy.lcmspy import GraphLCMSpy +from dimos.utils.cli.lcmspy.lcmspy import GraphTopic as SpyTopic def gradient(max_value: float, value: float) -> str: diff --git a/dimos/utils/cli/test_lcmspy.py b/dimos/utils/cli/lcmspy/test_lcmspy.py similarity index 97% rename from dimos/utils/cli/test_lcmspy.py rename to dimos/utils/cli/lcmspy/test_lcmspy.py index a77bb03d20..1d9624b13c 100644 --- a/dimos/utils/cli/test_lcmspy.py +++ b/dimos/utils/cli/lcmspy/test_lcmspy.py @@ -18,7 +18,8 @@ from dimos.protocol.pubsub.lcmpubsub import PickleLCM, Topic from dimos.protocol.service.lcmservice import autoconf -from dimos.utils.cli.lcmspy import GraphLCMSpy, GraphTopic, LCMSpy +from dimos.utils.cli.lcmspy.lcmspy import GraphLCMSpy, GraphTopic, LCMSpy +from dimos.utils.cli.lcmspy.lcmspy import Topic as TopicSpy autoconf() @@ -85,7 +86,6 @@ def test_spy_basic(): @pytest.mark.lcm def test_topic_statistics_direct(): """Test Topic statistics directly without LCM""" - from dimos.utils.cli.lcmspy import Topic as TopicSpy topic = TopicSpy("/test") @@ -112,7 +112,6 @@ def test_topic_statistics_direct(): def test_topic_cleanup(): """Test that old messages are properly cleaned up""" - from dimos.utils.cli.lcmspy import Topic as TopicSpy topic = TopicSpy("/test") diff --git a/dimos/utils/decorators/__init__.py b/dimos/utils/decorators/__init__.py new file mode 100644 index 0000000000..22ad478a00 --- /dev/null +++ b/dimos/utils/decorators/__init__.py @@ -0,0 +1,11 @@ +"""Decorators and accumulators for rate limiting and other utilities.""" + +from .accumulators import Accumulator, LatestAccumulator, RollingAverageAccumulator +from .decorators import limit + +__all__ = [ + "Accumulator", + "LatestAccumulator", + "RollingAverageAccumulator", + "limit", +] diff --git a/dimos/utils/decorators/accumulators.py b/dimos/utils/decorators/accumulators.py new file mode 100644 index 0000000000..4c57293b9f --- /dev/null +++ b/dimos/utils/decorators/accumulators.py @@ -0,0 +1,106 @@ +# 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 threading +from abc import ABC, abstractmethod +from typing import Any, Generic, Optional, TypeVar + +T = TypeVar("T") + + +class Accumulator(ABC, Generic[T]): + """Base class for accumulating messages between rate-limited calls.""" + + @abstractmethod + def add(self, *args, **kwargs) -> None: + """Add args and kwargs to the accumulator.""" + pass + + @abstractmethod + def get(self) -> Optional[tuple[tuple, dict]]: + """Get the accumulated args and kwargs and reset the accumulator.""" + pass + + @abstractmethod + def __len__(self) -> int: + """Return the number of accumulated items.""" + pass + + +class LatestAccumulator(Accumulator[T]): + """Simple accumulator that remembers only the latest args and kwargs.""" + + def __init__(self): + self._latest: Optional[tuple[tuple, dict]] = None + self._lock = threading.Lock() + + def add(self, *args, **kwargs) -> None: + with self._lock: + self._latest = (args, kwargs) + + def get(self) -> Optional[tuple[tuple, dict]]: + with self._lock: + result = self._latest + self._latest = None + return result + + def __len__(self) -> int: + with self._lock: + return 1 if self._latest is not None else 0 + + +class RollingAverageAccumulator(Accumulator[T]): + """Accumulator that maintains a rolling average of the first argument. + + This accumulator expects the first argument to be numeric and maintains + a rolling average without storing individual values. + """ + + def __init__(self): + self._sum: float = 0.0 + self._count: int = 0 + self._latest_kwargs: dict = {} + self._lock = threading.Lock() + + def add(self, *args, **kwargs) -> None: + if not args: + raise ValueError("RollingAverageAccumulator requires at least one argument") + + with self._lock: + try: + value = float(args[0]) + self._sum += value + self._count += 1 + self._latest_kwargs = kwargs + except (TypeError, ValueError): + raise TypeError(f"First argument must be numeric, got {type(args[0])}") + + def get(self) -> Optional[tuple[tuple, dict]]: + with self._lock: + if self._count == 0: + return None + + average = self._sum / self._count + result = ((average,), self._latest_kwargs) + + # Reset accumulator + self._sum = 0.0 + self._count = 0 + self._latest_kwargs = {} + + return result + + def __len__(self) -> int: + with self._lock: + return self._count diff --git a/dimos/utils/decorators/decorators.py b/dimos/utils/decorators/decorators.py new file mode 100644 index 0000000000..13ca5844a8 --- /dev/null +++ b/dimos/utils/decorators/decorators.py @@ -0,0 +1,102 @@ +# 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 threading +import time +from functools import wraps +from typing import Callable, Optional + +from .accumulators import Accumulator, LatestAccumulator + + +def limit(max_freq: float, accumulator: Optional[Accumulator] = None): + """ + Decorator that limits function call frequency. + + If calls come faster than max_freq, they are skipped. + If calls come slower than max_freq, they pass through immediately. + + Args: + max_freq: Maximum frequency in Hz (calls per second) + accumulator: Optional accumulator to collect skipped calls (defaults to LatestAccumulator) + + Returns: + Decorated function that respects the frequency limit + """ + if max_freq <= 0: + raise ValueError("Frequency must be positive") + + min_interval = 1.0 / max_freq + + # Create default accumulator if none provided + if accumulator is None: + accumulator = LatestAccumulator() + + def decorator(func: Callable) -> Callable: + last_call_time = 0.0 + lock = threading.Lock() + timer: Optional[threading.Timer] = None + + def execute_accumulated(): + nonlocal last_call_time, timer + with lock: + if len(accumulator): + acc_args, acc_kwargs = accumulator.get() + last_call_time = time.time() + timer = None + func(*acc_args, **acc_kwargs) + + @wraps(func) + def wrapper(*args, **kwargs): + nonlocal last_call_time, timer + current_time = time.time() + + with lock: + time_since_last = current_time - last_call_time + + if time_since_last >= min_interval: + # Cancel any pending timer + if timer is not None: + timer.cancel() + timer = None + + # Enough time has passed, execute the function + last_call_time = current_time + + # if we have accumulated data, we get a compound value + if len(accumulator): + accumulator.add(*args, **kwargs) + acc_args, acc_kwargs = accumulator.get() # accumulator resets here + return func(*acc_args, **acc_kwargs) + + # No accumulated data, normal call + return func(*args, **kwargs) + + else: + # Too soon, skip this call + accumulator.add(*args, **kwargs) + + # Schedule execution for when the interval expires + if timer is not None: + timer.cancel() + + time_to_wait = min_interval - time_since_last + timer = threading.Timer(time_to_wait, execute_accumulated) + timer.start() + + return None + + return wrapper + + return decorator diff --git a/dimos/utils/decorators/test_decorators.py b/dimos/utils/decorators/test_decorators.py new file mode 100644 index 0000000000..2a9162c762 --- /dev/null +++ b/dimos/utils/decorators/test_decorators.py @@ -0,0 +1,79 @@ +# 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 time + +import pytest + +from dimos.utils.decorators import LatestAccumulator, RollingAverageAccumulator, limit + + +def test_limit(): + """Test limit decorator with keyword arguments.""" + calls = [] + + @limit(20) # 20 Hz + def process(msg: str, keyword: int = 0): + calls.append((msg, keyword)) + return f"{msg}:{keyword}" + + # First call goes through + result1 = process("first", keyword=1) + assert result1 == "first:1" + assert calls == [("first", 1)] + + # Quick calls get accumulated + result2 = process("second", keyword=2) + assert result2 is None + + result3 = process("third", keyword=3) + assert result3 is None + + # Wait for interval, expect to be called after it passes + time.sleep(0.6) + + result4 = process("fourth") + assert result4 == "fourth:0" + + assert calls == [("first", 1), ("third", 3), ("fourth", 0)] + + +def test_latest_rolling_average(): + """Test RollingAverageAccumulator with limit decorator.""" + calls = [] + + accumulator = RollingAverageAccumulator() + + @limit(20, accumulator=accumulator) # 20 Hz + def process(value: float, label: str = ""): + calls.append((value, label)) + return f"{value}:{label}" + + # First call goes through + result1 = process(10.0, label="first") + assert result1 == "10.0:first" + assert calls == [(10.0, "first")] + + # Quick calls get accumulated + result2 = process(20.0, label="second") + assert result2 is None + + result3 = process(30.0, label="third") + assert result3 is None + + # Wait for interval + time.sleep(0.6) + + # Should see the average of accumulated values + assert calls == [(10.0, "first"), (25.0, "third")] # (20+30)/2 = 25 diff --git a/dimos/utils/test_foxglove_bridge.py b/dimos/utils/test_foxglove_bridge.py index ecedb90573..b845622d88 100644 --- a/dimos/utils/test_foxglove_bridge.py +++ b/dimos/utils/test_foxglove_bridge.py @@ -17,11 +17,12 @@ Test for foxglove bridge import and basic functionality """ -import pytest import threading import time import warnings -from unittest.mock import patch, MagicMock +from unittest.mock import MagicMock, patch + +import pytest warnings.filterwarnings("ignore", category=DeprecationWarning, module="websockets.server") warnings.filterwarnings("ignore", category=DeprecationWarning, module="websockets.legacy") @@ -30,33 +31,29 @@ def test_foxglove_bridge_import(): """Test that the foxglove bridge can be imported successfully.""" try: - import dimos_lcm.lcm_foxglove_bridge as bridge - - assert hasattr(bridge, "LcmFoxgloveBridgeRunner") + from dimos_lcm.foxglove_bridge import FoxgloveBridge except ImportError as e: pytest.fail(f"Failed to import foxglove bridge: {e}") def test_foxglove_bridge_runner_init(): - """Test that LcmFoxgloveBridgeRunner can be initialized with default parameters.""" + """Test that LcmFoxgloveBridge can be initialized with default parameters.""" try: - import dimos_lcm.lcm_foxglove_bridge as bridge + from dimos_lcm.foxglove_bridge import FoxgloveBridge - runner = bridge.LcmFoxgloveBridgeRunner( - host="localhost", port=8765, debug=False, num_threads=2 - ) + runner = FoxgloveBridge(host="localhost", port=8765, debug=False, num_threads=2) # Check that the runner was created successfully assert runner is not None except Exception as e: - pytest.fail(f"Failed to initialize LcmFoxgloveBridgeRunner: {e}") + pytest.fail(f"Failed to initialize LcmFoxgloveBridge: {e}") def test_foxglove_bridge_runner_params(): - """Test that LcmFoxgloveBridgeRunner accepts various parameter configurations.""" + """Test that LcmFoxgloveBridge accepts various parameter configurations.""" try: - import dimos_lcm.lcm_foxglove_bridge as bridge + from dimos_lcm.foxglove_bridge import FoxgloveBridge configs = [ {"host": "0.0.0.0", "port": 8765, "debug": True, "num_threads": 1}, @@ -65,7 +62,7 @@ def test_foxglove_bridge_runner_params(): ] for config in configs: - runner = bridge.LcmFoxgloveBridgeRunner(**config) + runner = FoxgloveBridge(**config) assert runner is not None except Exception as e: @@ -75,11 +72,9 @@ def test_foxglove_bridge_runner_params(): def test_bridge_runner_has_run_method(): """Test that the bridge runner has a run method that can be called.""" try: - import dimos_lcm.lcm_foxglove_bridge as bridge + from dimos_lcm.foxglove_bridge import FoxgloveBridge - runner = bridge.LcmFoxgloveBridgeRunner( - host="localhost", port=8765, debug=False, num_threads=1 - ) + runner = FoxgloveBridge(host="localhost", port=8765, debug=False, num_threads=1) # Check that the run method exists assert hasattr(runner, "run") diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index 514f0b01c6..ad81abdbbd 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -12,12 +12,10 @@ RUN apt-get update && apt-get install -y \ ccze \ tmux \ htop \ - python-is-python3 \ iputils-ping \ wget \ net-tools \ sudo \ - iproute2 # for LCM networking system config \ pre-commit diff --git a/docker/python/Dockerfile b/docker/python/Dockerfile index f8d06496b4..8689638b0d 100644 --- a/docker/python/Dockerfile +++ b/docker/python/Dockerfile @@ -4,6 +4,7 @@ FROM ${FROM_IMAGE} # Install basic requirements RUN apt-get update RUN apt-get install -y \ + python-is-python3 \ curl \ gnupg2 \ lsb-release \ @@ -29,7 +30,9 @@ RUN apt-get install -y \ qtchooser \ qt5-qmake \ qtbase5-dev-tools \ - supervisor + supervisor \ + iproute2 # for LCM networking system config \ + liblcm-dev # Fix distutils-installed packages that block pip upgrades RUN apt-get purge -y python3-blinker python3-sympy python3-oauthlib || true @@ -40,4 +43,4 @@ COPY . /app/ WORKDIR /app -RUN --mount=type=cache,target=/root/.cache/pip bash -c "pip install --upgrade 'pip>=24' 'setuptools>=70' 'wheel' 'packaging>=24' && pip install '.[cpu]'" \ No newline at end of file +RUN --mount=type=cache,target=/root/.cache/pip bash -c "pip install --upgrade 'pip>=24' 'setuptools>=70' 'wheel' 'packaging>=24' && pip install '.[cpu]'" diff --git a/flake.nix b/flake.nix index fe6749797e..fbbf45591d 100644 --- a/flake.nix +++ b/flake.nix @@ -39,6 +39,9 @@ ### Open3D & build-time eigen cmake ninja jsoncpp libjpeg libpng + + ### LCM (Lightweight Communications and Marshalling) + lcm ]; # ------------------------------------------------------------ @@ -54,7 +57,7 @@ pkgs.xorg.libXrender pkgs.xorg.libXdamage pkgs.xorg.libXcomposite pkgs.xorg.libxcb pkgs.xorg.libXScrnSaver pkgs.xorg.libXxf86vm pkgs.udev pkgs.portaudio pkgs.SDL2.dev pkgs.zlib pkgs.glib pkgs.gtk3 - pkgs.gdk-pixbuf pkgs.gobject-introspection]}:$LD_LIBRARY_PATH" + pkgs.gdk-pixbuf pkgs.gobject-introspection pkgs.lcm]}:$LD_LIBRARY_PATH" export DISPLAY=:0 diff --git a/pyproject.toml b/pyproject.toml index b06e9139b0..39350d489b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,8 +93,8 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@ae2371c78e38511c97dcd13d7933474d6bc56073" - + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@ba3445d16be75a7ade6fb2a516b39a3e44319d5c" + # "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] @@ -182,7 +182,6 @@ files = [ "dimos/msgs/**/*.py", "dimos/protocol/**/*.py" ] -addopts = "--disable-warnings" [tool.pytest.ini_options] testpaths = ["dimos"] @@ -194,11 +193,11 @@ markers = [ "tool: dev tooling", "needsdata: needs test data to be downloaded", "ros: depend on ros", - "lcm: tests that run actual LCM bus (can't execute in CI)" - + "lcm: tests that run actual LCM bus (can't execute in CI)", + "tofix: tests that require fixing and are temporarily disabled" ] -addopts = "-v -ra --color=yes -m 'not vis and not benchmark and not exclude and not tool and not needsdata and not lcm and not ros and not heavy'" +addopts = "-v -p no:warnings -ra --color=yes -m 'not vis and not benchmark and not exclude and not tool and not needsdata and not lcm and not ros and not heavy and not tofix'"