From e73218d1e1150bf4015a3e8d134338e5b00fdcee Mon Sep 17 00:00:00 2001 From: lesh Date: Thu, 17 Jul 2025 21:25:38 -0700 Subject: [PATCH 01/36] modifying lcm overrides --- dimos/msgs/foxglove_msgs/Arrow.py | 34 +- dimos/msgs/foxglove_msgs/SceneUpdate.py | 21 ++ dimos/msgs/foxglove_msgs/__init__.py | 1 + dimos/msgs/geometry_msgs/Pose.py | 27 +- dimos/msgs/geometry_msgs/PoseStamped.py | 4 +- dimos/msgs/geometry_msgs/Quaternion.py | 3 - dimos/msgs/geometry_msgs/Transform.py | 17 - dimos/msgs/geometry_msgs/Twist.py | 17 - dimos/msgs/geometry_msgs/Vector3.py | 15 - dimos/msgs/geometry_msgs/test_Pose.py | 3 + dimos/msgs/geometry_msgs/test_Twist.py | 2 + dimos/msgs/sensor_msgs/Image.py | 4 +- dimos/msgs/sensor_msgs/PointCloud2.py | 6 +- dimos/msgs/tf2_msgs/__init__.py | 2 + dimos/msgs/tf2_msgs/test_tfmessage.py | 301 ++++++++++++++++++ dimos/robot/local_planner/simple.py | 3 + .../multiprocess/unitree_go2.py | 6 +- dimos/utils/cli/lcmspy.py | 2 +- pyproject.toml | 2 +- 19 files changed, 359 insertions(+), 111 deletions(-) create mode 100644 dimos/msgs/foxglove_msgs/SceneUpdate.py create mode 100644 dimos/msgs/tf2_msgs/__init__.py create mode 100644 dimos/msgs/tf2_msgs/test_tfmessage.py diff --git a/dimos/msgs/foxglove_msgs/Arrow.py b/dimos/msgs/foxglove_msgs/Arrow.py index c2087071b1..629b92aafb 100644 --- a/dimos/msgs/foxglove_msgs/Arrow.py +++ b/dimos/msgs/foxglove_msgs/Arrow.py @@ -21,10 +21,11 @@ from dimos_lcm.foxglove_msgs import ArrowPrimitive, Color, LinePrimitive from dimos_lcm.geometry_msgs import Point, Vector3 +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Transform import Transform + 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 @@ -46,6 +47,9 @@ class ArrowConfig(TypedDict, total=False): class Arrow(ArrowPrimitive): + pose: Pose + color: Color + @classmethod def from_transform( cls, @@ -85,27 +89,3 @@ def from_transform( ) 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/SceneUpdate.py b/dimos/msgs/foxglove_msgs/SceneUpdate.py new file mode 100644 index 0000000000..3532d63625 --- /dev/null +++ b/dimos/msgs/foxglove_msgs/SceneUpdate.py @@ -0,0 +1,21 @@ +# 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 dimos_lcm.foxglove_msgs import SceneUpdate as LCMSceneUpdate + + +class SceneUpdate(LCMSceneUpdate): + """Wrapper for SceneUpdate to add lcm_encode/decode compatibility.""" diff --git a/dimos/msgs/foxglove_msgs/__init__.py b/dimos/msgs/foxglove_msgs/__init__.py index 638a3480f2..0cd707941d 100644 --- a/dimos/msgs/foxglove_msgs/__init__.py +++ b/dimos/msgs/foxglove_msgs/__init__.py @@ -1 +1,2 @@ from dimos.msgs.foxglove_msgs.Arrow import Arrow +from dimos.msgs.foxglove_msgs.SceneUpdate import SceneUpdate 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..129d0b9082 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -58,11 +58,11 @@ def lcm_encode(self) -> bytes: lcm_mgs.pose = self [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.sec] = 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..2eab991a43 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -76,23 +76,6 @@ def __init__(self, **kwargs): # 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() - def __repr__(self) -> str: return f"Transform(translation={self.translation!r}, rotation={self.rotation!r})" 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_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/tf2_msgs/__init__.py b/dimos/msgs/tf2_msgs/__init__.py new file mode 100644 index 0000000000..579b1865ac --- /dev/null +++ b/dimos/msgs/tf2_msgs/__init__.py @@ -0,0 +1,2 @@ +# tf2_msgs package +# TFMessage is used directly from dimos_lcm.tf2_msgs diff --git a/dimos/msgs/tf2_msgs/test_tfmessage.py b/dimos/msgs/tf2_msgs/test_tfmessage.py new file mode 100644 index 0000000000..013823d77f --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_tfmessage.py @@ -0,0 +1,301 @@ +# 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 numpy as np +import pytest +from dimos_lcm.geometry_msgs import TransformStamped +from dimos_lcm.std_msgs import Header, Time +from dimos_lcm.tf2_msgs import TFMessage + +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic + + +def test_tfmessage_single_transform(): + """Test sending TFMessage with a single transform through LCM.""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + + # Create topic for TFMessage + topic = Topic(topic="/tf", lcm_type=TFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Create a transform from world to base_link + header = Header(seq=1, stamp=Time(sec=123, nsec=456789), frame_id="world") + + # Create the transform - translation and 90 degree rotation around Z + transform = Transform( + translation=Vector3(2.0, 1.0, 0.5), + rotation=Quaternion(0.0, 0.0, 0.707107, 0.707107), # 90 degrees around Z + ) + + # Create TransformStamped + transform_stamped = TransformStamped( + header=header, child_frame_id="base_link", transform=transform + ) + + # Create TFMessage with one transform + tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) + + # Publish the TF message + lcm.publish(topic, tf_message) + + # Wait for message to be received + time.sleep(0.1) + + # Verify reception + assert len(received_messages) == 1 + + received_tf_msg, received_topic = received_messages[0] + + # Verify it's a TFMessage + assert isinstance(received_tf_msg, TFMessage) + assert received_topic == topic + + # Verify the content + assert received_tf_msg.transforms_length == 1 + + # Check the transform + received_transform = received_tf_msg.transforms[0] + assert received_transform.header.seq == 1 + assert received_transform.header.stamp.sec == 123 + assert received_transform.header.stamp.nsec == 456789 + assert received_transform.header.frame_id == "world" + assert received_transform.child_frame_id == "base_link" + + # Check transform values + assert np.isclose(received_transform.transform.translation.x, 2.0, atol=1e-10) + assert np.isclose(received_transform.transform.translation.y, 1.0, atol=1e-10) + assert np.isclose(received_transform.transform.translation.z, 0.5, atol=1e-10) + + assert np.isclose(received_transform.transform.rotation.x, 0.0, atol=1e-10) + assert np.isclose(received_transform.transform.rotation.y, 0.0, atol=1e-10) + assert np.isclose(received_transform.transform.rotation.z, 0.707107, atol=1e-10) + assert np.isclose(received_transform.transform.rotation.w, 0.707107, atol=1e-10) + + +def test_tfmessage_multiple_transforms(): + """Test TFMessage with multiple transforms (robot kinematic chain).""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_multi", lcm_type=TFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Create a kinematic chain: world -> base_link -> torso -> head + current_time = Time(sec=456, nsec=789012) + + transforms = [] + + # 1. world -> base_link (robot at position (1,2,0)) + transforms.append( + TransformStamped( + header=Header(seq=1, stamp=current_time, frame_id="world"), + child_frame_id="base_link", + transform=Transform( + translation=Vector3(1.0, 2.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z + ), + ) + ) + + # 2. base_link -> torso (torso 0.5m up from base) + transforms.append( + TransformStamped( + header=Header(seq=2, stamp=current_time, frame_id="base_link"), + child_frame_id="torso", + transform=Transform( + translation=Vector3(0.0, 0.0, 0.5), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation + ), + ) + ) + + # 3. torso -> head (head 0.3m up and tilted down) + angle = np.pi / 6 # 30 degrees + transforms.append( + TransformStamped( + header=Header(seq=3, stamp=current_time, frame_id="torso"), + child_frame_id="head", + transform=Transform( + translation=Vector3(0.0, 0.0, 0.3), + rotation=Quaternion( + 0.0, np.sin(angle / 2), 0.0, np.cos(angle / 2) + ), # 30 degrees around Y + ), + ) + ) + + # Create TFMessage with multiple transforms + tf_message = TFMessage(transforms_length=len(transforms), transforms=transforms) + + # Publish + lcm.publish(topic, tf_message) + time.sleep(0.1) + + # Verify + assert len(received_messages) == 1 + received_tf_msg, _ = received_messages[0] + + assert received_tf_msg.transforms_length == 3 + + # Verify each transform + for i, (sent, received) in enumerate(zip(transforms, received_tf_msg.transforms)): + assert received.header.seq == sent.header.seq + assert received.header.frame_id == sent.header.frame_id + assert received.child_frame_id == sent.child_frame_id + + # Check translation + assert np.isclose( + received.transform.translation.x, sent.transform.translation.x, atol=1e-10 + ) + assert np.isclose( + received.transform.translation.y, sent.transform.translation.y, atol=1e-10 + ) + assert np.isclose( + received.transform.translation.z, sent.transform.translation.z, atol=1e-10 + ) + + # Check rotation + assert np.isclose(received.transform.rotation.x, sent.transform.rotation.x, atol=1e-10) + assert np.isclose(received.transform.rotation.y, sent.transform.rotation.y, atol=1e-10) + assert np.isclose(received.transform.rotation.z, sent.transform.rotation.z, atol=1e-10) + assert np.isclose(received.transform.rotation.w, sent.transform.rotation.w, atol=1e-10) + + +def test_tfmessage_dynamic_updates(): + """Test sending dynamic TF updates (moving robot).""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_dynamic", lcm_type=TFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Simulate a robot moving in a circle + num_steps = 5 + radius = 2.0 + + for step in range(num_steps): + angle = (2 * np.pi * step) / num_steps + + # Calculate position on circle + x = radius * np.cos(angle) + y = radius * np.sin(angle) + + # Robot orientation faces tangent to circle + robot_angle = angle + np.pi / 2 + + # Create transform + transform_stamped = TransformStamped( + header=Header(seq=step, stamp=Time(sec=100 + step, nsec=0), frame_id="odom"), + child_frame_id="base_link", + transform=Transform( + translation=Vector3(x, y, 0.0), + rotation=Quaternion(0.0, 0.0, np.sin(robot_angle / 2), np.cos(robot_angle / 2)), + ), + ) + + # Send TF update + tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) + + lcm.publish(topic, tf_message) + time.sleep(0.05) # Small delay between updates + + # Wait for all messages + time.sleep(0.1) + + # Should have received all updates + assert len(received_messages) == num_steps + + # Verify each position update + for i, (msg, _) in enumerate(received_messages): + transform = msg.transforms[0] + angle = (2 * np.pi * i) / num_steps + expected_x = radius * np.cos(angle) + expected_y = radius * np.sin(angle) + + assert transform.header.seq == i + assert np.isclose(transform.transform.translation.x, expected_x, atol=1e-10) + assert np.isclose(transform.transform.translation.y, expected_y, atol=1e-10) + assert transform.child_frame_id == "base_link" + assert transform.header.frame_id == "odom" + + +def test_tfmessage_with_dimos_transforms(): + """Test creating TFMessage using our Transform wrapper classes.""" + from dimos.msgs.geometry_msgs import Transform as DimosTransform + + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_dimos", lcm_type=TFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Create transform using Dimos wrapper + dimos_transform = DimosTransform( + translation=Vector3(3.0, 4.0, 5.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) + ) + + # Convert to LCM format + lcm_transform = dimos_transform # Our wrapper inherits from LCM type + + # Create TransformStamped + transform_stamped = TransformStamped( + header=Header(seq=99, stamp=Time(sec=999, nsec=888777), frame_id="map"), + child_frame_id="robot", + transform=lcm_transform, + ) + + # Create and send TFMessage + tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) + + lcm.publish(topic, tf_message) + time.sleep(0.1) + + # Verify + assert len(received_messages) == 1 + received_tf_msg, _ = received_messages[0] + + received_transform = received_tf_msg.transforms[0] + assert received_transform.header.seq == 99 + assert received_transform.header.frame_id == "map" + assert received_transform.child_frame_id == "robot" + + # Verify transform values match + assert np.isclose(received_transform.transform.translation.x, 3.0, atol=1e-10) + assert np.isclose(received_transform.transform.translation.y, 4.0, atol=1e-10) + assert np.isclose(received_transform.transform.translation.z, 5.0, atol=1e-10) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index e9c4dfe545..2402c63d57 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -29,6 +29,7 @@ Pose, PoseLike, PoseStamped, + Transform, Twist, Vector3, VectorLike, @@ -60,6 +61,7 @@ class SimplePlanner(Module): movecmd: Out[Twist] = None arrow: Out[Arrow] = None + transform: Out[Transform] = None get_costmap: Callable[[], Costmap] @@ -80,6 +82,7 @@ def transform_to_robot_frame( ) -> Vector3: transform = global_robot_position.find_transform(global_target) self.arrow.publish(Arrow.from_transform(transform, global_robot_position)) + self.transform.publish(transform) return transform.translation def get_move_stream(self, frequency: float = 20.0) -> rx.Observable: diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index 9fef2492da..933ab3c4c4 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -28,7 +28,7 @@ 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.geometry_msgs import Pose, PoseStamped, Transform, Twist, Vector3 from dimos.msgs.sensor_msgs import Image from dimos.protocol import pubsub from dimos.robot.foxglove_bridge import FoxgloveBridge @@ -170,6 +170,10 @@ async def run(ip): global_planner.path.transport = core.pLCMTransport("/global_path") local_planner.arrow.transport = core.LCMTransport("/arrow", Arrow) + local_planner.transform.transport = core.LCMTransport( + "/transform", + Transform, + ) local_planner.path.connect(global_planner.path) local_planner.odom.connect(connection.odom) diff --git a/dimos/utils/cli/lcmspy.py b/dimos/utils/cli/lcmspy.py index aa5b48b297..134051302c 100755 --- a/dimos/utils/cli/lcmspy.py +++ b/dimos/utils/cli/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/pyproject.toml b/pyproject.toml index b06e9139b0..796f3447b6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ 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@94c6d2a2f6ef7a2d9b22f0fcf3e880a13fc618e8" ] From 24965e2a5773db26006992ef1aa2bc8ea453c864 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 12:38:15 -0700 Subject: [PATCH 02/36] updated dimos-lcm ref to fix tests in ci --- dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py | 1 - pyproject.toml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index 2213c1e87c..a26e2f4bf2 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, Transform, Twist, Vector3 from dimos.msgs.sensor_msgs import Image from dimos.protocol import pubsub diff --git a/pyproject.toml b/pyproject.toml index 735aaf1fff..dc439213d0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@94c6d2a2f6ef7a2d9b22f0fcf3e880a13fc618e8" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@c222046a8c98b800165d0f53fb67094089471a17" ] From cf6876c91474ce0cc3e17c0a0cc080f484ee5c47 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 13:43:07 -0700 Subject: [PATCH 03/36] foxglove bridge fixed to use dimos-lcm refactored code --- bin/foxglove-bridge | 7 +++++++ bin/lcmspy | 2 +- dimos/utils/cli/__init__.py | 0 .../{ => cli/foxglove_bridge}/run_foxglove_bridge.py | 12 +++++++++--- dimos/utils/cli/{ => lcmspy}/lcmspy.py | 0 .../cli/{lcmspy_cli.py => lcmspy/run_lcmspy.py} | 4 ++-- dimos/utils/cli/{ => lcmspy}/test_lcmspy.py | 5 ++--- 7 files changed, 21 insertions(+), 9 deletions(-) create mode 100755 bin/foxglove-bridge create mode 100644 dimos/utils/cli/__init__.py rename dimos/utils/{ => cli/foxglove_bridge}/run_foxglove_bridge.py (83%) rename dimos/utils/cli/{ => lcmspy}/lcmspy.py (100%) rename dimos/utils/cli/{lcmspy_cli.py => lcmspy/run_lcmspy.py} (96%) rename dimos/utils/cli/{ => lcmspy}/test_lcmspy.py (97%) 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/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 83% rename from dimos/utils/run_foxglove_bridge.py rename to dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py index dadb7c2529..c690516a18 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 LcmFoxgloveBridge + +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,11 @@ def bridge_thread(): loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) try: - bridge_runner = bridge.LcmFoxgloveBridgeRunner( + bridge_instance = LcmFoxgloveBridge( 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 100% rename from dimos/utils/cli/lcmspy.py rename to dimos/utils/cli/lcmspy/lcmspy.py 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") From 4d1653b7b27a78812f12bd5a9d6b3bb5d2e05ba5 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 13:46:43 -0700 Subject: [PATCH 04/36] bump dimos-lcm --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index dc439213d0..1f77aece50 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@c222046a8c98b800165d0f53fb67094089471a17" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@1c974457d5636fc2db1005c91d9e44bcb38b3c21" ] From a6666833261054754a154e291396bc19a003b834 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 13:49:50 -0700 Subject: [PATCH 05/36] fixed robot.foxglove_bridge to use the new API --- dimos/robot/foxglove_bridge.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/robot/foxglove_bridge.py b/dimos/robot/foxglove_bridge.py index a0374fc251..f5f0a0fbb6 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 LcmFoxgloveBridge from dimos.core import Module, rpc @@ -35,7 +35,9 @@ 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}") From 118f1d2833853879730eb00d2c782b4a7843afe5 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 14:02:40 -0700 Subject: [PATCH 06/36] workflow rebuilds py --- .github/workflows/docker.yml | 1 + 1 file changed, 1 insertion(+) 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/** From 6a8c26a48e7e26c5bbb7ea4fd525fb9ce3901c65 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 14:05:42 -0700 Subject: [PATCH 07/36] header type, transform tests --- dimos/msgs/std_msgs/Header.py | 87 ++++++++++++++++++++++ dimos/msgs/std_msgs/__init__.py | 17 +++++ dimos/msgs/std_msgs/test_header.py | 100 ++++++++++++++++++++++++++ dimos/msgs/tf2_msgs/test_tfmessage.py | 21 +++--- 4 files changed, 215 insertions(+), 10 deletions(-) create mode 100644 dimos/msgs/std_msgs/Header.py create mode 100644 dimos/msgs/std_msgs/__init__.py create mode 100644 dimos/msgs/std_msgs/test_header.py 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/test_tfmessage.py b/dimos/msgs/tf2_msgs/test_tfmessage.py index 013823d77f..b81346691e 100644 --- a/dimos/msgs/tf2_msgs/test_tfmessage.py +++ b/dimos/msgs/tf2_msgs/test_tfmessage.py @@ -17,10 +17,11 @@ import numpy as np import pytest from dimos_lcm.geometry_msgs import TransformStamped -from dimos_lcm.std_msgs import Header, Time +from dimos_lcm.std_msgs import Time from dimos_lcm.tf2_msgs import TFMessage from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.std_msgs import Header from dimos.protocol.pubsub.lcmpubsub import LCM, Topic @@ -40,7 +41,7 @@ def callback(msg, topic): lcm.subscribe(topic, callback) # Create a transform from world to base_link - header = Header(seq=1, stamp=Time(sec=123, nsec=456789), frame_id="world") + header = Header(123.456789, "world") # Much cleaner! # Create the transform - translation and 90 degree rotation around Z transform = Transform( @@ -78,7 +79,7 @@ def callback(msg, topic): received_transform = received_tf_msg.transforms[0] assert received_transform.header.seq == 1 assert received_transform.header.stamp.sec == 123 - assert received_transform.header.stamp.nsec == 456789 + assert received_transform.header.stamp.nsec == 456789000 assert received_transform.header.frame_id == "world" assert received_transform.child_frame_id == "base_link" @@ -107,14 +108,14 @@ def callback(msg, topic): lcm.subscribe(topic, callback) # Create a kinematic chain: world -> base_link -> torso -> head - current_time = Time(sec=456, nsec=789012) + current_timestamp = 456.789012 transforms = [] # 1. world -> base_link (robot at position (1,2,0)) transforms.append( TransformStamped( - header=Header(seq=1, stamp=current_time, frame_id="world"), + header=Header(current_timestamp, "world"), child_frame_id="base_link", transform=Transform( translation=Vector3(1.0, 2.0, 0.0), @@ -126,7 +127,7 @@ def callback(msg, topic): # 2. base_link -> torso (torso 0.5m up from base) transforms.append( TransformStamped( - header=Header(seq=2, stamp=current_time, frame_id="base_link"), + header=Header(current_timestamp, "base_link"), child_frame_id="torso", transform=Transform( translation=Vector3(0.0, 0.0, 0.5), @@ -139,7 +140,7 @@ def callback(msg, topic): angle = np.pi / 6 # 30 degrees transforms.append( TransformStamped( - header=Header(seq=3, stamp=current_time, frame_id="torso"), + header=Header(current_timestamp, "torso"), child_frame_id="head", transform=Transform( translation=Vector3(0.0, 0.0, 0.3), @@ -214,9 +215,9 @@ def callback(msg, topic): # Robot orientation faces tangent to circle robot_angle = angle + np.pi / 2 - # Create transform + # Create transform with current system time transform_stamped = TransformStamped( - header=Header(seq=step, stamp=Time(sec=100 + step, nsec=0), frame_id="odom"), + header=Header.now("odom", seq=step), # Use current system timestamp with step as seq child_frame_id="base_link", transform=Transform( translation=Vector3(x, y, 0.0), @@ -275,7 +276,7 @@ def callback(msg, topic): # Create TransformStamped transform_stamped = TransformStamped( - header=Header(seq=99, stamp=Time(sec=999, nsec=888777), frame_id="map"), + header=Header(999.000888777, "map", seq=99), child_frame_id="robot", transform=lcm_transform, ) From 0181ad89f4dcc0e93784ca0274a900b620eb61d3 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 17:33:50 -0700 Subject: [PATCH 08/36] tfmessage --- dimos/msgs/geometry_msgs/Transform.py | 75 ++--- dimos/msgs/geometry_msgs/test_Transform.py | 104 ++---- dimos/msgs/tf2_msgs/TFMessage.py | 146 +++++++++ dimos/msgs/tf2_msgs/__init__.py | 19 +- dimos/msgs/tf2_msgs/test_TFMessage.py | 178 +++++++++++ .../msgs/tf2_msgs/test_publish_transforms.py | 262 +++++++++++++++ dimos/msgs/tf2_msgs/test_tfmessage.py | 302 ------------------ dimos/robot/unitree_webrtc/type/lidar.py | 2 +- 8 files changed, 654 insertions(+), 434 deletions(-) create mode 100644 dimos/msgs/tf2_msgs/TFMessage.py create mode 100644 dimos/msgs/tf2_msgs/test_TFMessage.py create mode 100644 dimos/msgs/tf2_msgs/test_publish_transforms.py delete mode 100644 dimos/msgs/tf2_msgs/test_tfmessage.py diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index 2eab991a43..6c44cf6b23 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -15,66 +15,41 @@ 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.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) + 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})" diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index 64f8c6f8b1..69318e4d12 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,65 +191,26 @@ 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) assert np.isclose(transformed_pose2.orientation.z, np.sin(total_angle2 / 2), atol=1e-10) 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 - 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) - - # 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 - - 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 diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py new file mode 100644 index 0000000000..19c2bb52fd --- /dev/null +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -0,0 +1,146 @@ +# 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] + + 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, child_frame_ids: list[str] | None = None) -> 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. + """ + # Create LCM TransformStamped objects + lcm_transforms = [] + + for i, transform in enumerate(self.transforms): + # Determine child_frame_id + if child_frame_ids and i < len(child_frame_ids): + child_frame_id = child_frame_ids[i] + else: + child_frame_id = "base_link" + + # Create TransformStamped + transform_stamped = LCMTransformStamped() + + # Build header + sec = int(transform.ts) + nsec = int((transform.ts - sec) * 1_000_000_000) + transform_stamped.header = LCMHeader( + seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=transform.frame_id + ) + transform_stamped.child_frame_id = child_frame_id + + # Build transform + transform_stamped.transform = LCMTransform( + translation=transform.translation, rotation=transform.rotation + ) + + lcm_transforms.append(transform_stamped) + + # Create LCM TFMessage + lcm_msg = LCMTFMessage(transforms_length=len(lcm_transforms), transforms=lcm_transforms) + + 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 index 579b1865ac..683e4ec61b 100644 --- a/dimos/msgs/tf2_msgs/__init__.py +++ b/dimos/msgs/tf2_msgs/__init__.py @@ -1,2 +1,17 @@ -# tf2_msgs package -# TFMessage is used directly from dimos_lcm.tf2_msgs +# 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..b0164f6bae --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -0,0 +1,178 @@ +# 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(): + """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), + 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="map", + ts=124.567, + ) + + # Create TFMessage + msg = TFMessage(tf1, tf2) + + # Encode with custom child_frame_ids + encoded = msg.lcm_encode(child_frame_ids=["robot", "sensor"]) + + # 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 == "map" + assert ts2.child_frame_id == "sensor" + assert ts2.transform.rotation.z == 0.707 + assert ts2.transform.rotation.w == 0.707 + + +def test_tfmessage_lcm_encode_default_child_ids(): + """Test encoding with default child_frame_ids.""" + tf = Transform(translation=Vector3(1, 2, 3), frame_id="odom", ts=100.0) + msg = TFMessage(tf) + + encoded = msg.lcm_encode() # No child_frame_ids provided + + lcm_msg = LCMTFMessage.lcm_decode(encoded) + assert lcm_msg.transforms[0].child_frame_id == "base_link" + + +def test_tfmessage_lcm_decode(): + """Test decoding TFMessage from LCM bytes.""" + # Create original message + tf1 = Transform( + translation=Vector3(10, 20, 30), + rotation=Quaternion(0, 0, 0, 1), + frame_id="base", + ts=500.123, + ) + tf2 = Transform( + translation=Vector3(40, 50, 60), + rotation=Quaternion(0.5, 0.5, 0.5, 0.5), + frame_id="camera", + ts=501.234, + ) + + original = TFMessage(tf1, tf2) + + # Encode and decode + encoded = original.lcm_encode() + decoded = TFMessage.lcm_decode(encoded) + + # Verify + assert len(decoded) == 2 + + # Check first transform + assert decoded[0].translation.x == 10 + assert decoded[0].translation.y == 20 + assert decoded[0].translation.z == 30 + assert decoded[0].frame_id == "base" + assert abs(decoded[0].ts - 500.123) < 1e-6 + + # Check second transform + assert decoded[1].translation.x == 40 + assert decoded[1].rotation.x == 0.5 + assert decoded[1].rotation.y == 0.5 + assert decoded[1].rotation.z == 0.5 + assert decoded[1].rotation.w == 0.5 + assert decoded[1].frame_id == "camera" + + +def test_tfmessage_string_representations(): + """Test string representations of TFMessage.""" + tf1 = Transform(frame_id="world", ts=100.123) + tf2 = Transform(frame_id="map", ts=101.456) + + msg = TFMessage(tf1, tf2) + + # Test repr + repr_str = repr(msg) + assert "TFMessage" in repr_str + assert "2 transforms" in repr_str + + # Test str + str_str = str(msg) + assert "TFMessage with 2 transforms:" in str_str + assert "[0] world @ 100.123" in str_str + assert "[1] map @ 101.456" in str_str diff --git a/dimos/msgs/tf2_msgs/test_publish_transforms.py b/dimos/msgs/tf2_msgs/test_publish_transforms.py new file mode 100644 index 0000000000..d22fecd9c3 --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_publish_transforms.py @@ -0,0 +1,262 @@ +# 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 numpy as np +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 +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic + + +def test_publish_transforms_with_new_types(): + """Test publishing transforms using our new Transform and TFMessage types.""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_fancy", lcm_type=LCMTFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # 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(2.0, 3.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z + frame_id="world", + ts=current_time, + ) + + # 2. Base to arm transform (arm lifted up) + base_to_arm = Transform( + translation=Vector3(0.2, 0.0, 0.5), + rotation=Quaternion(0.0, 0.258819, 0.0, 0.965926), # 30 degrees around Y + frame_id="base_link", + ts=current_time, + ) + + # 3. Arm to gripper transform (gripper extended) + arm_to_gripper = Transform( + translation=Vector3(0.3, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation + frame_id="arm_link", + ts=current_time, + ) + + # Create TFMessage with all transforms + tf_msg = TFMessage(world_to_base, base_to_arm, arm_to_gripper) + + # Encode to LCM bytes with proper child frame IDs + encoded = tf_msg.lcm_encode(child_frame_ids=["base_link", "arm_link", "gripper_link"]) + + # Decode back to LCM TFMessage and publish + lcm_msg = LCMTFMessage.lcm_decode(encoded) + lcm.publish(topic, lcm_msg) + + # Wait for reception + time.sleep(0.1) + + # Verify we received the message + assert len(received_messages) == 1 + received_msg, received_topic = received_messages[0] + + # Verify it's an LCM TFMessage + assert isinstance(received_msg, LCMTFMessage) + assert received_topic == topic + + # Verify content + assert received_msg.transforms_length == 3 + + # Check world to base transform + tf0 = received_msg.transforms[0] + assert tf0.header.frame_id == "world" + assert tf0.child_frame_id == "base_link" + assert np.isclose(tf0.transform.translation.x, 2.0, atol=1e-10) + assert np.isclose(tf0.transform.translation.y, 3.0, atol=1e-10) + assert np.isclose(tf0.transform.rotation.z, 0.382683, atol=1e-6) + assert np.isclose(tf0.transform.rotation.w, 0.923880, atol=1e-6) + + # Check base to arm transform + tf1 = received_msg.transforms[1] + assert tf1.header.frame_id == "base_link" + assert tf1.child_frame_id == "arm_link" + assert np.isclose(tf1.transform.translation.x, 0.2, atol=1e-10) + assert np.isclose(tf1.transform.translation.z, 0.5, atol=1e-10) + assert np.isclose(tf1.transform.rotation.y, 0.258819, atol=1e-6) + assert np.isclose(tf1.transform.rotation.w, 0.965926, atol=1e-6) + + # Check arm to gripper transform + tf2 = received_msg.transforms[2] + assert tf2.header.frame_id == "arm_link" + assert tf2.child_frame_id == "gripper_link" + assert np.isclose(tf2.transform.translation.x, 0.3, atol=1e-10) + assert tf2.transform.rotation.w == 1.0 + + # Verify timestamps are preserved + for tf in received_msg.transforms: + received_ts = tf.header.stamp.sec + (tf.header.stamp.nsec / 1_000_000_000) + assert abs(received_ts - current_time) < 1e-6 + + +def test_dynamic_robot_movement(): + """Test publishing dynamic transforms as robot moves.""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_movement", lcm_type=LCMTFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Simulate robot moving forward and turning + num_steps = 4 + for i in range(num_steps): + t = i * 0.1 # Time progression + + # Robot moves forward and turns + x = t * 2.0 # Move 2 m/s forward + y = 0.5 * np.sin(t * np.pi) # Slight sinusoidal motion + angle = t * (np.pi / 4) # Turn 45 degrees per second + + # Create transform for current robot pose + robot_pose = Transform( + translation=Vector3(x, y, 0.0), + rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), + frame_id="odom", + ts=time.time(), + ) + + # Robot has a sensor that's always 0.3m above base + sensor_transform = Transform( + translation=Vector3(0.0, 0.0, 0.3), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + ts=robot_pose.ts, # Same timestamp + ) + + # Create and publish TFMessage + tf_msg = TFMessage(robot_pose, sensor_transform) + encoded = tf_msg.lcm_encode(child_frame_ids=["base_link", "sensor_link"]) + + lcm_msg = LCMTFMessage.lcm_decode(encoded) + lcm.publish(topic, lcm_msg) + time.sleep(0.05) + + # Wait for all messages + time.sleep(0.1) + + # Should have received all updates + assert len(received_messages) == num_steps + + # Verify motion progression + for i, (msg, _) in enumerate(received_messages): + t = i * 0.1 + expected_x = t * 2.0 + expected_y = 0.5 * np.sin(t * np.pi) + + # Check robot transform + robot_tf = msg.transforms[0] + assert robot_tf.header.frame_id == "odom" + assert robot_tf.child_frame_id == "base_link" + assert np.isclose(robot_tf.transform.translation.x, expected_x, atol=1e-10) + assert np.isclose(robot_tf.transform.translation.y, expected_y, atol=1e-10) + + # Check sensor is always 0.3m above base + sensor_tf = msg.transforms[1] + assert sensor_tf.header.frame_id == "base_link" + assert sensor_tf.child_frame_id == "sensor_link" + assert np.isclose(sensor_tf.transform.translation.z, 0.3, atol=1e-10) + + +def test_roundtrip_transform_types(): + """Test that our types can roundtrip through LCM.""" + lcm = LCM(autoconf=True) + lcm.start() + + received_messages = [] + topic = Topic(topic="/tf_roundtrip", lcm_type=LCMTFMessage) + + def callback(msg, topic): + received_messages.append((msg, topic)) + + lcm.subscribe(topic, callback) + + # Create transforms with various configurations + tf1 = Transform( + translation=Vector3(1.5, -2.5, 3.5), + rotation=Quaternion(0.1, 0.2, 0.3, 0.9045), # Normalized quaternion + frame_id="sensor1", + ts=1234.5678, + ) + + tf2 = Transform( + translation=Vector3(-10, 20, -30), + rotation=Quaternion(0.5, 0.5, 0.5, 0.5), # All equal components + frame_id="sensor2", + ts=2345.6789, + ) + + # Create TFMessage + original = TFMessage(tf1, tf2) + + # Encode and publish + encoded = original.lcm_encode(child_frame_ids=["child1", "child2"]) + lcm_msg = LCMTFMessage.lcm_decode(encoded) + lcm.publish(topic, lcm_msg) + + time.sleep(0.1) + + # Decode received message back to our types + assert len(received_messages) == 1 + received_lcm_msg, _ = received_messages[0] + + # Decode back to our TFMessage + decoded = TFMessage.lcm_decode(received_lcm_msg.lcm_encode()) + + # Verify transforms match + assert len(decoded) == 2 + + # Check first transform + assert decoded[0].translation.x == 1.5 + assert decoded[0].translation.y == -2.5 + assert decoded[0].translation.z == 3.5 + assert np.isclose(decoded[0].rotation.x, 0.1, atol=1e-10) + assert np.isclose(decoded[0].rotation.y, 0.2, atol=1e-10) + assert np.isclose(decoded[0].rotation.z, 0.3, atol=1e-10) + assert np.isclose(decoded[0].rotation.w, 0.9045, atol=1e-10) + assert decoded[0].frame_id == "sensor1" + assert abs(decoded[0].ts - 1234.5678) < 1e-6 + + # Check second transform + assert decoded[1].translation.x == -10 + assert decoded[1].translation.y == 20 + assert decoded[1].translation.z == -30 + assert decoded[1].rotation.x == 0.5 + assert decoded[1].rotation.y == 0.5 + assert decoded[1].rotation.z == 0.5 + assert decoded[1].rotation.w == 0.5 + assert decoded[1].frame_id == "sensor2" + assert abs(decoded[1].ts - 2345.6789) < 1e-6 diff --git a/dimos/msgs/tf2_msgs/test_tfmessage.py b/dimos/msgs/tf2_msgs/test_tfmessage.py deleted file mode 100644 index b81346691e..0000000000 --- a/dimos/msgs/tf2_msgs/test_tfmessage.py +++ /dev/null @@ -1,302 +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. - -import time - -import numpy as np -import pytest -from dimos_lcm.geometry_msgs import TransformStamped -from dimos_lcm.std_msgs import Time -from dimos_lcm.tf2_msgs import TFMessage - -from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 -from dimos.msgs.std_msgs import Header -from dimos.protocol.pubsub.lcmpubsub import LCM, Topic - - -def test_tfmessage_single_transform(): - """Test sending TFMessage with a single transform through LCM.""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - - # Create topic for TFMessage - topic = Topic(topic="/tf", lcm_type=TFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Create a transform from world to base_link - header = Header(123.456789, "world") # Much cleaner! - - # Create the transform - translation and 90 degree rotation around Z - transform = Transform( - translation=Vector3(2.0, 1.0, 0.5), - rotation=Quaternion(0.0, 0.0, 0.707107, 0.707107), # 90 degrees around Z - ) - - # Create TransformStamped - transform_stamped = TransformStamped( - header=header, child_frame_id="base_link", transform=transform - ) - - # Create TFMessage with one transform - tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) - - # Publish the TF message - lcm.publish(topic, tf_message) - - # Wait for message to be received - time.sleep(0.1) - - # Verify reception - assert len(received_messages) == 1 - - received_tf_msg, received_topic = received_messages[0] - - # Verify it's a TFMessage - assert isinstance(received_tf_msg, TFMessage) - assert received_topic == topic - - # Verify the content - assert received_tf_msg.transforms_length == 1 - - # Check the transform - received_transform = received_tf_msg.transforms[0] - assert received_transform.header.seq == 1 - assert received_transform.header.stamp.sec == 123 - assert received_transform.header.stamp.nsec == 456789000 - assert received_transform.header.frame_id == "world" - assert received_transform.child_frame_id == "base_link" - - # Check transform values - assert np.isclose(received_transform.transform.translation.x, 2.0, atol=1e-10) - assert np.isclose(received_transform.transform.translation.y, 1.0, atol=1e-10) - assert np.isclose(received_transform.transform.translation.z, 0.5, atol=1e-10) - - assert np.isclose(received_transform.transform.rotation.x, 0.0, atol=1e-10) - assert np.isclose(received_transform.transform.rotation.y, 0.0, atol=1e-10) - assert np.isclose(received_transform.transform.rotation.z, 0.707107, atol=1e-10) - assert np.isclose(received_transform.transform.rotation.w, 0.707107, atol=1e-10) - - -def test_tfmessage_multiple_transforms(): - """Test TFMessage with multiple transforms (robot kinematic chain).""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_multi", lcm_type=TFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Create a kinematic chain: world -> base_link -> torso -> head - current_timestamp = 456.789012 - - transforms = [] - - # 1. world -> base_link (robot at position (1,2,0)) - transforms.append( - TransformStamped( - header=Header(current_timestamp, "world"), - child_frame_id="base_link", - transform=Transform( - translation=Vector3(1.0, 2.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z - ), - ) - ) - - # 2. base_link -> torso (torso 0.5m up from base) - transforms.append( - TransformStamped( - header=Header(current_timestamp, "base_link"), - child_frame_id="torso", - transform=Transform( - translation=Vector3(0.0, 0.0, 0.5), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation - ), - ) - ) - - # 3. torso -> head (head 0.3m up and tilted down) - angle = np.pi / 6 # 30 degrees - transforms.append( - TransformStamped( - header=Header(current_timestamp, "torso"), - child_frame_id="head", - transform=Transform( - translation=Vector3(0.0, 0.0, 0.3), - rotation=Quaternion( - 0.0, np.sin(angle / 2), 0.0, np.cos(angle / 2) - ), # 30 degrees around Y - ), - ) - ) - - # Create TFMessage with multiple transforms - tf_message = TFMessage(transforms_length=len(transforms), transforms=transforms) - - # Publish - lcm.publish(topic, tf_message) - time.sleep(0.1) - - # Verify - assert len(received_messages) == 1 - received_tf_msg, _ = received_messages[0] - - assert received_tf_msg.transforms_length == 3 - - # Verify each transform - for i, (sent, received) in enumerate(zip(transforms, received_tf_msg.transforms)): - assert received.header.seq == sent.header.seq - assert received.header.frame_id == sent.header.frame_id - assert received.child_frame_id == sent.child_frame_id - - # Check translation - assert np.isclose( - received.transform.translation.x, sent.transform.translation.x, atol=1e-10 - ) - assert np.isclose( - received.transform.translation.y, sent.transform.translation.y, atol=1e-10 - ) - assert np.isclose( - received.transform.translation.z, sent.transform.translation.z, atol=1e-10 - ) - - # Check rotation - assert np.isclose(received.transform.rotation.x, sent.transform.rotation.x, atol=1e-10) - assert np.isclose(received.transform.rotation.y, sent.transform.rotation.y, atol=1e-10) - assert np.isclose(received.transform.rotation.z, sent.transform.rotation.z, atol=1e-10) - assert np.isclose(received.transform.rotation.w, sent.transform.rotation.w, atol=1e-10) - - -def test_tfmessage_dynamic_updates(): - """Test sending dynamic TF updates (moving robot).""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_dynamic", lcm_type=TFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Simulate a robot moving in a circle - num_steps = 5 - radius = 2.0 - - for step in range(num_steps): - angle = (2 * np.pi * step) / num_steps - - # Calculate position on circle - x = radius * np.cos(angle) - y = radius * np.sin(angle) - - # Robot orientation faces tangent to circle - robot_angle = angle + np.pi / 2 - - # Create transform with current system time - transform_stamped = TransformStamped( - header=Header.now("odom", seq=step), # Use current system timestamp with step as seq - child_frame_id="base_link", - transform=Transform( - translation=Vector3(x, y, 0.0), - rotation=Quaternion(0.0, 0.0, np.sin(robot_angle / 2), np.cos(robot_angle / 2)), - ), - ) - - # Send TF update - tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) - - lcm.publish(topic, tf_message) - time.sleep(0.05) # Small delay between updates - - # Wait for all messages - time.sleep(0.1) - - # Should have received all updates - assert len(received_messages) == num_steps - - # Verify each position update - for i, (msg, _) in enumerate(received_messages): - transform = msg.transforms[0] - angle = (2 * np.pi * i) / num_steps - expected_x = radius * np.cos(angle) - expected_y = radius * np.sin(angle) - - assert transform.header.seq == i - assert np.isclose(transform.transform.translation.x, expected_x, atol=1e-10) - assert np.isclose(transform.transform.translation.y, expected_y, atol=1e-10) - assert transform.child_frame_id == "base_link" - assert transform.header.frame_id == "odom" - - -def test_tfmessage_with_dimos_transforms(): - """Test creating TFMessage using our Transform wrapper classes.""" - from dimos.msgs.geometry_msgs import Transform as DimosTransform - - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_dimos", lcm_type=TFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Create transform using Dimos wrapper - dimos_transform = DimosTransform( - translation=Vector3(3.0, 4.0, 5.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) - ) - - # Convert to LCM format - lcm_transform = dimos_transform # Our wrapper inherits from LCM type - - # Create TransformStamped - transform_stamped = TransformStamped( - header=Header(999.000888777, "map", seq=99), - child_frame_id="robot", - transform=lcm_transform, - ) - - # Create and send TFMessage - tf_message = TFMessage(transforms_length=1, transforms=[transform_stamped]) - - lcm.publish(topic, tf_message) - time.sleep(0.1) - - # Verify - assert len(received_messages) == 1 - received_tf_msg, _ = received_messages[0] - - received_transform = received_tf_msg.transforms[0] - assert received_transform.header.seq == 99 - assert received_transform.header.frame_id == "map" - assert received_transform.child_frame_id == "robot" - - # Verify transform values match - assert np.isclose(received_transform.transform.translation.x, 3.0, atol=1e-10) - assert np.isclose(received_transform.transform.translation.y, 4.0, atol=1e-10) - assert np.isclose(received_transform.transform.translation.z, 5.0, atol=1e-10) 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") From 1c4f3585e452dd825a6ef3c69a8a3228618b9d71 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 17:41:34 -0700 Subject: [PATCH 09/36] triggering py rebuild --- pyproject.toml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 1f77aece50..49b4c34d2e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,8 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@1c974457d5636fc2db1005c91d9e44bcb38b3c21" - + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@1c974457d5636fc2db1005c91d9e44bcb38b3c21" ] From a4259121cf1657213f1d331404ac77e424568f1a Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 17:52:29 -0700 Subject: [PATCH 10/36] transform auto-encodes to tfmessage --- dimos/msgs/geometry_msgs/Transform.py | 41 ++++++++++++++++++++++ dimos/msgs/geometry_msgs/test_Transform.py | 14 ++++++++ 2 files changed, 55 insertions(+) diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index 6c44cf6b23..f0b5d47e2f 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -67,3 +67,44 @@ def __eq__(self, other) -> bool: def identity(cls) -> Transform: """Create an identity transform.""" return cls() + + def lcm_encode(self) -> bytes: + 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/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index 69318e4d12..3ffa8ce234 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -214,3 +214,17 @@ def test_pose_add_transform_with_rotation(): assert np.isclose(transformed_pose2.orientation.y, 0.0, atol=1e-10) assert np.isclose(transformed_pose2.orientation.z, np.sin(total_angle2 / 2), atol=1e-10) assert np.isclose(transformed_pose2.orientation.w, np.cos(total_angle2 / 2), atol=1e-10) + + +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)), + ) + + data = transform.lcm_encode() + + decoded_transform = Transform.lcm_decode(data) + + assert decoded_transform == transform From b3ea447ce0eece47f8eee83d2b8804beb8bdfbed Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 18:12:40 -0700 Subject: [PATCH 11/36] publish base_link transform --- .../unitree_webrtc/multiprocess/unitree_go2.py | 18 +++++++++++++++--- dimos/robot/unitree_webrtc/type/odometry.py | 3 +++ 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index a26e2f4bf2..c00aa76c02 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -81,19 +81,16 @@ def liedown(self): @functools.cache def lidar_stream(self): - print("lidar stream start") lidar_store = TimedSensorReplay("unitree_office_walk/lidar", autocast=LidarMessage.from_msg) return lidar_store.stream() @functools.cache def odom_stream(self): - print("odom stream start") odom_store = TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) return odom_store.stream() @functools.cache def video_stream(self, freq_hz=0.5): - print("video stream start") video_store = TimedSensorReplay("unitree_office_walk/video", autocast=Image.from_numpy) return video_store.stream().pipe(ops.sample(freq_hz)) @@ -106,6 +103,7 @@ class ConnectionModule(FakeRTC, Module): odom: Out[Vector3] = None lidar: Out[LidarMessage] = None video: Out[VideoMessage] = None + tf: Out[Transform] = None ip: str _odom: Callable[[], Odometry] @@ -119,6 +117,16 @@ def __init__(self, ip: str, *args, **kwargs): self.ip = ip Module.__init__(self, *args, **kwargs) + def _odom_to_tf(self, odom: Odometry) -> Transform: + """Convert Odometry to Transform.""" + return Transform( + translation=odom.position, + rotation=odom.orientation, + frame_id="world", + child_frame_id="base_link", + ts=odom.ts, + ) + @rpc def start(self): # Initialize the parent WebRTC connection @@ -129,6 +137,8 @@ def start(self): self.odom_stream().subscribe(self.odom.publish) self.video_stream().subscribe(self.video.publish) + self.odom_stream().pipe(ops.map(self._odom_to_tf)).subscribe(self.tf.publish) + # Connect LCM input to robot movement commands self.movecmd.subscribe(self.move) @@ -192,6 +202,8 @@ async def start(self): self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) # OUTPUT: Camera video frames to /video topic self.connection.video.transport = core.LCMTransport("/video", Image) + # OUTPUT: transforms to /tf topic + self.connection.tf.transport = core.LCMTransport("/tf", Transform) # ====================================================================== # Map Module - Point cloud accumulation and costmap generation ========= 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"] From e401f99e3ee07309e1e7e123b9aa62355dba1d25 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 18 Jul 2025 23:45:09 -0700 Subject: [PATCH 12/36] test run --- dimos/msgs/tf2_msgs/TFMessage.py | 18 ++-- dimos/msgs/tf2_msgs/test_TFMessage.py | 83 ++----------------- dimos/robot/global_planner/planner.py | 4 +- dimos/robot/local_planner/simple.py | 23 +++-- .../multiprocess/example_usage.py | 12 +-- .../multiprocess/unitree_go2.py | 42 +++++----- 6 files changed, 64 insertions(+), 118 deletions(-) diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index 19c2bb52fd..ccbbd4e3c2 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -42,6 +42,7 @@ 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) @@ -51,7 +52,7 @@ def add_transform(self, transform: Transform, child_frame_id: str = "base_link") self.transforms.append(transform) self.transforms_length = len(self.transforms) - def lcm_encode(self, child_frame_ids: list[str] | None = None) -> bytes: + def lcm_encode(self) -> bytes: """Encode as LCM TFMessage. Args: @@ -62,22 +63,19 @@ def lcm_encode(self, child_frame_ids: list[str] | None = None) -> bytes: lcm_transforms = [] for i, transform in enumerate(self.transforms): - # Determine child_frame_id - if child_frame_ids and i < len(child_frame_ids): - child_frame_id = child_frame_ids[i] - else: - child_frame_id = "base_link" - # Create TransformStamped - transform_stamped = LCMTransformStamped() + transform_stamped = LCMTransformStamped( + child_frame_id=transform.child_frame_id, + ) # Build header sec = int(transform.ts) nsec = int((transform.ts - sec) * 1_000_000_000) transform_stamped.header = LCMHeader( - seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=transform.frame_id + seq=1, + stamp=LCMTime(sec=sec, nsec=nsec), + frame_id=transform.frame_id, ) - transform_stamped.child_frame_id = child_frame_id # Build transform transform_stamped.transform = LCMTransform( diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index b0164f6bae..6eec4cbdcc 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -62,19 +62,21 @@ def test_tfmessage_add_transform(): assert msg[0] == tf -def test_tfmessage_lcm_encode(): +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="map", + frame_id="robot", + child_frame_id="target", ts=124.567, ) @@ -82,7 +84,7 @@ def test_tfmessage_lcm_encode(): msg = TFMessage(tf1, tf2) # Encode with custom child_frame_ids - encoded = msg.lcm_encode(child_frame_ids=["robot", "sensor"]) + encoded = msg.lcm_encode() # Decode using LCM to verify lcm_msg = LCMTFMessage.lcm_decode(encoded) @@ -101,78 +103,7 @@ def test_tfmessage_lcm_encode(): # Check second transform ts2 = lcm_msg.transforms[1] - assert ts2.header.frame_id == "map" - assert ts2.child_frame_id == "sensor" + 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 - - -def test_tfmessage_lcm_encode_default_child_ids(): - """Test encoding with default child_frame_ids.""" - tf = Transform(translation=Vector3(1, 2, 3), frame_id="odom", ts=100.0) - msg = TFMessage(tf) - - encoded = msg.lcm_encode() # No child_frame_ids provided - - lcm_msg = LCMTFMessage.lcm_decode(encoded) - assert lcm_msg.transforms[0].child_frame_id == "base_link" - - -def test_tfmessage_lcm_decode(): - """Test decoding TFMessage from LCM bytes.""" - # Create original message - tf1 = Transform( - translation=Vector3(10, 20, 30), - rotation=Quaternion(0, 0, 0, 1), - frame_id="base", - ts=500.123, - ) - tf2 = Transform( - translation=Vector3(40, 50, 60), - rotation=Quaternion(0.5, 0.5, 0.5, 0.5), - frame_id="camera", - ts=501.234, - ) - - original = TFMessage(tf1, tf2) - - # Encode and decode - encoded = original.lcm_encode() - decoded = TFMessage.lcm_decode(encoded) - - # Verify - assert len(decoded) == 2 - - # Check first transform - assert decoded[0].translation.x == 10 - assert decoded[0].translation.y == 20 - assert decoded[0].translation.z == 30 - assert decoded[0].frame_id == "base" - assert abs(decoded[0].ts - 500.123) < 1e-6 - - # Check second transform - assert decoded[1].translation.x == 40 - assert decoded[1].rotation.x == 0.5 - assert decoded[1].rotation.y == 0.5 - assert decoded[1].rotation.z == 0.5 - assert decoded[1].rotation.w == 0.5 - assert decoded[1].frame_id == "camera" - - -def test_tfmessage_string_representations(): - """Test string representations of TFMessage.""" - tf1 = Transform(frame_id="world", ts=100.123) - tf2 = Transform(frame_id="map", ts=101.456) - - msg = TFMessage(tf1, tf2) - - # Test repr - repr_str = repr(msg) - assert "TFMessage" in repr_str - assert "2 transforms" in repr_str - - # Test str - str_str = str(msg) - assert "TFMessage with 2 transforms:" in str_str - assert "[0] world @ 100.123" in str_str - assert "[1] map @ 101.456" in str_str 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 1fe75ff479..4b771f3124 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -34,6 +34,7 @@ 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 - transform: Out[Transform] = 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.transform.publish(transform) - 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/unitree_webrtc/multiprocess/example_usage.py b/dimos/robot/unitree_webrtc/multiprocess/example_usage.py index f666bfa814..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.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 c00aa76c02..a70d0d0c1b 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -29,12 +29,14 @@ from dimos.core import In, Module, Out, rpc from dimos.msgs.geometry_msgs import Pose, PoseStamped, Transform, Twist, Vector3 from dimos.msgs.sensor_msgs import Image +from dimos.msgs.tf2_msgs import TFMessage from dimos.protocol import pubsub from dimos.robot.foxglove_bridge import FoxgloveBridge from dimos.robot.frontier_exploration.wavefront_frontier_goal_selector import ( WavefrontFrontierExplorer, ) from dimos.robot.global_planner import AstarPlanner +from dimos.robot.local_planner.simple import SimplePlanner from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection, VideoMessage from dimos.robot.unitree_webrtc.type.lidar import LidarMessage @@ -137,7 +139,7 @@ def start(self): self.odom_stream().subscribe(self.odom.publish) self.video_stream().subscribe(self.video.publish) - self.odom_stream().pipe(ops.map(self._odom_to_tf)).subscribe(self.tf.publish) + # self.odom_stream().pipe(ops.map(self._odom_to_tf)).subscribe(self.tf.publish) # Connect LCM input to robot movement commands self.movecmd.subscribe(self.move) @@ -167,7 +169,8 @@ def start(self): def plancmd(): time.sleep(4) print(colors.red("requesting global plan")) - self.plancmd.publish(Pose(0, 0, 0, 0, 0, 0, 1)) + # self.plancmd.publish(Pose(0, 0, 0, 0, 0, 0, 1)) + self.plancmd.publish(Pose(4.0, 4.0, 0, 0, 0, 0, 1)) thread = threading.Thread(target=plancmd, daemon=True) thread.start() @@ -202,8 +205,6 @@ async def start(self): self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) # OUTPUT: Camera video frames to /video topic self.connection.video.transport = core.LCMTransport("/video", Image) - # OUTPUT: transforms to /tf topic - self.connection.tf.transport = core.LCMTransport("/tf", Transform) # ====================================================================== # Map Module - Point cloud accumulation and costmap generation ========= @@ -216,11 +217,26 @@ async def start(self): self.mapper.lidar.connect(self.connection.lidar) # ==================================================================== + # Global Planner Module =============== + self.global_planner = self.dimos.deploy( + AstarPlanner, + get_costmap=self.mapper.costmap, + get_robot_pos=self.connection.get_pos, + # set_local_nav=self.local_planner.navigate_path_local, + ) + + # Configure AstarPlanner OUTPUT path: Out[Path] to /global_path LCM topic + self.global_planner.path.transport = core.pLCMTransport("/global_path") + # ====================================== + # Local planner Module, LCM transport & connection ================ self.local_planner = self.dimos.deploy( - VFHPurePursuitPlanner, + SimplePlanner, get_costmap=self.connection.get_local_costmap, ) + self.local_planner.tf.transport = core.LCMTransport("/tf", TFMessage) + + self.local_planner.path.connect(self.global_planner.path) # Connects odometry LCM stream to BaseLocalPlanner odometry input self.local_planner.odom.connect(self.connection.odom) @@ -232,24 +248,12 @@ async def start(self): self.connection.movecmd.connect(self.local_planner.movecmd) # =================================================================== - # Global Planner Module =============== - self.global_planner = self.dimos.deploy( - AstarPlanner, - get_costmap=self.mapper.costmap, - get_robot_pos=self.connection.get_pos, - set_local_nav=self.local_planner.navigate_path_local, - ) - - # Configure AstarPlanner OUTPUT path: Out[Path] to /global_path LCM topic - self.global_planner.path.transport = core.pLCMTransport("/global_path") - # ====================================== - # Global Planner Control Module =========================== # Debug module that sends (0,0,0) goal after 4 second delay self.ctrl = self.dimos.deploy(ControlModule) # Configure ControlModule OUTPUT to publish goal coordinates to /global_target - self.ctrl.plancmd.transport = core.LCMTransport("/global_target", Vector3) + self.ctrl.plancmd.transport = core.LCMTransport("/global_target", Pose) # Connect ControlModule OUTPUT to AstarPlanner INPUT - triggers A* planning when goal received self.global_planner.target.connect(self.ctrl.plancmd) @@ -282,7 +286,7 @@ async def start(self): self.local_planner.start() self.global_planner.start() # self.foxglove_bridge.start() - # self.ctrl.start() # DEBUG + self.ctrl.start() # DEBUG await asyncio.sleep(2) print("querying system") From dc1e983a0e1a0853282b921d7003fce04f0b6915 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 15:46:57 -0700 Subject: [PATCH 13/36] tfmessage test review --- dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py | 67 +++++ .../msgs/tf2_msgs/test_publish_transforms.py | 262 ------------------ 2 files changed, 67 insertions(+), 262 deletions(-) create mode 100644 dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py delete mode 100644 dimos/msgs/tf2_msgs/test_publish_transforms.py 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..b6ab97f960 --- /dev/null +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -0,0 +1,67 @@ +# 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 numpy as np +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 +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 +def test_publish_transforms(): + 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, + ) + + # 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, + ) + + # Create TFMessage with all transforms + tf_msg = TFMessage(world_to_base, base_to_arm, arm_to_gripper) + + lcm.publish(topic, tf_msg) diff --git a/dimos/msgs/tf2_msgs/test_publish_transforms.py b/dimos/msgs/tf2_msgs/test_publish_transforms.py deleted file mode 100644 index d22fecd9c3..0000000000 --- a/dimos/msgs/tf2_msgs/test_publish_transforms.py +++ /dev/null @@ -1,262 +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. - -import time - -import numpy as np -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 -from dimos.protocol.pubsub.lcmpubsub import LCM, Topic - - -def test_publish_transforms_with_new_types(): - """Test publishing transforms using our new Transform and TFMessage types.""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_fancy", lcm_type=LCMTFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # 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(2.0, 3.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z - frame_id="world", - ts=current_time, - ) - - # 2. Base to arm transform (arm lifted up) - base_to_arm = Transform( - translation=Vector3(0.2, 0.0, 0.5), - rotation=Quaternion(0.0, 0.258819, 0.0, 0.965926), # 30 degrees around Y - frame_id="base_link", - ts=current_time, - ) - - # 3. Arm to gripper transform (gripper extended) - arm_to_gripper = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation - frame_id="arm_link", - ts=current_time, - ) - - # Create TFMessage with all transforms - tf_msg = TFMessage(world_to_base, base_to_arm, arm_to_gripper) - - # Encode to LCM bytes with proper child frame IDs - encoded = tf_msg.lcm_encode(child_frame_ids=["base_link", "arm_link", "gripper_link"]) - - # Decode back to LCM TFMessage and publish - lcm_msg = LCMTFMessage.lcm_decode(encoded) - lcm.publish(topic, lcm_msg) - - # Wait for reception - time.sleep(0.1) - - # Verify we received the message - assert len(received_messages) == 1 - received_msg, received_topic = received_messages[0] - - # Verify it's an LCM TFMessage - assert isinstance(received_msg, LCMTFMessage) - assert received_topic == topic - - # Verify content - assert received_msg.transforms_length == 3 - - # Check world to base transform - tf0 = received_msg.transforms[0] - assert tf0.header.frame_id == "world" - assert tf0.child_frame_id == "base_link" - assert np.isclose(tf0.transform.translation.x, 2.0, atol=1e-10) - assert np.isclose(tf0.transform.translation.y, 3.0, atol=1e-10) - assert np.isclose(tf0.transform.rotation.z, 0.382683, atol=1e-6) - assert np.isclose(tf0.transform.rotation.w, 0.923880, atol=1e-6) - - # Check base to arm transform - tf1 = received_msg.transforms[1] - assert tf1.header.frame_id == "base_link" - assert tf1.child_frame_id == "arm_link" - assert np.isclose(tf1.transform.translation.x, 0.2, atol=1e-10) - assert np.isclose(tf1.transform.translation.z, 0.5, atol=1e-10) - assert np.isclose(tf1.transform.rotation.y, 0.258819, atol=1e-6) - assert np.isclose(tf1.transform.rotation.w, 0.965926, atol=1e-6) - - # Check arm to gripper transform - tf2 = received_msg.transforms[2] - assert tf2.header.frame_id == "arm_link" - assert tf2.child_frame_id == "gripper_link" - assert np.isclose(tf2.transform.translation.x, 0.3, atol=1e-10) - assert tf2.transform.rotation.w == 1.0 - - # Verify timestamps are preserved - for tf in received_msg.transforms: - received_ts = tf.header.stamp.sec + (tf.header.stamp.nsec / 1_000_000_000) - assert abs(received_ts - current_time) < 1e-6 - - -def test_dynamic_robot_movement(): - """Test publishing dynamic transforms as robot moves.""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_movement", lcm_type=LCMTFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Simulate robot moving forward and turning - num_steps = 4 - for i in range(num_steps): - t = i * 0.1 # Time progression - - # Robot moves forward and turns - x = t * 2.0 # Move 2 m/s forward - y = 0.5 * np.sin(t * np.pi) # Slight sinusoidal motion - angle = t * (np.pi / 4) # Turn 45 degrees per second - - # Create transform for current robot pose - robot_pose = Transform( - translation=Vector3(x, y, 0.0), - rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), - frame_id="odom", - ts=time.time(), - ) - - # Robot has a sensor that's always 0.3m above base - sensor_transform = Transform( - translation=Vector3(0.0, 0.0, 0.3), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - ts=robot_pose.ts, # Same timestamp - ) - - # Create and publish TFMessage - tf_msg = TFMessage(robot_pose, sensor_transform) - encoded = tf_msg.lcm_encode(child_frame_ids=["base_link", "sensor_link"]) - - lcm_msg = LCMTFMessage.lcm_decode(encoded) - lcm.publish(topic, lcm_msg) - time.sleep(0.05) - - # Wait for all messages - time.sleep(0.1) - - # Should have received all updates - assert len(received_messages) == num_steps - - # Verify motion progression - for i, (msg, _) in enumerate(received_messages): - t = i * 0.1 - expected_x = t * 2.0 - expected_y = 0.5 * np.sin(t * np.pi) - - # Check robot transform - robot_tf = msg.transforms[0] - assert robot_tf.header.frame_id == "odom" - assert robot_tf.child_frame_id == "base_link" - assert np.isclose(robot_tf.transform.translation.x, expected_x, atol=1e-10) - assert np.isclose(robot_tf.transform.translation.y, expected_y, atol=1e-10) - - # Check sensor is always 0.3m above base - sensor_tf = msg.transforms[1] - assert sensor_tf.header.frame_id == "base_link" - assert sensor_tf.child_frame_id == "sensor_link" - assert np.isclose(sensor_tf.transform.translation.z, 0.3, atol=1e-10) - - -def test_roundtrip_transform_types(): - """Test that our types can roundtrip through LCM.""" - lcm = LCM(autoconf=True) - lcm.start() - - received_messages = [] - topic = Topic(topic="/tf_roundtrip", lcm_type=LCMTFMessage) - - def callback(msg, topic): - received_messages.append((msg, topic)) - - lcm.subscribe(topic, callback) - - # Create transforms with various configurations - tf1 = Transform( - translation=Vector3(1.5, -2.5, 3.5), - rotation=Quaternion(0.1, 0.2, 0.3, 0.9045), # Normalized quaternion - frame_id="sensor1", - ts=1234.5678, - ) - - tf2 = Transform( - translation=Vector3(-10, 20, -30), - rotation=Quaternion(0.5, 0.5, 0.5, 0.5), # All equal components - frame_id="sensor2", - ts=2345.6789, - ) - - # Create TFMessage - original = TFMessage(tf1, tf2) - - # Encode and publish - encoded = original.lcm_encode(child_frame_ids=["child1", "child2"]) - lcm_msg = LCMTFMessage.lcm_decode(encoded) - lcm.publish(topic, lcm_msg) - - time.sleep(0.1) - - # Decode received message back to our types - assert len(received_messages) == 1 - received_lcm_msg, _ = received_messages[0] - - # Decode back to our TFMessage - decoded = TFMessage.lcm_decode(received_lcm_msg.lcm_encode()) - - # Verify transforms match - assert len(decoded) == 2 - - # Check first transform - assert decoded[0].translation.x == 1.5 - assert decoded[0].translation.y == -2.5 - assert decoded[0].translation.z == 3.5 - assert np.isclose(decoded[0].rotation.x, 0.1, atol=1e-10) - assert np.isclose(decoded[0].rotation.y, 0.2, atol=1e-10) - assert np.isclose(decoded[0].rotation.z, 0.3, atol=1e-10) - assert np.isclose(decoded[0].rotation.w, 0.9045, atol=1e-10) - assert decoded[0].frame_id == "sensor1" - assert abs(decoded[0].ts - 1234.5678) < 1e-6 - - # Check second transform - assert decoded[1].translation.x == -10 - assert decoded[1].translation.y == 20 - assert decoded[1].translation.z == -30 - assert decoded[1].rotation.x == 0.5 - assert decoded[1].rotation.y == 0.5 - assert decoded[1].rotation.z == 0.5 - assert decoded[1].rotation.w == 0.5 - assert decoded[1].frame_id == "sensor2" - assert abs(decoded[1].ts - 2345.6789) < 1e-6 From bf25b0dff816dcdc3a264b4a5bbd90294ff4fbb8 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 19:54:19 -0700 Subject: [PATCH 14/36] decorators first pass, accumulators --- dimos/utils/decorators/__init__.py | 11 +++ dimos/utils/decorators/accumulators.py | 106 ++++++++++++++++++++++ dimos/utils/decorators/decorators.py | 102 +++++++++++++++++++++ dimos/utils/decorators/test_decorators.py | 79 ++++++++++++++++ 4 files changed, 298 insertions(+) create mode 100644 dimos/utils/decorators/__init__.py create mode 100644 dimos/utils/decorators/accumulators.py create mode 100644 dimos/utils/decorators/decorators.py create mode 100644 dimos/utils/decorators/test_decorators.py 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 From 844ec829ba021050e71f3993d91762afa4f9eeb9 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 20:06:00 -0700 Subject: [PATCH 15/36] lcmservice supports passing live lcm instance --- dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py | 33 +++++++++++++++++--- dimos/protocol/service/lcmservice.py | 14 ++++++--- 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py index b6ab97f960..6e3c5386c9 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -13,14 +13,39 @@ # limitations under the License. import time +from dataclasses import dataclass import numpy as np import pytest +import tf_lcm_py from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage +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.service import LCMConfig, LCMService, Service + + +@dataclass +class TFConfig[LCMConfig]: + topic: str = ("/tf",) + buffer_size: float = 10.0 # seconds + + +class TFService(LCMService[TFConfig]): + default_config = TFConfig + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + self.buffer = tf_lcm_py.Buffer(self.config.buffer_size) + + self.listener = tf_lcm_py.TransformListener(self.l, self.buffer) + self.broadcaster = tf_lcm_py.TransformBroadcaster() + self.static_broadcaster = tf_lcm_py.StaticTransformBroadcaster() + + def start(self): + super().start() # Publishes a series of transforms representing a robot kinematic chain @@ -52,6 +77,9 @@ def test_publish_transforms(): 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), @@ -61,7 +89,4 @@ def test_publish_transforms(): ts=current_time, ) - # Create TFMessage with all transforms - tf_msg = TFMessage(world_to_base, base_to_arm, arm_to_gripper) - - lcm.publish(topic, tf_msg) + 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 From 30138b21288e009c8d91a65b81b1f31f8ecd8679 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 20:55:12 -0700 Subject: [PATCH 16/36] tf module added --- dimos/msgs/geometry_msgs/Transform.py | 12 ++ dimos/msgs/tf2_msgs/TFMessage.py | 33 ++---- dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py | 23 ---- dimos/robot/module/test_tf.py | 78 +++++++++++++ dimos/robot/module/tf.py | 110 +++++++++++++++++++ 5 files changed, 208 insertions(+), 48 deletions(-) create mode 100644 dimos/robot/module/test_tf.py create mode 100644 dimos/robot/module/tf.py diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index f0b5d47e2f..267575dcb5 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -25,6 +25,7 @@ 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 @@ -68,7 +69,18 @@ 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() diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index ccbbd4e3c2..ddd9ee2b29 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -59,33 +59,16 @@ def lcm_encode(self) -> bytes: child_frame_ids: Optional list of child frame IDs for each transform. If not provided, defaults to "base_link" for all. """ - # Create LCM TransformStamped objects - lcm_transforms = [] - for i, transform in enumerate(self.transforms): - # Create TransformStamped - transform_stamped = LCMTransformStamped( - child_frame_id=transform.child_frame_id, - ) - - # Build header - sec = int(transform.ts) - nsec = int((transform.ts - sec) * 1_000_000_000) - transform_stamped.header = LCMHeader( - seq=1, - stamp=LCMTime(sec=sec, nsec=nsec), - frame_id=transform.frame_id, - ) - - # Build transform - transform_stamped.transform = LCMTransform( - translation=transform.translation, rotation=transform.rotation - ) - - lcm_transforms.append(transform_stamped) + print("WILL MAP", self.transforms) + res = list(map(lambda t: t.lcm_transform(), self.transforms)) + print("RES IS", res) - # Create LCM TFMessage - lcm_msg = LCMTFMessage(transforms_length=len(lcm_transforms), transforms=lcm_transforms) + print("HEADER", res[0].header) + lcm_msg = LCMTFMessage( + transforms_length=len(self.transforms), + transforms=res, + ) return lcm_msg.lcm_encode() diff --git a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py index 6e3c5386c9..c88c3a7ea8 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -20,32 +20,9 @@ import tf_lcm_py from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage -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.service import LCMConfig, LCMService, Service - - -@dataclass -class TFConfig[LCMConfig]: - topic: str = ("/tf",) - buffer_size: float = 10.0 # seconds - - -class TFService(LCMService[TFConfig]): - default_config = TFConfig - - def __init__(self, **kwargs) -> None: - super().__init__(**kwargs) - self.buffer = tf_lcm_py.Buffer(self.config.buffer_size) - - self.listener = tf_lcm_py.TransformListener(self.l, self.buffer) - self.broadcaster = tf_lcm_py.TransformBroadcaster() - self.static_broadcaster = tf_lcm_py.StaticTransformBroadcaster() - - def start(self): - super().start() # Publishes a series of transforms representing a robot kinematic chain diff --git a/dimos/robot/module/test_tf.py b/dimos/robot/module/test_tf.py new file mode 100644 index 0000000000..175eb0ddbe --- /dev/null +++ b/dimos/robot/module/test_tf.py @@ -0,0 +1,78 @@ +#!/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 Quaternion, Transform, Vector3 +from dimos.robot.module.tf import TF, TFConfig + + +def test_tf_broadcast_and_query(): + """Test TF broadcasting and querying between two TF instances.""" + + 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, + ) + + 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) + + # Stop services + broadcaster.stop() + querier.stop() + + +if __name__ == "__main__": + test_tf_broadcast_and_query() + print("Test passed!") diff --git a/dimos/robot/module/tf.py b/dimos/robot/module/tf.py new file mode 100644 index 0000000000..8fce802acd --- /dev/null +++ b/dimos/robot/module/tf.py @@ -0,0 +1,110 @@ +#!/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 dataclasses import dataclass +from datetime import datetime +from typing import Optional + +import dimos_lcm +import numpy as np +import pytest +import tf_lcm_py as tf + +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.service.lcmservice import LCMConfig, LCMService, Service + +print("TF LIB", tf) + + +@dataclass +class TFConfig: + topic: str = "/tf" + buffer_size: float = 10.0 # seconds + rate_limit: float = 10.0 # Hz + autostart: bool = True + + +class TF(Service[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) + 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], + time_tolerance: Optional[float] = None, + ): + self.buffer.lookup_transform( + parent_frame, + child_frame, + time_point or time.time(), + lcm_module=dimos_lcm, # a remote exploit here? + ) + + 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): ... From 5c941aede62f7624aaf97fffb4ff63a9e9f1d518 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 20:55:19 -0700 Subject: [PATCH 17/36] nix env changes for lcm_tf build --- flake.nix | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/flake.nix b/flake.nix index fe6749797e..5308db40ba 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,17 @@ 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 PKG_CONFIG_PATH="${pkgs.lib.makeSearchPathOutput "dev" "lib/pkgconfig" [ + pkgs.lcm pkgs.glib pkgs.gtk3 pkgs.cairo pkgs.pango pkgs.gdk-pixbuf + pkgs.libGL pkgs.libGLU pkgs.mesa pkgs.glfw pkgs.xorg.libX11 + pkgs.xorg.libXi pkgs.xorg.libXext pkgs.xorg.libXrandr + pkgs.xorg.libXinerama pkgs.xorg.libXcursor pkgs.xorg.libXfixes + pkgs.xorg.libXrender pkgs.xorg.libXdamage pkgs.xorg.libXcomposite + pkgs.xorg.libxcb pkgs.xorg.libXScrnSaver pkgs.xorg.libXxf86vm + pkgs.SDL2 pkgs.zlib pkgs.portaudio pkgs.ffmpeg_6 + ]}:$PKG_CONFIG_PATH" export DISPLAY=:0 From 52d29f8683caa52117759a87768278c2da8d4ef9 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 23:06:24 -0700 Subject: [PATCH 18/36] dimos-lcm bump, dimos-tf added --- dimos/robot/module/tf.py | 8 ++++---- pyproject.toml | 3 ++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/dimos/robot/module/tf.py b/dimos/robot/module/tf.py index 8fce802acd..0428b1920a 100644 --- a/dimos/robot/module/tf.py +++ b/dimos/robot/module/tf.py @@ -79,14 +79,14 @@ def lookup( self, parent_frame: str, child_frame: str, - time_point: Optional[float], + time_point: Optional[float] = None, time_tolerance: Optional[float] = None, ): - self.buffer.lookup_transform( + return self.buffer.lookup_transform( parent_frame, child_frame, - time_point or time.time(), - lcm_module=dimos_lcm, # a remote exploit here? + datetime.now(), + lcm_module=dimos_lcm, ) def can_transform( diff --git a/pyproject.toml b/pyproject.toml index 49b4c34d2e..694d552f0c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,8 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@1c974457d5636fc2db1005c91d9e44bcb38b3c21" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36", + "dimos-tf @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] From 39b178169f9537f6cfcacacc0eb00ca971c6f548 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 19 Jul 2025 23:46:48 -0700 Subject: [PATCH 19/36] correct package ref --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 694d552f0c..fdd1fbe786 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -94,7 +94,7 @@ dependencies = [ # LCM / DimOS utilities "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36", - "dimos-tf @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" + "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] From 5b4699ff456204981785920bb3fdf435a96423ba Mon Sep 17 00:00:00 2001 From: lesh Date: Sun, 20 Jul 2025 00:26:39 -0700 Subject: [PATCH 20/36] added lcm to dev image --- docker/dev/Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index 514f0b01c6..fb48ebd7f2 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -18,7 +18,8 @@ RUN apt-get update && apt-get install -y \ net-tools \ sudo \ iproute2 # for LCM networking system config \ - pre-commit + pre-commit \ + lcm # Configure git to trust any directory (resolves dubious ownership issues in containers) From c12a14f0b37af25c913eb7ab1a8b5937144f037a Mon Sep 17 00:00:00 2001 From: lesh Date: Sun, 20 Jul 2025 13:22:53 +0300 Subject: [PATCH 21/36] added liblcm-bin --- docker/dev/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index fb48ebd7f2..46d5c42910 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -19,7 +19,7 @@ RUN apt-get update && apt-get install -y \ sudo \ iproute2 # for LCM networking system config \ pre-commit \ - lcm + liblcm-bin # Configure git to trust any directory (resolves dubious ownership issues in containers) From b8ea216328fd8481a1ff72fba9890c8c32e1237d Mon Sep 17 00:00:00 2001 From: lesh Date: Sun, 20 Jul 2025 11:13:58 -0700 Subject: [PATCH 22/36] time conversion functions --- dimos/types/test_timestamped.py | 74 ++++++++++++++++++++++++++++++++- dimos/types/timestamped.py | 37 +++++++++++++++-- 2 files changed, 106 insertions(+), 5 deletions(-) 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 From cbe9d61d71787ae036fa1aab98e104cc41bbcb87 Mon Sep 17 00:00:00 2001 From: lesh Date: Mon, 21 Jul 2025 21:17:46 -0700 Subject: [PATCH 23/36] tf push --- dimos/robot/module/test_tf.py | 15 ++++++++-- dimos/robot/module/tf.py | 56 +++++++++++++++++++++++++++++++++-- 2 files changed, 66 insertions(+), 5 deletions(-) diff --git a/dimos/robot/module/test_tf.py b/dimos/robot/module/test_tf.py index 175eb0ddbe..4f6cdd8f12 100644 --- a/dimos/robot/module/test_tf.py +++ b/dimos/robot/module/test_tf.py @@ -19,18 +19,20 @@ import pytest import lcm -from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 from dimos.robot.module.tf import TF, TFConfig def test_tf_broadcast_and_query(): - """Test TF broadcasting and querying between two TF instances.""" + """Test TF broadcasting and querying between two TF instances. + If you run foxglove-bridge this will show up in the UI""" 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 @@ -62,12 +64,21 @@ def test_tf_broadcast_and_query(): 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 index 0428b1920a..7dbed95b73 100644 --- a/dimos/robot/module/tf.py +++ b/dimos/robot/module/tf.py @@ -17,20 +17,39 @@ import time from dataclasses import dataclass from datetime import datetime -from typing import Optional +from typing import Optional, TypeVar import dimos_lcm import numpy as np import pytest import tf_lcm_py as tf +from ABC import abstractmethod 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 -print("TF LIB", tf) +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 @@ -41,7 +60,38 @@ class TFConfig: autostart: bool = True -class TF(Service[TFConfig]): +@dataclass +class GenericTFConfig(TFConfig): + pubsub: PubSub + + +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(): ... + + +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. From 79a0663d0ef175a1e9fd41ac3fdf977529a6c39d Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 15:45:05 -0700 Subject: [PATCH 24/36] disabling tf_lcm_py import --- dimos/robot/module/test_tf.py | 6 +----- dimos/robot/module/tf.py | 3 ++- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/dimos/robot/module/test_tf.py b/dimos/robot/module/test_tf.py index 4f6cdd8f12..400724c15f 100644 --- a/dimos/robot/module/test_tf.py +++ b/dimos/robot/module/test_tf.py @@ -23,6 +23,7 @@ from dimos.robot.module.tf import TF, TFConfig +@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""" @@ -82,8 +83,3 @@ def test_tf_broadcast_and_query(): # Stop services broadcaster.stop() querier.stop() - - -if __name__ == "__main__": - test_tf_broadcast_and_query() - print("Test passed!") diff --git a/dimos/robot/module/tf.py b/dimos/robot/module/tf.py index 7dbed95b73..1d690f0996 100644 --- a/dimos/robot/module/tf.py +++ b/dimos/robot/module/tf.py @@ -22,7 +22,8 @@ import dimos_lcm import numpy as np import pytest -import tf_lcm_py as tf + +# import tf_lcm_py as tf from ABC import abstractmethod import lcm From 5841406ae7e8d1a3447a74f83a955bf34e00acef Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 16:11:50 -0700 Subject: [PATCH 25/36] removed lcm_tf_py for now --- dimos/robot/module/test_tf.py | 2 +- dimos/robot/module/tf.py | 12 +++++++----- docker/python/Dockerfile | 5 +++-- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/dimos/robot/module/test_tf.py b/dimos/robot/module/test_tf.py index 400724c15f..7b71a33a28 100644 --- a/dimos/robot/module/test_tf.py +++ b/dimos/robot/module/test_tf.py @@ -20,13 +20,13 @@ import lcm from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 -from dimos.robot.module.tf import TF, TFConfig @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() diff --git a/dimos/robot/module/tf.py b/dimos/robot/module/tf.py index 1d690f0996..71aee4c8c2 100644 --- a/dimos/robot/module/tf.py +++ b/dimos/robot/module/tf.py @@ -15,6 +15,7 @@ # limitations under the License. import time +from abc import abstractmethod from dataclasses import dataclass from datetime import datetime from typing import Optional, TypeVar @@ -23,9 +24,6 @@ import numpy as np import pytest -# import tf_lcm_py as tf -from ABC import abstractmethod - import lcm from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.tf2_msgs import TFMessage @@ -54,7 +52,7 @@ def lookup( @dataclass -class TFConfig: +class TFConfig(LCMConfig): topic: str = "/tf" buffer_size: float = 10.0 # seconds rate_limit: float = 10.0 # Hz @@ -63,7 +61,7 @@ class TFConfig: @dataclass class GenericTFConfig(TFConfig): - pubsub: PubSub + pubsub: PubSub = None class GenericTF(TFSpec[TFConfig]): @@ -92,6 +90,7 @@ def lookup( 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 @@ -109,6 +108,9 @@ class TFLCM(LCMService, TFSpec[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) diff --git a/docker/python/Dockerfile b/docker/python/Dockerfile index f8d06496b4..c4f93cfa13 100644 --- a/docker/python/Dockerfile +++ b/docker/python/Dockerfile @@ -29,7 +29,8 @@ RUN apt-get install -y \ qtchooser \ qt5-qmake \ qtbase5-dev-tools \ - supervisor + supervisor \ + liblcm-dev # Fix distutils-installed packages that block pip upgrades RUN apt-get purge -y python3-blinker python3-sympy python3-oauthlib || true @@ -40,4 +41,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]'" From 574200e1de602511d7dc16e9e00f53eb1b8209b7 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 16:14:33 -0700 Subject: [PATCH 26/36] temporarily disabled foxglove-bridge --- dimos/utils/test_foxglove_bridge.py | 4 ++++ pyproject.toml | 8 ++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/dimos/utils/test_foxglove_bridge.py b/dimos/utils/test_foxglove_bridge.py index ecedb90573..ae87f64aa7 100644 --- a/dimos/utils/test_foxglove_bridge.py +++ b/dimos/utils/test_foxglove_bridge.py @@ -27,6 +27,7 @@ warnings.filterwarnings("ignore", category=DeprecationWarning, module="websockets.legacy") +@pytest.mark.tofix def test_foxglove_bridge_import(): """Test that the foxglove bridge can be imported successfully.""" try: @@ -37,6 +38,7 @@ def test_foxglove_bridge_import(): pytest.fail(f"Failed to import foxglove bridge: {e}") +@pytest.mark.tofix def test_foxglove_bridge_runner_init(): """Test that LcmFoxgloveBridgeRunner can be initialized with default parameters.""" try: @@ -53,6 +55,7 @@ def test_foxglove_bridge_runner_init(): pytest.fail(f"Failed to initialize LcmFoxgloveBridgeRunner: {e}") +@pytest.mark.tofix def test_foxglove_bridge_runner_params(): """Test that LcmFoxgloveBridgeRunner accepts various parameter configurations.""" try: @@ -72,6 +75,7 @@ def test_foxglove_bridge_runner_params(): pytest.fail(f"Failed to create runner with different configs: {e}") +@pytest.mark.tofix def test_bridge_runner_has_run_method(): """Test that the bridge runner has a run method that can be called.""" try: diff --git a/pyproject.toml b/pyproject.toml index fdd1fbe786..1757aaa479 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -94,7 +94,7 @@ dependencies = [ # LCM / DimOS utilities "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36", - "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" + #"tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] @@ -193,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 -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'" +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'" From c64a136913efc103d76d563da418aa0fd491374e Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 16:25:56 -0700 Subject: [PATCH 27/36] dimos-lcm upgrade --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 1757aaa479..30acdd85f8 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@9befcc1a75f43fb1110d468c182dd19a549eaa36", - #"tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@84fce51e575281bc35f4a094064b7be2ad4901d7" + # "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] From 0c71a0968007fb051e03c64f293f163743063222 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 16:42:54 -0700 Subject: [PATCH 28/36] removed tf_lcm tests --- dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py | 6 ++++-- dimos/utils/test_foxglove_bridge.py | 9 +++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py index c88c3a7ea8..8eefdb3aee 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -17,8 +17,6 @@ import numpy as np import pytest -import tf_lcm_py -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 @@ -27,7 +25,11 @@ # 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() diff --git a/dimos/utils/test_foxglove_bridge.py b/dimos/utils/test_foxglove_bridge.py index ae87f64aa7..6d6632176f 100644 --- a/dimos/utils/test_foxglove_bridge.py +++ b/dimos/utils/test_foxglove_bridge.py @@ -17,17 +17,17 @@ 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") -@pytest.mark.tofix def test_foxglove_bridge_import(): """Test that the foxglove bridge can be imported successfully.""" try: @@ -38,7 +38,6 @@ def test_foxglove_bridge_import(): pytest.fail(f"Failed to import foxglove bridge: {e}") -@pytest.mark.tofix def test_foxglove_bridge_runner_init(): """Test that LcmFoxgloveBridgeRunner can be initialized with default parameters.""" try: @@ -55,7 +54,6 @@ def test_foxglove_bridge_runner_init(): pytest.fail(f"Failed to initialize LcmFoxgloveBridgeRunner: {e}") -@pytest.mark.tofix def test_foxglove_bridge_runner_params(): """Test that LcmFoxgloveBridgeRunner accepts various parameter configurations.""" try: @@ -75,7 +73,6 @@ def test_foxglove_bridge_runner_params(): pytest.fail(f"Failed to create runner with different configs: {e}") -@pytest.mark.tofix def test_bridge_runner_has_run_method(): """Test that the bridge runner has a run method that can be called.""" try: From 9e79ad4767439f49c6a0d57ecc9048c4afbac7a2 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 17:05:26 -0700 Subject: [PATCH 29/36] fixed foxglove bridge tests --- dimos/utils/test_foxglove_bridge.py | 26 ++++++++++---------------- pyproject.toml | 2 +- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/dimos/utils/test_foxglove_bridge.py b/dimos/utils/test_foxglove_bridge.py index 6d6632176f..b845622d88 100644 --- a/dimos/utils/test_foxglove_bridge.py +++ b/dimos/utils/test_foxglove_bridge.py @@ -31,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}, @@ -66,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: @@ -76,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/pyproject.toml b/pyproject.toml index 30acdd85f8..cb4e9d5dfe 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@84fce51e575281bc35f4a094064b7be2ad4901d7" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@9985fe4c6116dc34b3bcebe91ad1e3439a433298" # "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] From 27c0cc2419552d58ac9eac05e816b0a00ca13969 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 17:08:46 -0700 Subject: [PATCH 30/36] foxglove bridge class name fix --- dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py b/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py index c690516a18..bbcb70faee 100644 --- a/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py +++ b/dimos/utils/cli/foxglove_bridge/run_foxglove_bridge.py @@ -22,7 +22,7 @@ import threading import dimos_lcm -from dimos_lcm.foxglove_bridge import LcmFoxgloveBridge +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}") @@ -36,9 +36,7 @@ def bridge_thread(): loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) try: - bridge_instance = LcmFoxgloveBridge( - 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_instance.run()) except Exception as e: From fb54e22b7638446c31d8e0d8a2d5140bec93293e Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 17:17:03 -0700 Subject: [PATCH 31/36] foxglove bridge module fix --- dimos/robot/foxglove_bridge.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/robot/foxglove_bridge.py b/dimos/robot/foxglove_bridge.py index f5f0a0fbb6..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 -from dimos_lcm.foxglove_bridge import LcmFoxgloveBridge +from dimos_lcm.foxglove_bridge import FoxgloveBridge as LCMFoxgloveBridge from dimos.core import Module, rpc @@ -35,8 +35,7 @@ def run_bridge(): self._loop = asyncio.new_event_loop() asyncio.set_event_loop(self._loop) try: - bridge = LcmFoxgloveBridge(host="0.0.0.0", port=8765, debug=False, num_threads=4) - + 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}") From c1aa8d71f4915a36816e2ec02dfcfa62f93fe0f7 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 18:34:33 -0700 Subject: [PATCH 32/36] upgrade dimos_lcm --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index cb4e9d5dfe..c8eab3edc9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@9985fe4c6116dc34b3bcebe91ad1e3439a433298" + "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@34f8fa016e31269912a8032ed4ae0ca71b94003b" # "tf-lcm-py @ git+https://github.com/dimensionalOS/dimos-lcm.git@9befcc1a75f43fb1110d468c182dd19a549eaa36#subdirectory=tf_lcm/python" ] From 1cab6cb62311b6abe828bb9551fbbd015cfb6499 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 18:42:20 -0700 Subject: [PATCH 33/36] removed go2 changes --- .../multiprocess/unitree_go2.py | 57 +++++++------------ 1 file changed, 21 insertions(+), 36 deletions(-) diff --git a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py index c8867510dd..aa66291e68 100644 --- a/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py @@ -27,16 +27,15 @@ import dimos.core.colors as colors from dimos import core from dimos.core import In, Module, Out, rpc -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Transform, Twist, Vector3 +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.msgs.tf2_msgs import TFMessage from dimos.protocol import pubsub from dimos.robot.foxglove_bridge import FoxgloveBridge from dimos.robot.frontier_exploration.wavefront_frontier_goal_selector import ( WavefrontFrontierExplorer, ) from dimos.robot.global_planner import AstarPlanner -from dimos.robot.local_planner.simple import SimplePlanner from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection, VideoMessage from dimos.robot.unitree_webrtc.type.lidar import LidarMessage @@ -83,16 +82,19 @@ def liedown(self): @functools.cache def lidar_stream(self): + print("lidar stream start") lidar_store = TimedSensorReplay("unitree_office_walk/lidar", autocast=LidarMessage.from_msg) return lidar_store.stream() @functools.cache def odom_stream(self): + print("odom stream start") odom_store = TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) return odom_store.stream() @functools.cache def video_stream(self, freq_hz=0.5): + print("video stream start") video_store = TimedSensorReplay("unitree_office_walk/video", autocast=Image.from_numpy) return video_store.stream().pipe(ops.sample(freq_hz)) @@ -105,7 +107,6 @@ class ConnectionModule(UnitreeWebRTCConnection, Module): odom: Out[Vector3] = None lidar: Out[LidarMessage] = None video: Out[VideoMessage] = None - tf: Out[Transform] = None ip: str _odom: Callable[[], Odometry] @@ -119,16 +120,6 @@ def __init__(self, ip: str, *args, **kwargs): self.ip = ip Module.__init__(self, *args, **kwargs) - def _odom_to_tf(self, odom: Odometry) -> Transform: - """Convert Odometry to Transform.""" - return Transform( - translation=odom.position, - rotation=odom.orientation, - frame_id="world", - child_frame_id="base_link", - ts=odom.ts, - ) - @rpc def start(self): # Initialize the parent WebRTC connection @@ -139,8 +130,6 @@ def start(self): self.odom_stream().subscribe(self.odom.publish) self.video_stream().subscribe(self.video.publish) - # self.odom_stream().pipe(ops.map(self._odom_to_tf)).subscribe(self.tf.publish) - # Connect LCM input to robot movement commands self.movecmd.subscribe(self.move) @@ -169,8 +158,7 @@ def start(self): def plancmd(): time.sleep(4) print(colors.red("requesting global plan")) - # self.plancmd.publish(Pose(0, 0, 0, 0, 0, 0, 1)) - self.plancmd.publish(Pose(4.0, 4.0, 0, 0, 0, 0, 1)) + self.plancmd.publish(Pose(0, 0, 0, 0, 0, 0, 1)) thread = threading.Thread(target=plancmd, daemon=True) thread.start() @@ -217,26 +205,11 @@ async def start(self): self.mapper.lidar.connect(self.connection.lidar) # ==================================================================== - # Global Planner Module =============== - self.global_planner = self.dimos.deploy( - AstarPlanner, - get_costmap=self.mapper.costmap, - get_robot_pos=self.connection.get_pos, - # set_local_nav=self.local_planner.navigate_path_local, - ) - - # Configure AstarPlanner OUTPUT path: Out[Path] to /global_path LCM topic - self.global_planner.path.transport = core.pLCMTransport("/global_path") - # ====================================== - # Local planner Module, LCM transport & connection ================ self.local_planner = self.dimos.deploy( - SimplePlanner, + VFHPurePursuitPlanner, get_costmap=self.connection.get_local_costmap, ) - self.local_planner.tf.transport = core.LCMTransport("/tf", TFMessage) - - self.local_planner.path.connect(self.global_planner.path) # Connects odometry LCM stream to BaseLocalPlanner odometry input self.local_planner.odom.connect(self.connection.odom) @@ -248,12 +221,24 @@ async def start(self): self.connection.movecmd.connect(self.local_planner.movecmd) # =================================================================== + # Global Planner Module =============== + self.global_planner = self.dimos.deploy( + AstarPlanner, + get_costmap=self.mapper.costmap, + get_robot_pos=self.connection.get_pos, + set_local_nav=self.local_planner.navigate_path_local, + ) + + # Configure AstarPlanner OUTPUT path: Out[Path] to /global_path LCM topic + self.global_planner.path.transport = core.pLCMTransport("/global_path") + # ====================================== + # Global Planner Control Module =========================== # Debug module that sends (0,0,0) goal after 4 second delay self.ctrl = self.dimos.deploy(ControlModule) # Configure ControlModule OUTPUT to publish goal coordinates to /global_target - self.ctrl.plancmd.transport = core.LCMTransport("/global_target", Pose) + self.ctrl.plancmd.transport = core.LCMTransport("/global_target", Vector3) # Connect ControlModule OUTPUT to AstarPlanner INPUT - triggers A* planning when goal received self.global_planner.target.connect(self.ctrl.plancmd) @@ -285,8 +270,8 @@ async def start(self): self.connection.start() self.local_planner.start() self.global_planner.start() - # self.ctrl.start() # DEBUG self.foxglove_bridge.start() + # self.ctrl.start() # DEBUG await asyncio.sleep(2) print("querying system") From 897ad58b4a8e7dcd0640f592f43d318e9ad680d4 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 18:44:53 -0700 Subject: [PATCH 34/36] dockerfile cleanup --- docker/dev/Dockerfile | 5 +---- docker/python/Dockerfile | 2 ++ 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index 46d5c42910..ad81abdbbd 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -12,14 +12,11 @@ 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 \ - liblcm-bin + pre-commit # Configure git to trust any directory (resolves dubious ownership issues in containers) diff --git a/docker/python/Dockerfile b/docker/python/Dockerfile index c4f93cfa13..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 \ @@ -30,6 +31,7 @@ RUN apt-get install -y \ qt5-qmake \ qtbase5-dev-tools \ supervisor \ + iproute2 # for LCM networking system config \ liblcm-dev # Fix distutils-installed packages that block pip upgrades From c59d1b917bd0f9cccb36c439399bfeed44e42a42 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 18:45:15 -0700 Subject: [PATCH 35/36] flake cleanup --- flake.nix | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/flake.nix b/flake.nix index 5308db40ba..fbbf45591d 100644 --- a/flake.nix +++ b/flake.nix @@ -59,16 +59,6 @@ pkgs.udev pkgs.portaudio pkgs.SDL2.dev pkgs.zlib pkgs.glib pkgs.gtk3 pkgs.gdk-pixbuf pkgs.gobject-introspection pkgs.lcm]}:$LD_LIBRARY_PATH" - export PKG_CONFIG_PATH="${pkgs.lib.makeSearchPathOutput "dev" "lib/pkgconfig" [ - pkgs.lcm pkgs.glib pkgs.gtk3 pkgs.cairo pkgs.pango pkgs.gdk-pixbuf - pkgs.libGL pkgs.libGLU pkgs.mesa pkgs.glfw pkgs.xorg.libX11 - pkgs.xorg.libXi pkgs.xorg.libXext pkgs.xorg.libXrandr - pkgs.xorg.libXinerama pkgs.xorg.libXcursor pkgs.xorg.libXfixes - pkgs.xorg.libXrender pkgs.xorg.libXdamage pkgs.xorg.libXcomposite - pkgs.xorg.libxcb pkgs.xorg.libXScrnSaver pkgs.xorg.libXxf86vm - pkgs.SDL2 pkgs.zlib pkgs.portaudio pkgs.ffmpeg_6 - ]}:$PKG_CONFIG_PATH" - export DISPLAY=:0 PROJECT_ROOT=$(git rev-parse --show-toplevel 2>/dev/null || echo "$PWD") From 08ffef13121b7cc4cdae4484ba7e852e66a120f9 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 22 Jul 2025 19:18:29 -0700 Subject: [PATCH 36/36] dimos_lcm upgrade to fix foxglove bridge --- dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py | 1 - pyproject.toml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) 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/pyproject.toml b/pyproject.toml index c8eab3edc9..39350d489b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -93,7 +93,7 @@ dependencies = [ "dask[complete]==2025.5.1", # LCM / DimOS utilities - "dimos-lcm @ git+https://github.com/dimensionalOS/dimos-lcm.git@34f8fa016e31269912a8032ed4ae0ca71b94003b" + "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" ]