From 19c5d0946b762621d8dc95ad207f10f0e6cdcbf0 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 7 Sep 2025 11:45:13 -0700 Subject: [PATCH 01/21] added and tested ROS to DIMOS bridge --- dimos/msgs/geometry_msgs/Pose.py | 45 +- dimos/msgs/geometry_msgs/PoseStamped.py | 42 ++ dimos/msgs/geometry_msgs/Transform.py | 66 ++ dimos/msgs/geometry_msgs/Twist.py | 27 + dimos/msgs/geometry_msgs/test_Pose.py | 52 ++ dimos/msgs/geometry_msgs/test_PoseStamped.py | 76 +++ dimos/msgs/geometry_msgs/test_Transform.py | 81 +++ dimos/msgs/geometry_msgs/test_Twist.py | 91 +++ dimos/msgs/nav_msgs/Path.py | 40 ++ dimos/msgs/nav_msgs/test_Path.py | 95 +++ dimos/msgs/tf2_msgs/TFMessage.py | 33 + dimos/msgs/tf2_msgs/test_TFMessage.py | 151 +++++ dimos/robot/ros_bridge.py | 173 +++++ dimos/robot/test_ros_bridge.py | 656 +++++++++++++++++++ 14 files changed, 1624 insertions(+), 4 deletions(-) create mode 100644 dimos/robot/ros_bridge.py create mode 100644 dimos/robot/test_ros_bridge.py diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index 0706a144f6..55ecaeca9e 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -14,13 +14,13 @@ from __future__ import annotations -import struct -import traceback -from io import BytesIO -from typing import BinaryIO, TypeAlias +from typing import TypeAlias from dimos_lcm.geometry_msgs import Pose as LCMPose from dimos_lcm.geometry_msgs import Transform as LCMTransform +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion from plum import dispatch from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable @@ -207,6 +207,43 @@ def __add__(self, other: "Pose" | PoseConvertable | LCMTransform | Transform) -> return Pose(new_position, new_orientation) + @classmethod + def from_ros_msg(cls, ros_msg: ROSPose) -> "Pose": + """Create a Pose from a ROS geometry_msgs/Pose message. + + Args: + ros_msg: ROS Pose message + + Returns: + Pose instance + """ + position = Vector3(ros_msg.position.x, ros_msg.position.y, ros_msg.position.z) + orientation = Quaternion( + ros_msg.orientation.x, + ros_msg.orientation.y, + ros_msg.orientation.z, + ros_msg.orientation.w, + ) + return cls(position, orientation) + + def to_ros_msg(self) -> ROSPose: + """Convert to a ROS geometry_msgs/Pose message. + + Returns: + ROS Pose message + """ + ros_msg = ROSPose() + ros_msg.position = ROSPoint( + x=float(self.position.x), y=float(self.position.y), z=float(self.position.z) + ) + ros_msg.orientation = ROSQuaternion( + x=float(self.orientation.x), + y=float(self.orientation.y), + z=float(self.orientation.z), + w=float(self.orientation.w), + ) + return ros_msg + @dispatch def to_pose(value: "Pose") -> "Pose": diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py index ea1198818d..2927247d89 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -22,6 +22,7 @@ from dimos_lcm.geometry_msgs import PoseStamped as LCMPoseStamped from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime +from geometry_msgs.msg import PoseStamped as ROSPoseStamped from plum import dispatch from dimos.msgs.geometry_msgs.Pose import Pose @@ -109,3 +110,44 @@ def find_transform(self, other: PoseStamped) -> Transform: translation=local_translation, rotation=relative_rotation, ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSPoseStamped) -> "PoseStamped": + """Create a PoseStamped from a ROS geometry_msgs/PoseStamped message. + + Args: + ros_msg: ROS PoseStamped message + + Returns: + PoseStamped instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert pose + pose = Pose.from_ros_msg(ros_msg.pose) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + position=pose.position, + orientation=pose.orientation, + ) + + def to_ros_msg(self) -> ROSPoseStamped: + """Convert to a ROS geometry_msgs/PoseStamped message. + + Returns: + ROS PoseStamped message + """ + ros_msg = ROSPoseStamped() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set pose + ros_msg.pose = Pose.to_ros_msg(self) + + return ros_msg diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index a47c58337c..1f6121f6cf 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -19,6 +19,10 @@ from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped +from geometry_msgs.msg import TransformStamped as ROSTransformStamped +from geometry_msgs.msg import Transform as ROSTransform +from geometry_msgs.msg import Vector3 as ROSVector3 +from geometry_msgs.msg import Quaternion as ROSQuaternion from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -137,6 +141,68 @@ def inverse(self) -> "Transform": ts=self.ts, ) + @classmethod + def from_ros_transform_stamped(cls, ros_msg: ROSTransformStamped) -> "Transform": + """Create a Transform from a ROS geometry_msgs/TransformStamped message. + + Args: + ros_msg: ROS TransformStamped message + + Returns: + Transform instance + """ + # Convert timestamp + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert translation + translation = Vector3( + ros_msg.transform.translation.x, + ros_msg.transform.translation.y, + ros_msg.transform.translation.z, + ) + + # Convert rotation + rotation = Quaternion( + ros_msg.transform.rotation.x, + ros_msg.transform.rotation.y, + ros_msg.transform.rotation.z, + ros_msg.transform.rotation.w, + ) + + return cls( + translation=translation, + rotation=rotation, + frame_id=ros_msg.header.frame_id, + child_frame_id=ros_msg.child_frame_id, + ts=ts, + ) + + def to_ros_transform_stamped(self) -> ROSTransformStamped: + """Convert to a ROS geometry_msgs/TransformStamped message. + + Returns: + ROS TransformStamped message + """ + ros_msg = ROSTransformStamped() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set child frame + ros_msg.child_frame_id = self.child_frame_id + + # Set transform + ros_msg.transform.translation = ROSVector3( + x=self.translation.x, y=self.translation.y, z=self.translation.z + ) + ros_msg.transform.rotation = ROSQuaternion( + x=self.rotation.x, y=self.rotation.y, z=self.rotation.z, w=self.rotation.w + ) + + return ros_msg + def __neg__(self) -> "Transform": """Unary minus operator returns the inverse transform.""" return self.inverse() diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py index 10e07eaeb5..fe951bff09 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -19,6 +19,8 @@ from typing import BinaryIO from dimos_lcm.geometry_msgs import Twist as LCMTwist +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Vector3 as ROSVector3 from plum import dispatch from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -100,3 +102,28 @@ def __bool__(self) -> bool: False if twist is zero, True otherwise """ return not self.is_zero() + + @classmethod + def from_ros_msg(cls, ros_msg: ROSTwist) -> "Twist": + """Create a Twist from a ROS geometry_msgs/Twist message. + + Args: + ros_msg: ROS Twist message + + Returns: + Twist instance + """ + linear = Vector3(ros_msg.linear.x, ros_msg.linear.y, ros_msg.linear.z) + angular = Vector3(ros_msg.angular.x, ros_msg.angular.y, ros_msg.angular.z) + return cls(linear, angular) + + def to_ros_msg(self) -> ROSTwist: + """Convert to a ROS geometry_msgs/Twist message. + + Returns: + ROS Twist message + """ + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=self.linear.x, y=self.linear.y, z=self.linear.z) + ros_msg.angular = ROSVector3(x=self.angular.x, y=self.angular.y, z=self.angular.z) + return ros_msg diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index 2524f12faa..31f05934c9 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -17,6 +17,9 @@ import numpy as np import pytest from dimos_lcm.geometry_msgs import Pose as LCMPose +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion from dimos.msgs.geometry_msgs.Pose import Pose, to_pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -747,3 +750,52 @@ def test_pose_addition_3d_rotation(): assert np.isclose(result.position.x, 1.0, atol=1e-10) # X unchanged assert np.isclose(result.position.y, cos45 - sin45, atol=1e-10) assert np.isclose(result.position.z, sin45 + cos45, atol=1e-10) + + +def test_pose_from_ros_msg(): + """Test creating a Pose from a ROS Pose message.""" + ros_msg = ROSPose() + ros_msg.position = ROSPoint(x=1.0, y=2.0, z=3.0) + ros_msg.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) + + pose = Pose.from_ros_msg(ros_msg) + + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 + assert pose.orientation.x == 0.1 + assert pose.orientation.y == 0.2 + assert pose.orientation.z == 0.3 + assert pose.orientation.w == 0.9 + + +def test_pose_to_ros_msg(): + """Test converting a Pose to a ROS Pose message.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + + ros_msg = pose.to_ros_msg() + + assert isinstance(ros_msg, ROSPose) + assert ros_msg.position.x == 1.0 + assert ros_msg.position.y == 2.0 + assert ros_msg.position.z == 3.0 + assert ros_msg.orientation.x == 0.1 + assert ros_msg.orientation.y == 0.2 + assert ros_msg.orientation.z == 0.3 + assert ros_msg.orientation.w == 0.9 + + +def test_pose_ros_roundtrip(): + """Test round-trip conversion between Pose and ROS Pose.""" + original = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) + + ros_msg = original.to_ros_msg() + restored = Pose.from_ros_msg(ros_msg) + + assert restored.position.x == original.position.x + assert restored.position.y == original.position.y + assert restored.position.z == original.position.z + assert restored.orientation.x == original.orientation.x + assert restored.orientation.y == original.orientation.y + assert restored.orientation.z == original.orientation.z + assert restored.orientation.w == original.orientation.w diff --git a/dimos/msgs/geometry_msgs/test_PoseStamped.py b/dimos/msgs/geometry_msgs/test_PoseStamped.py index 86dbf72bdc..33ddee1fc3 100644 --- a/dimos/msgs/geometry_msgs/test_PoseStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -15,6 +15,8 @@ import pickle import time +from geometry_msgs.msg import PoseStamped as ROSPoseStamped + from dimos.msgs.geometry_msgs import PoseStamped @@ -53,3 +55,77 @@ def test_pickle_encode_decode(): assert isinstance(pose_dest, PoseStamped) assert pose_dest is not pose_source assert pose_dest == pose_source + + +def test_pose_stamped_from_ros_msg(): + """Test creating a PoseStamped from a ROS PoseStamped message.""" + ros_msg = ROSPoseStamped() + ros_msg.header.frame_id = "world" + ros_msg.header.stamp.sec = 123 + ros_msg.header.stamp.nanosec = 456000000 + ros_msg.pose.position.x = 1.0 + ros_msg.pose.position.y = 2.0 + ros_msg.pose.position.z = 3.0 + ros_msg.pose.orientation.x = 0.1 + ros_msg.pose.orientation.y = 0.2 + ros_msg.pose.orientation.z = 0.3 + ros_msg.pose.orientation.w = 0.9 + + pose_stamped = PoseStamped.from_ros_msg(ros_msg) + + assert pose_stamped.frame_id == "world" + assert pose_stamped.ts == 123.456 + assert pose_stamped.position.x == 1.0 + assert pose_stamped.position.y == 2.0 + assert pose_stamped.position.z == 3.0 + assert pose_stamped.orientation.x == 0.1 + assert pose_stamped.orientation.y == 0.2 + assert pose_stamped.orientation.z == 0.3 + assert pose_stamped.orientation.w == 0.9 + + +def test_pose_stamped_to_ros_msg(): + """Test converting a PoseStamped to a ROS PoseStamped message.""" + pose_stamped = PoseStamped( + ts=123.456, + frame_id="base_link", + position=(1.0, 2.0, 3.0), + orientation=(0.1, 0.2, 0.3, 0.9), + ) + + ros_msg = pose_stamped.to_ros_msg() + + assert isinstance(ros_msg, ROSPoseStamped) + assert ros_msg.header.frame_id == "base_link" + assert ros_msg.header.stamp.sec == 123 + assert ros_msg.header.stamp.nanosec == 456000000 + assert ros_msg.pose.position.x == 1.0 + assert ros_msg.pose.position.y == 2.0 + assert ros_msg.pose.position.z == 3.0 + assert ros_msg.pose.orientation.x == 0.1 + assert ros_msg.pose.orientation.y == 0.2 + assert ros_msg.pose.orientation.z == 0.3 + assert ros_msg.pose.orientation.w == 0.9 + + +def test_pose_stamped_ros_roundtrip(): + """Test round-trip conversion between PoseStamped and ROS PoseStamped.""" + original = PoseStamped( + ts=123.789, + frame_id="odom", + position=(1.5, 2.5, 3.5), + orientation=(0.15, 0.25, 0.35, 0.85), + ) + + ros_msg = original.to_ros_msg() + restored = PoseStamped.from_ros_msg(ros_msg) + + assert restored.frame_id == original.frame_id + assert restored.ts == original.ts + assert restored.position.x == original.position.x + assert restored.position.y == original.position.y + assert restored.position.z == original.position.z + assert restored.orientation.x == original.orientation.x + assert restored.orientation.y == original.orientation.y + assert restored.orientation.z == original.orientation.z + assert restored.orientation.w == original.orientation.w diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index e069305bca..d6c695c858 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -19,6 +19,7 @@ import pytest from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped +from geometry_msgs.msg import TransformStamped as ROSTransformStamped from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 @@ -421,3 +422,83 @@ def test_transform_from_pose_invalid_type(): with pytest.raises(TypeError): Transform.from_pose(None) + + +def test_transform_from_ros_transform_stamped(): + """Test creating a Transform from a ROS TransformStamped message.""" + ros_msg = ROSTransformStamped() + ros_msg.header.frame_id = "world" + ros_msg.header.stamp.sec = 123 + ros_msg.header.stamp.nanosec = 456000000 + ros_msg.child_frame_id = "robot" + ros_msg.transform.translation.x = 1.0 + ros_msg.transform.translation.y = 2.0 + ros_msg.transform.translation.z = 3.0 + ros_msg.transform.rotation.x = 0.1 + ros_msg.transform.rotation.y = 0.2 + ros_msg.transform.rotation.z = 0.3 + ros_msg.transform.rotation.w = 0.9 + + transform = Transform.from_ros_transform_stamped(ros_msg) + + assert transform.frame_id == "world" + assert transform.child_frame_id == "robot" + assert transform.ts == 123.456 + assert transform.translation.x == 1.0 + assert transform.translation.y == 2.0 + assert transform.translation.z == 3.0 + assert transform.rotation.x == 0.1 + assert transform.rotation.y == 0.2 + assert transform.rotation.z == 0.3 + assert transform.rotation.w == 0.9 + + +def test_transform_to_ros_transform_stamped(): + """Test converting a Transform to a ROS TransformStamped message.""" + transform = Transform( + translation=Vector3(4.0, 5.0, 6.0), + rotation=Quaternion(0.15, 0.25, 0.35, 0.85), + frame_id="base_link", + child_frame_id="sensor", + ts=124.789, + ) + + ros_msg = transform.to_ros_transform_stamped() + + assert isinstance(ros_msg, ROSTransformStamped) + assert ros_msg.header.frame_id == "base_link" + assert ros_msg.child_frame_id == "sensor" + assert ros_msg.header.stamp.sec == 124 + assert ros_msg.header.stamp.nanosec == 789000000 + assert ros_msg.transform.translation.x == 4.0 + assert ros_msg.transform.translation.y == 5.0 + assert ros_msg.transform.translation.z == 6.0 + assert ros_msg.transform.rotation.x == 0.15 + assert ros_msg.transform.rotation.y == 0.25 + assert ros_msg.transform.rotation.z == 0.35 + assert ros_msg.transform.rotation.w == 0.85 + + +def test_transform_ros_roundtrip(): + """Test round-trip conversion between Transform and ROS TransformStamped.""" + original = Transform( + translation=Vector3(7.5, 8.5, 9.5), + rotation=Quaternion(0.0, 0.0, 0.383, 0.924), # ~45 degrees around Z + frame_id="odom", + child_frame_id="base_footprint", + ts=99.123, + ) + + ros_msg = original.to_ros_transform_stamped() + restored = Transform.from_ros_transform_stamped(ros_msg) + + assert restored.frame_id == original.frame_id + assert restored.child_frame_id == original.child_frame_id + assert restored.ts == original.ts + assert restored.translation.x == original.translation.x + assert restored.translation.y == original.translation.y + assert restored.translation.z == original.translation.z + assert restored.rotation.x == original.rotation.x + assert restored.rotation.y == original.rotation.y + assert restored.rotation.z == original.rotation.z + assert restored.rotation.w == original.rotation.w diff --git a/dimos/msgs/geometry_msgs/test_Twist.py b/dimos/msgs/geometry_msgs/test_Twist.py index 2e57523826..6cf3fe0f03 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -15,6 +15,8 @@ import numpy as np import pytest from dimos_lcm.geometry_msgs import Twist as LCMTwist +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Vector3 as ROSVector3 from dimos.msgs.geometry_msgs import Quaternion, Twist, Vector3 @@ -198,3 +200,92 @@ def test_twist_with_lists(): tw2 = Twist(linear=np.array([4, 5, 6]), angular=np.array([0.4, 0.5, 0.6])) assert tw2.linear == Vector3(4, 5, 6) assert tw2.angular == Vector3(0.4, 0.5, 0.6) + + +def test_twist_from_ros_msg(): + """Test Twist.from_ros_msg conversion.""" + # Create ROS message + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=10.0, y=20.0, z=30.0) + ros_msg.angular = ROSVector3(x=1.0, y=2.0, z=3.0) + + # Convert to LCM + lcm_msg = Twist.from_ros_msg(ros_msg) + + assert isinstance(lcm_msg, Twist) + assert lcm_msg.linear.x == 10.0 + assert lcm_msg.linear.y == 20.0 + assert lcm_msg.linear.z == 30.0 + assert lcm_msg.angular.x == 1.0 + assert lcm_msg.angular.y == 2.0 + assert lcm_msg.angular.z == 3.0 + + +def test_twist_to_ros_msg(): + """Test Twist.to_ros_msg conversion.""" + # Create LCM message + lcm_msg = Twist(linear=Vector3(40.0, 50.0, 60.0), angular=Vector3(4.0, 5.0, 6.0)) + + # Convert to ROS + ros_msg = lcm_msg.to_ros_msg() + + assert isinstance(ros_msg, ROSTwist) + assert ros_msg.linear.x == 40.0 + assert ros_msg.linear.y == 50.0 + assert ros_msg.linear.z == 60.0 + assert ros_msg.angular.x == 4.0 + assert ros_msg.angular.y == 5.0 + assert ros_msg.angular.z == 6.0 + + +def test_ros_zero_twist_conversion(): + """Test conversion of zero twist messages between ROS and LCM.""" + # Test ROS to LCM with zero twist + ros_zero = ROSTwist() + lcm_zero = Twist.from_ros_msg(ros_zero) + assert lcm_zero.is_zero() + + # Test LCM to ROS with zero twist + lcm_zero2 = Twist.zero() + ros_zero2 = lcm_zero2.to_ros_msg() + assert ros_zero2.linear.x == 0.0 + assert ros_zero2.linear.y == 0.0 + assert ros_zero2.linear.z == 0.0 + assert ros_zero2.angular.x == 0.0 + assert ros_zero2.angular.y == 0.0 + assert ros_zero2.angular.z == 0.0 + + +def test_ros_negative_values_conversion(): + """Test ROS conversion with negative values.""" + # Create ROS message with negative values + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=-1.5, y=-2.5, z=-3.5) + ros_msg.angular = ROSVector3(x=-0.1, y=-0.2, z=-0.3) + + # Convert to LCM and back + lcm_msg = Twist.from_ros_msg(ros_msg) + ros_msg2 = lcm_msg.to_ros_msg() + + assert ros_msg2.linear.x == -1.5 + assert ros_msg2.linear.y == -2.5 + assert ros_msg2.linear.z == -3.5 + assert ros_msg2.angular.x == -0.1 + assert ros_msg2.angular.y == -0.2 + assert ros_msg2.angular.z == -0.3 + + +def test_ros_roundtrip_conversion(): + """Test round-trip conversion maintains data integrity.""" + # LCM -> ROS -> LCM + original_lcm = Twist(linear=Vector3(1.234, 5.678, 9.012), angular=Vector3(0.111, 0.222, 0.333)) + ros_intermediate = original_lcm.to_ros_msg() + final_lcm = Twist.from_ros_msg(ros_intermediate) + + assert final_lcm == original_lcm + assert final_lcm.linear.x == 1.234 + assert final_lcm.linear.y == 5.678 + assert final_lcm.linear.z == 9.012 + assert final_lcm.angular.x == 0.111 + assert final_lcm.angular.y == 0.222 + assert final_lcm.angular.z == 0.333 diff --git a/dimos/msgs/nav_msgs/Path.py b/dimos/msgs/nav_msgs/Path.py index 8fca1cf25f..bb0b509369 100644 --- a/dimos/msgs/nav_msgs/Path.py +++ b/dimos/msgs/nav_msgs/Path.py @@ -26,6 +26,7 @@ from dimos_lcm.nav_msgs import Path as LCMPath from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime +from nav_msgs.msg import Path as ROSPath from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -187,3 +188,42 @@ def reverse(self) -> "Path": def clear(self) -> None: """Clear all poses from this path (mutable).""" self.poses.clear() + + @classmethod + def from_ros_msg(cls, ros_msg: ROSPath) -> "Path": + """Create a Path from a ROS nav_msgs/Path message. + + Args: + ros_msg: ROS Path message + + Returns: + Path instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert poses + poses = [] + for ros_pose_stamped in ros_msg.poses: + poses.append(PoseStamped.from_ros_msg(ros_pose_stamped)) + + return cls(ts=ts, frame_id=ros_msg.header.frame_id, poses=poses) + + def to_ros_msg(self) -> ROSPath: + """Convert to a ROS nav_msgs/Path message. + + Returns: + ROS Path message + """ + ros_msg = ROSPath() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Convert poses + for pose in self.poses: + ros_msg.poses.append(pose.to_ros_msg()) + + return ros_msg diff --git a/dimos/msgs/nav_msgs/test_Path.py b/dimos/msgs/nav_msgs/test_Path.py index 0a34245448..e7156d48c0 100644 --- a/dimos/msgs/nav_msgs/test_Path.py +++ b/dimos/msgs/nav_msgs/test_Path.py @@ -15,6 +15,7 @@ import time import pytest +from nav_msgs.msg import Path as ROSPath from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -288,3 +289,97 @@ def test_str_representation(): path.push_mut(create_test_pose(1, 1, 0)) path.push_mut(create_test_pose(2, 2, 0)) assert str(path) == "Path(frame_id='map', poses=2)" + + +def test_path_from_ros_msg(): + """Test creating a Path from a ROS Path message.""" + ros_msg = ROSPath() + ros_msg.header.frame_id = "map" + ros_msg.header.stamp.sec = 123 + ros_msg.header.stamp.nanosec = 456000000 + + # Add some poses + for i in range(3): + from geometry_msgs.msg import PoseStamped as ROSPoseStamped + + ros_pose = ROSPoseStamped() + ros_pose.header.frame_id = "map" + ros_pose.header.stamp.sec = 123 + i + ros_pose.header.stamp.nanosec = 0 + ros_pose.pose.position.x = float(i) + ros_pose.pose.position.y = float(i * 2) + ros_pose.pose.position.z = float(i * 3) + ros_pose.pose.orientation.x = 0.0 + ros_pose.pose.orientation.y = 0.0 + ros_pose.pose.orientation.z = 0.0 + ros_pose.pose.orientation.w = 1.0 + ros_msg.poses.append(ros_pose) + + path = Path.from_ros_msg(ros_msg) + + assert path.frame_id == "map" + assert path.ts == 123.456 + assert len(path.poses) == 3 + + for i, pose in enumerate(path.poses): + assert pose.position.x == float(i) + assert pose.position.y == float(i * 2) + assert pose.position.z == float(i * 3) + assert pose.orientation.w == 1.0 + + +def test_path_to_ros_msg(): + """Test converting a Path to a ROS Path message.""" + poses = [ + PoseStamped( + ts=124.0 + i, frame_id="odom", position=[i, i * 2, i * 3], orientation=[0, 0, 0, 1] + ) + for i in range(3) + ] + + path = Path(ts=123.456, frame_id="odom", poses=poses) + + ros_msg = path.to_ros_msg() + + assert isinstance(ros_msg, ROSPath) + assert ros_msg.header.frame_id == "odom" + assert ros_msg.header.stamp.sec == 123 + assert ros_msg.header.stamp.nanosec == 456000000 + assert len(ros_msg.poses) == 3 + + for i, ros_pose in enumerate(ros_msg.poses): + assert ros_pose.pose.position.x == float(i) + assert ros_pose.pose.position.y == float(i * 2) + assert ros_pose.pose.position.z == float(i * 3) + assert ros_pose.pose.orientation.w == 1.0 + + +def test_path_ros_roundtrip(): + """Test round-trip conversion between Path and ROS Path.""" + poses = [ + PoseStamped( + ts=100.0 + i * 0.1, + frame_id="world", + position=[i * 1.5, i * 2.5, i * 3.5], + orientation=[0.1, 0.2, 0.3, 0.9], + ) + for i in range(3) + ] + + original = Path(ts=99.789, frame_id="world", poses=poses) + + ros_msg = original.to_ros_msg() + restored = Path.from_ros_msg(ros_msg) + + assert restored.frame_id == original.frame_id + assert restored.ts == original.ts + assert len(restored.poses) == len(original.poses) + + for orig_pose, rest_pose in zip(original.poses, restored.poses): + assert rest_pose.position.x == orig_pose.position.x + assert rest_pose.position.y == orig_pose.position.y + assert rest_pose.position.z == orig_pose.position.z + assert rest_pose.orientation.x == orig_pose.orientation.x + assert rest_pose.orientation.y == orig_pose.orientation.y + assert rest_pose.orientation.z == orig_pose.orientation.z + assert rest_pose.orientation.w == orig_pose.orientation.w diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index 9ccba615b2..33576a72dc 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -34,6 +34,7 @@ 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 tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -119,3 +120,35 @@ def __str__(self) -> str: for i, transform in enumerate(self.transforms): lines.append(f" [{i}] {transform.frame_id} @ {transform.ts:.3f}") return "\n".join(lines) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSTFMessage) -> "TFMessage": + """Create a TFMessage from a ROS tf2_msgs/TFMessage message. + + Args: + ros_msg: ROS TFMessage message + + Returns: + TFMessage instance + """ + transforms = [] + for ros_transform_stamped in ros_msg.transforms: + # Convert from ROS TransformStamped to our Transform + transform = Transform.from_ros_transform_stamped(ros_transform_stamped) + transforms.append(transform) + + return cls(*transforms) + + def to_ros_msg(self) -> ROSTFMessage: + """Convert to a ROS tf2_msgs/TFMessage message. + + Returns: + ROS TFMessage message + """ + ros_msg = ROSTFMessage() + + # Convert each Transform to ROS TransformStamped + for transform in self.transforms: + ros_msg.transforms.append(transform.to_ros_transform_stamped()) + + return ros_msg diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index 6eec4cbdcc..4bb5b2f3b0 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -14,6 +14,7 @@ import pytest from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage +from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.tf2_msgs import TFMessage @@ -107,3 +108,153 @@ def test_tfmessage_lcm_encode_decode(): assert ts2.child_frame_id == "target" assert ts2.transform.rotation.z == 0.707 assert ts2.transform.rotation.w == 0.707 + + +def test_tfmessage_from_ros_msg(): + """Test creating a TFMessage from a ROS TFMessage message.""" + from geometry_msgs.msg import TransformStamped as ROSTransformStamped + + ros_msg = ROSTFMessage() + + # Add first transform + tf1 = ROSTransformStamped() + tf1.header.frame_id = "world" + tf1.header.stamp.sec = 123 + tf1.header.stamp.nanosec = 456000000 + tf1.child_frame_id = "robot" + tf1.transform.translation.x = 1.0 + tf1.transform.translation.y = 2.0 + tf1.transform.translation.z = 3.0 + tf1.transform.rotation.x = 0.0 + tf1.transform.rotation.y = 0.0 + tf1.transform.rotation.z = 0.0 + tf1.transform.rotation.w = 1.0 + ros_msg.transforms.append(tf1) + + # Add second transform + tf2 = ROSTransformStamped() + tf2.header.frame_id = "robot" + tf2.header.stamp.sec = 124 + tf2.header.stamp.nanosec = 567000000 + tf2.child_frame_id = "sensor" + tf2.transform.translation.x = 4.0 + tf2.transform.translation.y = 5.0 + tf2.transform.translation.z = 6.0 + tf2.transform.rotation.x = 0.0 + tf2.transform.rotation.y = 0.0 + tf2.transform.rotation.z = 0.707 + tf2.transform.rotation.w = 0.707 + ros_msg.transforms.append(tf2) + + # Convert to TFMessage + tfmsg = TFMessage.from_ros_msg(ros_msg) + + assert len(tfmsg) == 2 + + # Check first transform + assert tfmsg[0].frame_id == "world" + assert tfmsg[0].child_frame_id == "robot" + assert tfmsg[0].ts == 123.456 + assert tfmsg[0].translation.x == 1.0 + assert tfmsg[0].translation.y == 2.0 + assert tfmsg[0].translation.z == 3.0 + assert tfmsg[0].rotation.w == 1.0 + + # Check second transform + assert tfmsg[1].frame_id == "robot" + assert tfmsg[1].child_frame_id == "sensor" + assert tfmsg[1].ts == 124.567 + assert tfmsg[1].translation.x == 4.0 + assert tfmsg[1].translation.y == 5.0 + assert tfmsg[1].translation.z == 6.0 + assert tfmsg[1].rotation.z == 0.707 + assert tfmsg[1].rotation.w == 0.707 + + +def test_tfmessage_to_ros_msg(): + """Test converting a TFMessage to a ROS TFMessage message.""" + # Create transforms + tf1 = Transform( + translation=Vector3(1.0, 2.0, 3.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="map", + child_frame_id="base_link", + ts=123.456, + ) + tf2 = Transform( + translation=Vector3(7.0, 8.0, 9.0), + rotation=Quaternion(0.1, 0.2, 0.3, 0.9), + frame_id="base_link", + child_frame_id="lidar", + ts=125.789, + ) + + tfmsg = TFMessage(tf1, tf2) + + # Convert to ROS message + ros_msg = tfmsg.to_ros_msg() + + assert isinstance(ros_msg, ROSTFMessage) + assert len(ros_msg.transforms) == 2 + + # Check first transform + assert ros_msg.transforms[0].header.frame_id == "map" + assert ros_msg.transforms[0].child_frame_id == "base_link" + assert ros_msg.transforms[0].header.stamp.sec == 123 + assert ros_msg.transforms[0].header.stamp.nanosec == 456000000 + assert ros_msg.transforms[0].transform.translation.x == 1.0 + assert ros_msg.transforms[0].transform.translation.y == 2.0 + assert ros_msg.transforms[0].transform.translation.z == 3.0 + assert ros_msg.transforms[0].transform.rotation.w == 1.0 + + # Check second transform + assert ros_msg.transforms[1].header.frame_id == "base_link" + assert ros_msg.transforms[1].child_frame_id == "lidar" + assert ros_msg.transforms[1].header.stamp.sec == 125 + assert ros_msg.transforms[1].header.stamp.nanosec == 789000000 + assert ros_msg.transforms[1].transform.translation.x == 7.0 + assert ros_msg.transforms[1].transform.translation.y == 8.0 + assert ros_msg.transforms[1].transform.translation.z == 9.0 + assert ros_msg.transforms[1].transform.rotation.x == 0.1 + assert ros_msg.transforms[1].transform.rotation.y == 0.2 + assert ros_msg.transforms[1].transform.rotation.z == 0.3 + assert ros_msg.transforms[1].transform.rotation.w == 0.9 + + +def test_tfmessage_ros_roundtrip(): + """Test round-trip conversion between TFMessage and ROS TFMessage.""" + # Create transforms with various properties + tf1 = Transform( + translation=Vector3(1.5, 2.5, 3.5), + rotation=Quaternion(0.15, 0.25, 0.35, 0.85), + frame_id="odom", + child_frame_id="base_footprint", + ts=100.123, + ) + tf2 = Transform( + translation=Vector3(0.1, 0.2, 0.3), + rotation=Quaternion(0.0, 0.0, 0.383, 0.924), + frame_id="base_footprint", + child_frame_id="camera", + ts=100.456, + ) + + original = TFMessage(tf1, tf2) + + # Convert to ROS and back + ros_msg = original.to_ros_msg() + restored = TFMessage.from_ros_msg(ros_msg) + + assert len(restored) == len(original) + + for orig_tf, rest_tf in zip(original, restored): + assert rest_tf.frame_id == orig_tf.frame_id + assert rest_tf.child_frame_id == orig_tf.child_frame_id + assert rest_tf.ts == orig_tf.ts + assert rest_tf.translation.x == orig_tf.translation.x + assert rest_tf.translation.y == orig_tf.translation.y + assert rest_tf.translation.z == orig_tf.translation.z + assert rest_tf.rotation.x == orig_tf.rotation.x + assert rest_tf.rotation.y == orig_tf.rotation.y + assert rest_tf.rotation.z == orig_tf.rotation.z + assert rest_tf.rotation.w == orig_tf.rotation.w diff --git a/dimos/robot/ros_bridge.py b/dimos/robot/ros_bridge.py new file mode 100644 index 0000000000..450b3eb317 --- /dev/null +++ b/dimos/robot/ros_bridge.py @@ -0,0 +1,173 @@ +# 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 logging +import threading +import time +from typing import Dict, Any, Type, Literal, Optional +from enum import Enum + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy + +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic +from dimos.utils.logging_config import setup_logger + +logger = setup_logger("dimos.robot.ros_bridge", level=logging.INFO) + + +class BridgeDirection(Enum): + """Direction of message bridging.""" + + ROS_TO_DIMOS = "ros_to_dimos" + DIMOS_TO_ROS = "dimos_to_ros" + + +class ROSBridge: + """Unidirectional bridge between ROS and DIMOS for message passing.""" + + def __init__(self, node_name: str = "dimos_ros_bridge"): + """Initialize the ROS-DIMOS bridge. + + Args: + node_name: Name for the ROS node (default: "dimos_ros_bridge") + """ + if not rclpy.ok(): + rclpy.init() + + self.node = Node(node_name) + self.lcm = LCM() + self.lcm.start() + + self._executor = MultiThreadedExecutor() + self._executor.add_node(self.node) + + self._spin_thread = threading.Thread(target=self._ros_spin, daemon=True) + self._spin_thread.start() + + self._bridges: Dict[str, Dict[str, Any]] = {} + + self._qos = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.VOLATILE, + depth=10, + ) + + logger.info(f"ROSBridge initialized with node name: {node_name}") + + def _ros_spin(self): + """Background thread for spinning ROS executor.""" + try: + self._executor.spin() + finally: + self._executor.shutdown() + + def add_topic( + self, topic_name: str, dimos_type: Type, ros_type: Type, direction: BridgeDirection + ) -> None: + """Add unidirectional bridging for a topic. + + Args: + topic_name: Name of the topic (e.g., "/cmd_vel") + dimos_type: DIMOS message type (e.g., dimos.msgs.geometry_msgs.Twist) + ros_type: ROS message type (e.g., geometry_msgs.msg.Twist) + direction: Direction of bridging (ROS_TO_DIMOS or DIMOS_TO_ROS) + """ + if topic_name in self._bridges: + logger.warning(f"Topic {topic_name} already bridged") + return + + # Create DIMOS/LCM topic + dimos_topic = Topic(topic_name, dimos_type) + + ros_subscription = None + ros_publisher = None + dimos_subscription = None + + if direction == BridgeDirection.ROS_TO_DIMOS: + + def ros_callback(msg): + self._ros_to_dimos(msg, dimos_topic, dimos_type, topic_name) + + ros_subscription = self.node.create_subscription( + ros_type, topic_name, ros_callback, self._qos + ) + logger.info(f" ROS → DIMOS: Subscribing to ROS topic {topic_name}") + + elif direction == BridgeDirection.DIMOS_TO_ROS: + ros_publisher = self.node.create_publisher(ros_type, topic_name, self._qos) + + def dimos_callback(msg, _topic): + self._dimos_to_ros(msg, ros_publisher, topic_name) + + dimos_subscription = self.lcm.subscribe(dimos_topic, dimos_callback) + logger.info(f" DIMOS → ROS: Subscribing to DIMOS topic {topic_name}") + else: + raise ValueError(f"Invalid bridge direction: {direction}") + + self._bridges[topic_name] = { + "dimos_topic": dimos_topic, + "dimos_type": dimos_type, + "ros_type": ros_type, + "ros_subscription": ros_subscription, + "ros_publisher": ros_publisher, + "dimos_subscription": dimos_subscription, + "direction": direction, + } + + direction_str = { + BridgeDirection.ROS_TO_DIMOS: "ROS → DIMOS", + BridgeDirection.DIMOS_TO_ROS: "DIMOS → ROS", + }[direction] + + logger.info(f"Bridged topic: {topic_name} ({direction_str})") + logger.info(f" DIMOS type: {dimos_type.__name__}, ROS type: {ros_type.__name__}") + + def _ros_to_dimos( + self, ros_msg: Any, dimos_topic: Topic, dimos_type: Type, _topic_name: str + ) -> None: + """Convert ROS message to DIMOS and publish. + + Args: + ros_msg: ROS message + dimos_topic: DIMOS topic to publish to + dimos_type: DIMOS message type + topic_name: Name of the topic for tracking + """ + dimos_msg = dimos_type.from_ros_msg(ros_msg) + self.lcm.publish(dimos_topic, dimos_msg) + + def _dimos_to_ros(self, dimos_msg: Any, ros_publisher, _topic_name: str) -> None: + """Convert DIMOS message to ROS and publish. + + Args: + dimos_msg: DIMOS message + ros_publisher: ROS publisher to use + _topic_name: Name of the topic (unused, kept for consistency) + """ + ros_msg = dimos_msg.to_ros_msg() + ros_publisher.publish(ros_msg) + + def shutdown(self): + """Shutdown the bridge and clean up resources.""" + self._executor.shutdown() + self.node.destroy_node() + + if rclpy.ok(): + rclpy.shutdown() + + logger.info("ROSBridge shutdown complete") diff --git a/dimos/robot/test_ros_bridge.py b/dimos/robot/test_ros_bridge.py new file mode 100644 index 0000000000..bd49238c77 --- /dev/null +++ b/dimos/robot/test_ros_bridge.py @@ -0,0 +1,656 @@ +# 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 unittest.mock import MagicMock, patch + +import pytest +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Vector3 as ROSVector3 +from geometry_msgs.msg import PoseStamped as ROSPoseStamped +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion +from nav_msgs.msg import Path as ROSPath +from sensor_msgs.msg import LaserScan as ROSLaserScan +from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 +from std_msgs.msg import String as ROSString +from std_msgs.msg import Header as ROSHeader + +from dimos.msgs.geometry_msgs import Twist, Vector3, PoseStamped, Pose, Quaternion +from dimos.msgs.nav_msgs import Path +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic +from dimos.robot.ros_bridge import ROSBridge, BridgeDirection + + +@pytest.fixture +def bridge(): + """Create a ROSBridge instance with mocked internals.""" + with ( + patch("dimos.robot.ros_bridge.rclpy") as mock_rclpy, + patch("dimos.robot.ros_bridge.Node") as mock_node_class, + patch("dimos.robot.ros_bridge.LCM") as mock_lcm_class, + patch("dimos.robot.ros_bridge.MultiThreadedExecutor") as mock_executor_class, + ): + mock_rclpy.ok.return_value = False + mock_node = MagicMock() + mock_node.create_subscription = MagicMock(return_value=MagicMock()) + mock_node.create_publisher = MagicMock(return_value=MagicMock()) + mock_node_class.return_value = mock_node + + mock_lcm = MagicMock() + mock_lcm.subscribe = MagicMock(return_value=MagicMock()) + mock_lcm.publish = MagicMock() + mock_lcm_class.return_value = mock_lcm + + mock_executor = MagicMock() + mock_executor_class.return_value = mock_executor + + bridge = ROSBridge("test_bridge") + + bridge._mock_rclpy = mock_rclpy + bridge._mock_node_class = mock_node_class + bridge._mock_lcm_class = mock_lcm_class + + return bridge + + +def test_bridge_initialization(): + """Test that the bridge initializes correctly with its own instances.""" + with ( + patch("dimos.robot.ros_bridge.rclpy") as mock_rclpy, + patch("dimos.robot.ros_bridge.Node") as mock_node_class, + patch("dimos.robot.ros_bridge.LCM") as mock_lcm_class, + patch("dimos.robot.ros_bridge.MultiThreadedExecutor"), + ): + mock_rclpy.ok.return_value = False + + bridge = ROSBridge("test_bridge") + + mock_rclpy.init.assert_called_once() + mock_node_class.assert_called_once_with("test_bridge") + mock_lcm_class.assert_called_once() + bridge.lcm.start.assert_called_once() + + assert bridge._bridges == {} + assert bridge._qos is not None + + +def test_add_topic_ros_to_dimos(bridge): + """Test that add_topic creates ROS subscription for ROS->DIMOS direction.""" + topic_name = "/cmd_vel" + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + bridge.node.create_subscription.assert_called_once() + call_args = bridge.node.create_subscription.call_args + assert call_args[0][0] == ROSTwist + assert call_args[0][1] == topic_name + + bridge.node.create_publisher.assert_not_called() + bridge.lcm.subscribe.assert_not_called() + + assert topic_name in bridge._bridges + assert "dimos_topic" in bridge._bridges[topic_name] + assert "dimos_type" in bridge._bridges[topic_name] + assert "ros_type" in bridge._bridges[topic_name] + assert bridge._bridges[topic_name]["dimos_type"] == Twist + assert bridge._bridges[topic_name]["ros_type"] == ROSTwist + assert bridge._bridges[topic_name]["direction"] == BridgeDirection.ROS_TO_DIMOS + + +def test_add_topic_dimos_to_ros(bridge): + """Test that add_topic creates ROS publisher for DIMOS->ROS direction.""" + topic_name = "/cmd_vel" + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) + + bridge.node.create_subscription.assert_not_called() + bridge.node.create_publisher.assert_called_once_with(ROSTwist, topic_name, bridge._qos) + bridge.lcm.subscribe.assert_called_once() + + assert topic_name in bridge._bridges + assert "dimos_topic" in bridge._bridges[topic_name] + assert "dimos_type" in bridge._bridges[topic_name] + assert "ros_type" in bridge._bridges[topic_name] + assert bridge._bridges[topic_name]["dimos_type"] == Twist + assert bridge._bridges[topic_name]["ros_type"] == ROSTwist + assert bridge._bridges[topic_name]["direction"] == BridgeDirection.DIMOS_TO_ROS + + +def test_ros_to_dimos_conversion(bridge): + """Test ROS to DIMOS message conversion and publishing.""" + # Create a ROS Twist message + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=1.0, y=2.0, z=3.0) + ros_msg.angular = ROSVector3(x=0.1, y=0.2, z=0.3) + + # Create DIMOS topic + dimos_topic = Topic("/test", Twist) + + # Call the conversion method with type + bridge._ros_to_dimos(ros_msg, dimos_topic, Twist, "/test") + + # Verify DIMOS publish was called + bridge.lcm.publish.assert_called_once() + + # Get the published message + published_topic, published_msg = bridge.lcm.publish.call_args[0] + + assert published_topic == dimos_topic + assert isinstance(published_msg, Twist) + assert published_msg.linear.x == 1.0 + assert published_msg.linear.y == 2.0 + assert published_msg.linear.z == 3.0 + assert published_msg.angular.x == 0.1 + assert published_msg.angular.y == 0.2 + assert published_msg.angular.z == 0.3 + + +def test_dimos_to_ros_conversion(bridge): + """Test DIMOS to ROS message conversion and publishing.""" + # Create a DIMOS Twist message + dimos_msg = Twist(linear=Vector3(4.0, 5.0, 6.0), angular=Vector3(0.4, 0.5, 0.6)) + + # Create mock ROS publisher + ros_pub = MagicMock() + + # Call the conversion method + bridge._dimos_to_ros(dimos_msg, ros_pub, "/test") + + # Verify ROS publish was called + ros_pub.publish.assert_called_once() + + # Get the published message + published_msg = ros_pub.publish.call_args[0][0] + + assert isinstance(published_msg, ROSTwist) + assert published_msg.linear.x == 4.0 + assert published_msg.linear.y == 5.0 + assert published_msg.linear.z == 6.0 + assert published_msg.angular.x == 0.4 + assert published_msg.angular.y == 0.5 + assert published_msg.angular.z == 0.6 + + +def test_unidirectional_flow_ros_to_dimos(bridge): + """Test that messages flow from ROS to DIMOS when configured.""" + topic_name = "/cmd_vel" + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=1.5, y=2.5, z=3.5) + ros_msg.angular = ROSVector3(x=0.15, y=0.25, z=0.35) + + ros_callback(ros_msg) + + bridge.lcm.publish.assert_called_once() + _, published_msg = bridge.lcm.publish.call_args[0] + assert isinstance(published_msg, Twist) + assert published_msg.linear.x == 1.5 + + +def test_unidirectional_flow_dimos_to_ros(bridge): + """Test that messages flow from DIMOS to ROS when configured.""" + topic_name = "/cmd_vel" + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) + + dimos_callback = bridge.lcm.subscribe.call_args[0][1] + + dimos_msg = Twist(linear=Vector3(7.0, 8.0, 9.0), angular=Vector3(0.7, 0.8, 0.9)) + + ros_publisher = bridge.node.create_publisher.return_value + + dimos_callback(dimos_msg, None) + + ros_publisher.publish.assert_called_once() + published_ros_msg = ros_publisher.publish.call_args[0][0] + assert isinstance(published_ros_msg, ROSTwist) + assert published_ros_msg.linear.x == 7.0 + + +def test_multiple_topics(bridge): + """Test that multiple topics can be bridged simultaneously.""" + topics = [ + ("/cmd_vel", BridgeDirection.ROS_TO_DIMOS), + ("/teleop", BridgeDirection.DIMOS_TO_ROS), + ("/nav_cmd", BridgeDirection.ROS_TO_DIMOS), + ] + + for topic, direction in topics: + bridge.add_topic(topic, Twist, ROSTwist, direction=direction) + + assert len(bridge._bridges) == 3 + for topic, _ in topics: + assert topic in bridge._bridges + + assert bridge.node.create_subscription.call_count == 2 + assert bridge.node.create_publisher.call_count == 1 + assert bridge.lcm.subscribe.call_count == 1 + + +def test_stress_ros_to_dimos_100_messages(bridge): + """Test publishing 100 ROS messages and verify DIMOS receives them all.""" + topic_name = "/stress_test" + num_messages = 100 + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + + for i in range(num_messages): + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=float(i), y=float(i * 2), z=float(i * 3)) + ros_msg.angular = ROSVector3(x=float(i * 0.1), y=float(i * 0.2), z=float(i * 0.3)) + + ros_callback(ros_msg) + + assert bridge.lcm.publish.call_count == num_messages + + last_call = bridge.lcm.publish.call_args_list[-1] + _, last_msg = last_call[0] + assert isinstance(last_msg, Twist) + assert last_msg.linear.x == 99.0 + assert last_msg.linear.y == 198.0 + assert last_msg.linear.z == 297.0 + assert abs(last_msg.angular.x - 9.9) < 0.01 + assert abs(last_msg.angular.y - 19.8) < 0.01 + assert abs(last_msg.angular.z - 29.7) < 0.01 + + +def test_stress_dimos_to_ros_100_messages(bridge): + """Test publishing 100 DIMOS messages and verify ROS receives them all.""" + topic_name = "/stress_test_reverse" + num_messages = 100 + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) + + dimos_callback = bridge.lcm.subscribe.call_args[0][1] + ros_publisher = bridge.node.create_publisher.return_value + + for i in range(num_messages): + dimos_msg = Twist( + linear=Vector3(float(i * 10), float(i * 20), float(i * 30)), + angular=Vector3(float(i * 0.01), float(i * 0.02), float(i * 0.03)), + ) + + dimos_callback(dimos_msg, None) + + assert ros_publisher.publish.call_count == num_messages + + last_call = ros_publisher.publish.call_args_list[-1] + last_ros_msg = last_call[0][0] + assert isinstance(last_ros_msg, ROSTwist) + assert last_ros_msg.linear.x == 990.0 + assert last_ros_msg.linear.y == 1980.0 + assert last_ros_msg.linear.z == 2970.0 + assert abs(last_ros_msg.angular.x - 0.99) < 0.001 + assert abs(last_ros_msg.angular.y - 1.98) < 0.001 + assert abs(last_ros_msg.angular.z - 2.97) < 0.001 + + +def test_two_topics_different_directions(bridge): + """Test two topics with different directions handling messages.""" + topic_r2d = "/ros_to_dimos" + topic_d2r = "/dimos_to_ros" + + bridge.add_topic(topic_r2d, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + bridge.add_topic(topic_d2r, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + dimos_callback = bridge.lcm.subscribe.call_args[0][1] + ros_publisher = bridge.node.create_publisher.return_value + + for i in range(50): + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=float(i), y=0.0, z=0.0) + ros_msg.angular = ROSVector3(x=0.0, y=0.0, z=float(i * 0.1)) + ros_callback(ros_msg) + + dimos_msg = Twist( + linear=Vector3(0.0, float(i), 0.0), angular=Vector3(0.0, 0.0, float(i * 0.2)) + ) + dimos_callback(dimos_msg, None) + + assert bridge.lcm.publish.call_count == 50 + assert ros_publisher.publish.call_count == 50 + + last_dimos_call = bridge.lcm.publish.call_args_list[-1] + _, last_dimos_msg = last_dimos_call[0] + assert last_dimos_msg.linear.x == 49.0 + + last_ros_call = ros_publisher.publish.call_args_list[-1] + last_ros_msg = last_ros_call[0][0] + assert last_ros_msg.linear.y == 49.0 + + +def test_high_frequency_burst(bridge): + """Test handling a burst of 1000 messages to ensure no drops.""" + topic_name = "/burst_test" + burst_size = 1000 + + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + + messages_sent = [] + for i in range(burst_size): + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=float(i), y=float(i), z=float(i)) + ros_msg.angular = ROSVector3(x=0.0, y=0.0, z=0.0) + messages_sent.append(i) + ros_callback(ros_msg) + + assert bridge.lcm.publish.call_count == burst_size + + for idx, call in enumerate(bridge.lcm.publish.call_args_list): + _, msg = call[0] + assert msg.linear.x == float(idx) + + +def test_multiple_topics_with_different_rates(bridge): + """Test multiple topics receiving messages at different rates.""" + topics = { + "/fast_topic": 100, # 100 messages + "/medium_topic": 50, # 50 messages + "/slow_topic": 10, # 10 messages + } + + for topic_name in topics: + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + callbacks = [] + for i in range(3): + callbacks.append(bridge.node.create_subscription.call_args_list[i][0][2]) + + bridge.lcm.publish.reset_mock() + + for topic_idx, (topic_name, msg_count) in enumerate(topics.items()): + for i in range(msg_count): + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=float(topic_idx), y=float(i), z=0.0) + callbacks[topic_idx](ros_msg) + + total_expected = sum(topics.values()) + assert bridge.lcm.publish.call_count == total_expected + + +def test_pose_stamped_bridging(bridge): + """Test bridging PoseStamped messages.""" + topic_name = "/robot_pose" + + # Test ROS to DIMOS + bridge.add_topic( + topic_name, PoseStamped, ROSPoseStamped, direction=BridgeDirection.ROS_TO_DIMOS + ) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + + ros_msg = ROSPoseStamped() + ros_msg.header.frame_id = "map" + ros_msg.header.stamp.sec = 100 + ros_msg.header.stamp.nanosec = 500000000 + ros_msg.pose.position.x = 10.0 + ros_msg.pose.position.y = 20.0 + ros_msg.pose.position.z = 30.0 + ros_msg.pose.orientation.x = 0.0 + ros_msg.pose.orientation.y = 0.0 + ros_msg.pose.orientation.z = 0.707 + ros_msg.pose.orientation.w = 0.707 + + ros_callback(ros_msg) + + bridge.lcm.publish.assert_called_once() + _, published_msg = bridge.lcm.publish.call_args[0] + assert hasattr(published_msg, "frame_id") + assert hasattr(published_msg, "position") + assert hasattr(published_msg, "orientation") + + +def test_path_bridging(bridge): + """Test bridging Path messages.""" + topic_name = "/planned_path" + + # Test DIMOS to ROS + bridge.add_topic(topic_name, Path, ROSPath, direction=BridgeDirection.DIMOS_TO_ROS) + + dimos_callback = bridge.lcm.subscribe.call_args[0][1] + ros_publisher = bridge.node.create_publisher.return_value + + # Create a DIMOS Path with multiple poses + poses = [] + for i in range(5): + pose = PoseStamped( + ts=100.0 + i, + frame_id="map", + position=Vector3(float(i), float(i * 2), 0.0), + orientation=Quaternion(0, 0, 0, 1), + ) + poses.append(pose) + + dimos_path = Path(frame_id="map", poses=poses) + + dimos_callback(dimos_path, None) + + ros_publisher.publish.assert_called_once() + published_ros_msg = ros_publisher.publish.call_args[0][0] + assert isinstance(published_ros_msg, ROSPath) + + +def test_multiple_message_types(bridge): + """Test bridging multiple different message types simultaneously.""" + topics = [ + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS), + ("/robot_pose", PoseStamped, ROSPoseStamped, BridgeDirection.DIMOS_TO_ROS), + ("/global_path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/local_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), + ("/teleop_twist", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS), + ] + + for topic_name, dimos_type, ros_type, direction in topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == 5 + + # Count subscriptions and publishers + ros_to_dimos_count = sum(1 for _, _, _, d in topics if d == BridgeDirection.ROS_TO_DIMOS) + dimos_to_ros_count = sum(1 for _, _, _, d in topics if d == BridgeDirection.DIMOS_TO_ROS) + + assert bridge.node.create_subscription.call_count == ros_to_dimos_count + assert bridge.node.create_publisher.call_count == dimos_to_ros_count + assert bridge.lcm.subscribe.call_count == dimos_to_ros_count + + +def test_navigation_stack_topics(bridge): + """Test common navigation stack topics.""" + nav_topics = [ + ("/move_base/goal", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/move_base/global_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/move_base/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/robot_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + for topic_name, dimos_type, ros_type, direction in nav_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == len(nav_topics) + + # Verify each topic is configured correctly + for topic_name, dimos_type, ros_type, direction in nav_topics: + assert topic_name in bridge._bridges + assert bridge._bridges[topic_name]["dimos_type"] == dimos_type + assert bridge._bridges[topic_name]["ros_type"] == ros_type + assert bridge._bridges[topic_name]["direction"] == direction + + +def test_control_topics(bridge): + """Test control system topics.""" + control_topics = [ + ("/joint_commands", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/feedback", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + for topic_name, dimos_type, ros_type, direction in control_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == len(control_topics) + + +def test_perception_topics(bridge): + """Test perception system topics.""" + perception_topics = [ + ("/detected_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/tracked_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), + ("/vision_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + for topic_name, dimos_type, ros_type, direction in perception_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + # All perception topics are ROS to DIMOS + assert bridge.node.create_subscription.call_count == len(perception_topics) + assert bridge.node.create_publisher.call_count == 0 + + +def test_mixed_frequency_topics(bridge): + """Test topics with different expected frequencies.""" + # High frequency (100Hz+) + high_freq_topics = [ + ("/imu/data", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + # Medium frequency (10-50Hz) + medium_freq_topics = [ + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + # Low frequency (1-5Hz) + low_freq_topics = [ + ("/global_path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/goal", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ] + + all_topics = high_freq_topics + medium_freq_topics + low_freq_topics + + for topic_name, dimos_type, ros_type, direction in all_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == len(all_topics) + + # Test high frequency message handling + for topic_name, _, _, direction in high_freq_topics: + if direction == BridgeDirection.ROS_TO_DIMOS: + # Find the callback for this topic + for i, call in enumerate(bridge.node.create_subscription.call_args_list): + if call[0][1] == topic_name: + callback = call[0][2] + # Send 100 messages rapidly + for j in range(100): + ros_msg = ROSPoseStamped() + ros_msg.header.stamp.sec = j + callback(ros_msg) + break + + +def test_bidirectional_prevention(bridge): + """Test that the same topic cannot be added in both directions.""" + topic_name = "/cmd_vel" + + # Add topic in one direction + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + + # Try to add the same topic in opposite direction should not create duplicate + # The bridge should handle this gracefully + initial_bridges = len(bridge._bridges) + bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) + + # Should still have the same number of bridges (topic gets reconfigured, not duplicated) + assert len(bridge._bridges) == initial_bridges + + +def test_robot_arm_topics(bridge): + """Test robot arm control topics.""" + arm_topics = [ + ("/arm/joint_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/arm/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/arm/end_effector_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/arm/gripper_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/arm/cartesian_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ] + + for topic_name, dimos_type, ros_type, direction in arm_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == len(arm_topics) + + # Check that arm control commands go from DIMOS to ROS + dimos_to_ros = [t for t in arm_topics if t[3] == BridgeDirection.DIMOS_TO_ROS] + ros_to_dimos = [t for t in arm_topics if t[3] == BridgeDirection.ROS_TO_DIMOS] + + assert bridge.node.create_publisher.call_count == len(dimos_to_ros) + assert bridge.node.create_subscription.call_count == len(ros_to_dimos) + + +def test_mobile_base_topics(bridge): + """Test mobile robot base topics.""" + base_topics = [ + ("/base/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/base/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/base/global_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/base/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/base/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ] + + for topic_name, dimos_type, ros_type, direction in base_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + # Verify topics are properly categorized + for topic_name, dimos_type, ros_type, direction in base_topics: + bridge_info = bridge._bridges[topic_name] + assert bridge_info["direction"] == direction + assert bridge_info["dimos_type"] == dimos_type + assert bridge_info["ros_type"] == ros_type + + +def test_autonomous_vehicle_topics(bridge): + """Test autonomous vehicle topics.""" + av_topics = [ + ("/vehicle/steering_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/vehicle/throttle_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/vehicle/brake_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), + ("/vehicle/pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), + ("/vehicle/planned_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), + ("/vehicle/current_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), + ] + + for topic_name, dimos_type, ros_type, direction in av_topics: + bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) + + assert len(bridge._bridges) == len(av_topics) + + # Count control vs feedback topics + control_topics = [t for t in av_topics if "cmd" in t[0] or "planned" in t[0]] + feedback_topics = [t for t in av_topics if "pose" in t[0] or "current" in t[0]] + + assert len(control_topics) == 4 # steering, throttle, brake, planned_trajectory + assert len(feedback_topics) == 2 # pose, current_path From 57f37fdb3e2876b579730ae5594adeb0b657ff73 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 7 Sep 2025 12:34:20 -0700 Subject: [PATCH 02/21] added ros cmd_vel to g1 --- dimos/robot/unitree_webrtc/connection.py | 23 ++++- dimos/robot/unitree_webrtc/unitree_g1.py | 95 ++++++++++++------- .../web/websocket_vis/websocket_vis_module.py | 10 +- 3 files changed, 90 insertions(+), 38 deletions(-) diff --git a/dimos/robot/unitree_webrtc/connection.py b/dimos/robot/unitree_webrtc/connection.py index faad61833d..d05676d3e4 100644 --- a/dimos/robot/unitree_webrtc/connection.py +++ b/dimos/robot/unitree_webrtc/connection.py @@ -73,6 +73,8 @@ class UnitreeWebRTCConnection: def __init__(self, ip: str, mode: str = "ai"): self.ip = ip self.mode = mode + self.stop_timer = None + self.cmd_vel_timeout = 0.2 self.conn = Go2WebRTCConnection(WebRTCConnectionMethod.LocalSTA, ip=self.ip) self.connect() @@ -127,7 +129,7 @@ def move(self, twist: Twist, duration: float = 0.0) -> bool: async def async_move(): self.conn.datachannel.pub_sub.publish_without_callback( RTC_TOPIC["WIRELESS_CONTROLLER"], - data={"lx": y, "ly": x, "rx": -yaw, "ry": 0}, + data={"lx": -y, "ly": x, "rx": -yaw, "ry": 0}, ) async def async_move_duration(): @@ -139,6 +141,15 @@ async def async_move_duration(): await async_move() await asyncio.sleep(sleep_time) + # Cancel existing timer and start a new one + if self.stop_timer: + self.stop_timer.cancel() + + # Auto-stop after 0.5 seconds if no new commands + self.stop_timer = threading.Timer(self.cmd_vel_timeout, self.stop) + self.stop_timer.daemon = True + self.stop_timer.start() + try: if duration > 0: # Send continuous move commands for the duration @@ -323,10 +334,20 @@ def stop(self) -> bool: Returns: bool: True if stop command was sent successfully """ + # Cancel timer since we're explicitly stopping + if self.stop_timer: + self.stop_timer.cancel() + self.stop_timer = None + return self.move(Twist()) def disconnect(self) -> None: """Disconnect from the robot and clean up resources.""" + # Cancel timer + if self.stop_timer: + self.stop_timer.cancel() + self.stop_timer = None + if hasattr(self, "task") and self.task: self.task.cancel() if hasattr(self, "conn"): diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 2049d41c7c..0dcd129455 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -14,8 +14,8 @@ # limitations under the License. """ -Unitree G1 humanoid robot with ZED camera integration. -Minimal implementation using WebRTC connection for robot control and ZED for vision. +Unitree G1 humanoid robot. +Minimal implementation using WebRTC connection for robot control. """ import os @@ -25,18 +25,19 @@ from dimos import core from dimos.core import Module, In, Out, rpc -from dimos.hardware.zed_camera import ZEDModule from dimos.msgs.geometry_msgs import PoseStamped, Transform, Twist, Vector3, Quaternion -from dimos.msgs.sensor_msgs import Image -from dimos_lcm.sensor_msgs import CameraInfo from dimos.protocol import pubsub from dimos.protocol.pubsub.lcmpubsub import LCM from dimos.protocol.tf import TF from dimos.robot.foxglove_bridge import FoxgloveBridge +from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills +from dimos.robot.ros_bridge import ROSBridge, BridgeDirection +from geometry_msgs.msg import Twist as ROSTwist from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot +from dimos.hardware.zed_camera import ZEDModule from dimos.types.robot_capabilities import RobotCapability from dimos.utils.logging_config import setup_logger @@ -51,7 +52,7 @@ class G1ConnectionModule(Module): - """Simplified connection module for G1 - uses WebRTC for control, no video.""" + """Simplified connection module for G1 - uses WebRTC for control.""" movecmd: In[Twist] = None odom: Out[PoseStamped] = None @@ -83,16 +84,6 @@ def _publish_tf(self, msg): self.odom.publish(msg) self.tf.publish(Transform.from_pose("base_link", msg)) - # Publish ZED camera transform relative to robot base - zed_transform = Transform( - translation=Vector3(0.0, 0.0, 1.5), # ZED mounted at ~1.5m height on G1 - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="zed_camera", - ts=time.time(), - ) - self.tf.publish(zed_transform) - @rpc def get_odom(self) -> Optional[PoseStamped]: """Get the robot's odometry.""" @@ -120,12 +111,13 @@ def publish_request(self, topic: str, data: dict): class UnitreeG1(Robot): - """Unitree G1 humanoid robot with ZED camera for vision.""" + """Unitree G1 humanoid robot.""" def __init__( self, ip: str, output_dir: str = None, + websocket_port: int = 7779, skill_library: Optional[SkillLibrary] = None, recording_path: str = None, replay_path: str = None, @@ -136,6 +128,7 @@ def __init__( Args: ip: Robot IP address output_dir: Directory for saving outputs + websocket_port: Port for web visualization skill_library: Skill library instance recording_path: Path to save recordings (if recording) replay_path: Path to replay recordings from (if replaying) @@ -147,6 +140,7 @@ def __init__( self.recording_path = recording_path self.replay_path = replay_path self.enable_joystick = enable_joystick + self.websocket_port = websocket_port self.lcm = LCM() # Initialize skill library with G1 robot type @@ -157,14 +151,15 @@ def __init__( self.skill_library = skill_library # Set robot capabilities - self.capabilities = [RobotCapability.LOCOMOTION, RobotCapability.VISION] + self.capabilities = [RobotCapability.LOCOMOTION] # Module references self.dimos = None self.connection = None - self.zed_camera = None + self.websocket_vis = None self.foxglove_bridge = None self.joystick = None + self.ros_bridge = None self._setup_directories() @@ -175,30 +170,28 @@ def _setup_directories(self): def start(self): """Start the robot system with all modules.""" - self.dimos = core.start( - 3 if self.enable_joystick else 2 - ) # Extra worker for joystick if enabled + self.dimos = core.start(4) # 2 workers for connection and visualization self._deploy_connection() - self._deploy_camera() self._deploy_visualization() if self.enable_joystick: self._deploy_joystick() + self._deploy_ros_bridge() self._start_modules() self.lcm.start() logger.info("UnitreeG1 initialized and started") - logger.info("ZED camera module deployed for vision") + logger.info(f"WebSocket visualization available at http://localhost:{self.websocket_port}") def _deploy_connection(self): """Deploy and configure the connection module.""" self.connection = self.dimos.deploy(G1ConnectionModule, self.ip) # Configure LCM transports - self.connection.odom.transport = core.LCMTransport("/g1/odom", PoseStamped) + self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) # Use standard /cmd_vel topic for compatibility with joystick and navigation self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) @@ -241,7 +234,15 @@ def _deploy_camera(self): logger.info("ZED camera module configured") def _deploy_visualization(self): - """Deploy visualization tools.""" + """Deploy and configure visualization modules.""" + # Deploy WebSocket visualization module + self.websocket_vis = self.dimos.deploy(WebsocketVisModule, port=self.websocket_port) + self.websocket_vis.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) + + # Connect to robot pose + self.websocket_vis.robot_pose.connect(self.connection.odom) + + # Deploy Foxglove bridge self.foxglove_bridge = FoxgloveBridge() def _deploy_joystick(self): @@ -253,10 +254,21 @@ def _deploy_joystick(self): self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist) logger.info("Joystick module deployed - pygame window will open") + def _deploy_ros_bridge(self): + """Deploy and configure ROS bridge.""" + self.ros_bridge = ROSBridge("g1_ros_bridge") + + # Add /cmd_vel topic from ROS to DIMOS + self.ros_bridge.add_topic( + "/cmd_vel", Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS + ) + + logger.info("ROS bridge deployed: /cmd_vel (ROS → DIMOS)") + def _start_modules(self): """Start all deployed modules.""" self.connection.start() - self.zed_camera.start() + self.websocket_vis.start() self.foxglove_bridge.start() if self.joystick: @@ -272,13 +284,6 @@ def _start_modules(self): self.skill_library.init() self.skill_library.initialize_skills() - def get_single_rgb_frame(self, timeout: float = 2.0) -> Image: - """Get a single RGB frame from the ZED camera.""" - from dimos.protocol.pubsub.lcmpubsub import Topic - - topic = Topic("/zed/color_image", Image) - return self.lcm.wait_for_message(topic, timeout=timeout) - def move(self, twist: Twist, duration: float = 0.0): """Send movement command to robot.""" self.connection.move(twist, duration) @@ -295,6 +300,27 @@ def liedown(self): """Make the robot lie down.""" return self.connection.liedown() + def shutdown(self): + """Shutdown the robot and clean up resources.""" + logger.info("Shutting down UnitreeG1...") + + # Shutdown ROS bridge if it exists + if self.ros_bridge is not None: + try: + self.ros_bridge.shutdown() + logger.info("ROS bridge shut down successfully") + except Exception as e: + logger.error(f"Error shutting down ROS bridge: {e}") + + # Stop other modules if needed + if self.websocket_vis: + try: + self.websocket_vis.stop() + except Exception as e: + logger.error(f"Error stopping websocket vis: {e}") + + logger.info("UnitreeG1 shutdown complete") + def main(): """Main entry point for testing.""" @@ -346,6 +372,7 @@ def main(): time.sleep(1) except KeyboardInterrupt: logger.info("Shutting down...") + robot.shutdown() if __name__ == "__main__": diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index 25141a85ec..f127486e55 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -111,9 +111,13 @@ def start(self): self.server_thread = threading.Thread(target=self._run_server, daemon=True) self.server_thread.start() - self.robot_pose.subscribe(self._on_robot_pose) - self.path.subscribe(self._on_path) - self.global_costmap.subscribe(self._on_global_costmap) + # Only subscribe to connected topics + if self.robot_pose.connection is not None: + self.robot_pose.subscribe(self._on_robot_pose) + if self.path.connection is not None: + self.path.subscribe(self._on_path) + if self.global_costmap.connection is not None: + self.global_costmap.subscribe(self._on_global_costmap) logger.info(f"WebSocket server started on http://localhost:{self.port}") From 37037826f833a98de05e9f2471ffa991e45edc7d Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 7 Sep 2025 14:34:20 -0700 Subject: [PATCH 03/21] added twist stamped --- dimos/msgs/geometry_msgs/TwistStamped.py | 116 ++++++++++++++ dimos/msgs/geometry_msgs/__init__.py | 1 + dimos/msgs/geometry_msgs/test_TwistStamped.py | 151 ++++++++++++++++++ 3 files changed, 268 insertions(+) create mode 100644 dimos/msgs/geometry_msgs/TwistStamped.py create mode 100644 dimos/msgs/geometry_msgs/test_TwistStamped.py diff --git a/dimos/msgs/geometry_msgs/TwistStamped.py b/dimos/msgs/geometry_msgs/TwistStamped.py new file mode 100644 index 0000000000..0afa7e662f --- /dev/null +++ b/dimos/msgs/geometry_msgs/TwistStamped.py @@ -0,0 +1,116 @@ +# 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 struct +import time +from io import BytesIO +from typing import BinaryIO, TypeAlias + +from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped +from dimos_lcm.std_msgs import Header as LCMHeader +from dimos_lcm.std_msgs import Time as LCMTime +from geometry_msgs.msg import TwistStamped as ROSTwistStamped +from plum import dispatch + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from TwistStamped +TwistConvertable: TypeAlias = ( + tuple[VectorConvertable, VectorConvertable] | LCMTwistStamped | dict[str, VectorConvertable] +) + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class TwistStamped(Twist, Timestamped): + msg_name = "geometry_msgs.TwistStamped" + ts: float + frame_id: str + + @dispatch + def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None: + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + super().__init__(**kwargs) + + def lcm_encode(self) -> bytes: + lcm_msg = LCMTwistStamped() + lcm_msg.twist = self + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes | BinaryIO) -> TwistStamped: + lcm_msg = LCMTwistStamped.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, + linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], + angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], + ) + + def __str__(self) -> str: + return ( + f"TwistStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " + f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}])" + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSTwistStamped) -> "TwistStamped": + """Create a TwistStamped from a ROS geometry_msgs/TwistStamped message. + + Args: + ros_msg: ROS TwistStamped message + + Returns: + TwistStamped instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert twist + twist = Twist.from_ros_msg(ros_msg.twist) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + linear=twist.linear, + angular=twist.angular, + ) + + def to_ros_msg(self) -> ROSTwistStamped: + """Convert to a ROS geometry_msgs/TwistStamped message. + + Returns: + ROS TwistStamped message + """ + ros_msg = ROSTwistStamped() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set twist + ros_msg.twist = Twist.to_ros_msg(self) + + return ros_msg diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index 86d25bb843..137b113a4d 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -3,4 +3,5 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike diff --git a/dimos/msgs/geometry_msgs/test_TwistStamped.py b/dimos/msgs/geometry_msgs/test_TwistStamped.py new file mode 100644 index 0000000000..c84cba8cf2 --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_TwistStamped.py @@ -0,0 +1,151 @@ +# Copyright 2025 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pickle +import time + +from geometry_msgs.msg import TwistStamped as ROSTwistStamped + +from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped + + +def test_lcm_encode_decode(): + """Test encoding and decoding of TwistStamped to/from binary LCM format.""" + + twist_source = TwistStamped( + ts=time.time(), + linear=(1.0, 2.0, 3.0), + angular=(0.1, 0.2, 0.3), + ) + binary_msg = twist_source.lcm_encode() + twist_dest = TwistStamped.lcm_decode(binary_msg) + + assert isinstance(twist_dest, TwistStamped) + assert twist_dest is not twist_source + + print(twist_source.linear) + print(twist_source.angular) + + print(twist_dest.linear) + print(twist_dest.angular) + assert twist_dest == twist_source + + +def test_pickle_encode_decode(): + """Test encoding and decoding of TwistStamped to/from binary pickle format.""" + + twist_source = TwistStamped( + ts=time.time(), + linear=(1.0, 2.0, 3.0), + angular=(0.1, 0.2, 0.3), + ) + binary_msg = pickle.dumps(twist_source) + twist_dest = pickle.loads(binary_msg) + assert isinstance(twist_dest, TwistStamped) + assert twist_dest is not twist_source + assert twist_dest == twist_source + + +def test_twist_stamped_from_ros_msg(): + """Test creating a TwistStamped from a ROS TwistStamped message.""" + ros_msg = ROSTwistStamped() + ros_msg.header.frame_id = "world" + ros_msg.header.stamp.sec = 123 + ros_msg.header.stamp.nanosec = 456000000 + ros_msg.twist.linear.x = 1.0 + ros_msg.twist.linear.y = 2.0 + ros_msg.twist.linear.z = 3.0 + ros_msg.twist.angular.x = 0.1 + ros_msg.twist.angular.y = 0.2 + ros_msg.twist.angular.z = 0.3 + + twist_stamped = TwistStamped.from_ros_msg(ros_msg) + + assert twist_stamped.frame_id == "world" + assert twist_stamped.ts == 123.456 + assert twist_stamped.linear.x == 1.0 + assert twist_stamped.linear.y == 2.0 + assert twist_stamped.linear.z == 3.0 + assert twist_stamped.angular.x == 0.1 + assert twist_stamped.angular.y == 0.2 + assert twist_stamped.angular.z == 0.3 + + +def test_twist_stamped_to_ros_msg(): + """Test converting a TwistStamped to a ROS TwistStamped message.""" + twist_stamped = TwistStamped( + ts=123.456, + frame_id="base_link", + linear=(1.0, 2.0, 3.0), + angular=(0.1, 0.2, 0.3), + ) + + ros_msg = twist_stamped.to_ros_msg() + + assert isinstance(ros_msg, ROSTwistStamped) + assert ros_msg.header.frame_id == "base_link" + assert ros_msg.header.stamp.sec == 123 + assert ros_msg.header.stamp.nanosec == 456000000 + assert ros_msg.twist.linear.x == 1.0 + assert ros_msg.twist.linear.y == 2.0 + assert ros_msg.twist.linear.z == 3.0 + assert ros_msg.twist.angular.x == 0.1 + assert ros_msg.twist.angular.y == 0.2 + assert ros_msg.twist.angular.z == 0.3 + + +def test_twist_stamped_ros_roundtrip(): + """Test round-trip conversion between TwistStamped and ROS TwistStamped.""" + original = TwistStamped( + ts=123.789, + frame_id="odom", + linear=(1.5, 2.5, 3.5), + angular=(0.15, 0.25, 0.35), + ) + + ros_msg = original.to_ros_msg() + restored = TwistStamped.from_ros_msg(ros_msg) + + assert restored.frame_id == original.frame_id + assert restored.ts == original.ts + assert restored.linear.x == original.linear.x + assert restored.linear.y == original.linear.y + assert restored.linear.z == original.linear.z + assert restored.angular.x == original.angular.x + assert restored.angular.y == original.angular.y + assert restored.angular.z == original.angular.z + + +if __name__ == "__main__": + print("Running test_lcm_encode_decode...") + test_lcm_encode_decode() + print("✓ test_lcm_encode_decode passed") + + print("Running test_pickle_encode_decode...") + test_pickle_encode_decode() + print("✓ test_pickle_encode_decode passed") + + print("Running test_twist_stamped_from_ros_msg...") + test_twist_stamped_from_ros_msg() + print("✓ test_twist_stamped_from_ros_msg passed") + + print("Running test_twist_stamped_to_ros_msg...") + test_twist_stamped_to_ros_msg() + print("✓ test_twist_stamped_to_ros_msg passed") + + print("Running test_twist_stamped_ros_roundtrip...") + test_twist_stamped_ros_roundtrip() + print("✓ test_twist_stamped_ros_roundtrip passed") + + print("\nAll tests passed!") From ec3f3efbac1fad2606d72bb16c3675050c07118d Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sun, 7 Sep 2025 14:54:11 -0700 Subject: [PATCH 04/21] use twist stamped to match ros nav stack --- dimos/robot/unitree_webrtc/unitree_g1.py | 28 +++++++---------- .../web/websocket_vis/websocket_vis_module.py | 30 +++++++++++++++---- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 0dcd129455..b108661878 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -25,7 +25,7 @@ from dimos import core from dimos.core import Module, In, Out, rpc -from dimos.msgs.geometry_msgs import PoseStamped, Transform, Twist, Vector3, Quaternion +from dimos.msgs.geometry_msgs import PoseStamped, Transform, Twist, TwistStamped from dimos.protocol import pubsub from dimos.protocol.pubsub.lcmpubsub import LCM from dimos.protocol.tf import TF @@ -34,7 +34,7 @@ from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills from dimos.robot.ros_bridge import ROSBridge, BridgeDirection -from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import TwistStamped as ROSTwistStamped from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot from dimos.hardware.zed_camera import ZEDModule @@ -54,7 +54,7 @@ class G1ConnectionModule(Module): """Simplified connection module for G1 - uses WebRTC for control.""" - movecmd: In[Twist] = None + movecmd: In[TwistStamped] = None odom: Out[PoseStamped] = None ip: str connection_type: str = "webrtc" @@ -90,8 +90,9 @@ def get_odom(self) -> Optional[PoseStamped]: return self._odom @rpc - def move(self, twist: Twist, duration: float = 0.0): + def move(self, twist_stamped: TwistStamped, duration: float = 0.0): """Send movement command to robot.""" + twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular) self.connection.move(twist, duration) @rpc @@ -192,8 +193,7 @@ def _deploy_connection(self): # Configure LCM transports self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) - # Use standard /cmd_vel topic for compatibility with joystick and navigation - self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) + self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped) def _deploy_camera(self): """Deploy and configure the ZED camera module (real or fake based on replay_path).""" @@ -237,7 +237,7 @@ def _deploy_visualization(self): """Deploy and configure visualization modules.""" # Deploy WebSocket visualization module self.websocket_vis = self.dimos.deploy(WebsocketVisModule, port=self.websocket_port) - self.websocket_vis.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) + self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) # Connect to robot pose self.websocket_vis.robot_pose.connect(self.connection.odom) @@ -260,7 +260,7 @@ def _deploy_ros_bridge(self): # Add /cmd_vel topic from ROS to DIMOS self.ros_bridge.add_topic( - "/cmd_vel", Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS + "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS ) logger.info("ROS bridge deployed: /cmd_vel (ROS → DIMOS)") @@ -284,22 +284,14 @@ def _start_modules(self): self.skill_library.init() self.skill_library.initialize_skills() - def move(self, twist: Twist, duration: float = 0.0): + def move(self, twist_stamped: TwistStamped, duration: float = 0.0): """Send movement command to robot.""" - self.connection.move(twist, duration) + self.connection.move(twist_stamped, duration) def get_odom(self) -> PoseStamped: """Get the robot's odometry.""" return self.connection.get_odom() - def standup(self): - """Make the robot stand up.""" - return self.connection.standup() - - def liedown(self): - """Make the robot lie down.""" - return self.connection.liedown() - def shutdown(self): """Shutdown the robot and clean up resources.""" logger.info("Shutting down UnitreeG1...") diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index f127486e55..760edc60e6 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -20,6 +20,7 @@ import asyncio import threading +import time from typing import Any, Dict, Optional import base64 import numpy as np @@ -32,7 +33,7 @@ from dimos.core import Module, In, Out, rpc from dimos_lcm.std_msgs import Bool -from dimos.msgs.geometry_msgs import PoseStamped, Twist, Vector3 +from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped, Vector3 from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.utils.logging_config import setup_logger @@ -68,6 +69,7 @@ class WebsocketVisModule(Module): explore_cmd: Out[Bool] = None stop_explore_cmd: Out[Bool] = None movecmd: Out[Twist] = None + movecmd_stamped: Out[TwistStamped] = None def __init__(self, port: int = 7779, **kwargs): """Initialize the WebSocket visualization module. @@ -171,11 +173,27 @@ async def stop_explore(sid): @self.sio.event async def move_command(sid, data): - twist = Twist( - linear=Vector3(data["linear"]["x"], data["linear"]["y"], data["linear"]["z"]), - angular=Vector3(data["angular"]["x"], data["angular"]["y"], data["angular"]["z"]), - ) - self.movecmd.publish(twist) + # Publish Twist if transport is configured + if self.movecmd and self.movecmd.transport: + twist = Twist( + linear=Vector3(data["linear"]["x"], data["linear"]["y"], data["linear"]["z"]), + angular=Vector3( + data["angular"]["x"], data["angular"]["y"], data["angular"]["z"] + ), + ) + self.movecmd.publish(twist) + + # Publish TwistStamped if transport is configured + if self.movecmd_stamped and self.movecmd_stamped.transport: + twist_stamped = TwistStamped( + ts=time.time(), + frame_id="base_link", + linear=Vector3(data["linear"]["x"], data["linear"]["y"], data["linear"]["z"]), + angular=Vector3( + data["angular"]["x"], data["angular"]["y"], data["angular"]["z"] + ), + ) + self.movecmd_stamped.publish(twist_stamped) def _run_server(self): uvicorn.run( From d8ebddd6f49c0d522c94c57467f2118f6fac1ed1 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Thu, 11 Sep 2025 10:01:14 -0700 Subject: [PATCH 05/21] added twist with covariance and odometry messages --- .../msgs/geometry_msgs/PoseWithCovariance.py | 219 ++++++++ .../PoseWithCovarianceStamped.py | 155 ++++++ .../msgs/geometry_msgs/TwistWithCovariance.py | 219 ++++++++ .../TwistWithCovarianceStamped.py | 163 ++++++ .../geometry_msgs/test_PoseWithCovariance.py | 378 ++++++++++++++ .../test_PoseWithCovarianceStamped.py | 342 +++++++++++++ .../geometry_msgs/test_TwistWithCovariance.py | 407 +++++++++++++++ .../test_TwistWithCovarianceStamped.py | 367 ++++++++++++++ dimos/msgs/nav_msgs/Odometry.py | 373 ++++++++++++++ dimos/msgs/nav_msgs/test_Odometry.py | 466 ++++++++++++++++++ 10 files changed, 3089 insertions(+) create mode 100644 dimos/msgs/geometry_msgs/PoseWithCovariance.py create mode 100644 dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py create mode 100644 dimos/msgs/geometry_msgs/TwistWithCovariance.py create mode 100644 dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py create mode 100644 dimos/msgs/geometry_msgs/test_PoseWithCovariance.py create mode 100644 dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py create mode 100644 dimos/msgs/geometry_msgs/test_TwistWithCovariance.py create mode 100644 dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py create mode 100644 dimos/msgs/nav_msgs/Odometry.py create mode 100644 dimos/msgs/nav_msgs/test_Odometry.py diff --git a/dimos/msgs/geometry_msgs/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py new file mode 100644 index 0000000000..e4c93cede9 --- /dev/null +++ b/dimos/msgs/geometry_msgs/PoseWithCovariance.py @@ -0,0 +1,219 @@ +# 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 TypeAlias + +import numpy as np +from dimos_lcm.geometry_msgs import PoseWithCovariance as LCMPoseWithCovariance +from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance +from plum import dispatch + +from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +# Types that can be converted to/from PoseWithCovariance +PoseWithCovarianceConvertable: TypeAlias = ( + tuple[PoseConvertable, list[float] | np.ndarray] + | LCMPoseWithCovariance + | dict[str, PoseConvertable | list[float] | np.ndarray] +) + + +class PoseWithCovariance(LCMPoseWithCovariance): + pose: Pose + msg_name = "geometry_msgs.PoseWithCovariance" + + @dispatch + def __init__(self) -> None: + """Initialize with default pose and zero covariance.""" + self.pose = Pose() + self.covariance = np.zeros(36) + + @dispatch + def __init__( + self, pose: Pose | PoseConvertable, covariance: list[float] | np.ndarray | None = None + ) -> None: + """Initialize with pose and optional covariance.""" + self.pose = Pose(pose) if not isinstance(pose, Pose) else pose + if covariance is None: + self.covariance = np.zeros(36) + else: + self.covariance = np.array(covariance, dtype=float).reshape(36) + + @dispatch + def __init__(self, pose_with_cov: PoseWithCovariance) -> None: + """Initialize from another PoseWithCovariance (copy constructor).""" + self.pose = Pose(pose_with_cov.pose) + self.covariance = np.array(pose_with_cov.covariance).copy() + + @dispatch + def __init__(self, lcm_pose_with_cov: LCMPoseWithCovariance) -> None: + """Initialize from an LCM PoseWithCovariance.""" + self.pose = Pose(lcm_pose_with_cov.pose) + self.covariance = np.array(lcm_pose_with_cov.covariance) + + @dispatch + def __init__(self, pose_dict: dict[str, PoseConvertable | list[float] | np.ndarray]) -> None: + """Initialize from a dictionary with 'pose' and 'covariance' keys.""" + self.pose = Pose(pose_dict["pose"]) + covariance = pose_dict.get("covariance") + if covariance is None: + self.covariance = np.zeros(36) + else: + self.covariance = np.array(covariance, dtype=float).reshape(36) + + @dispatch + def __init__(self, pose_tuple: tuple[PoseConvertable, list[float] | np.ndarray]) -> None: + """Initialize from a tuple of (pose, covariance).""" + self.pose = Pose(pose_tuple[0]) + self.covariance = np.array(pose_tuple[1], dtype=float).reshape(36) + + def __getattribute__(self, name): + """Override to ensure covariance is always returned as numpy array.""" + if name == "covariance": + cov = object.__getattribute__(self, "covariance") + if not isinstance(cov, np.ndarray): + return np.array(cov, dtype=float) + return cov + return super().__getattribute__(name) + + def __setattr__(self, name, value): + """Override to ensure covariance is stored as numpy array.""" + if name == "covariance": + if not isinstance(value, np.ndarray): + value = np.array(value, dtype=float).reshape(36) + super().__setattr__(name, value) + + @property + def x(self) -> float: + """X coordinate of position.""" + return self.pose.x + + @property + def y(self) -> float: + """Y coordinate of position.""" + return self.pose.y + + @property + def z(self) -> float: + """Z coordinate of position.""" + return self.pose.z + + @property + def position(self) -> Vector3: + """Position vector.""" + return self.pose.position + + @property + def orientation(self) -> Quaternion: + """Orientation quaternion.""" + return self.pose.orientation + + @property + def roll(self) -> float: + """Roll angle in radians.""" + return self.pose.roll + + @property + def pitch(self) -> float: + """Pitch angle in radians.""" + return self.pose.pitch + + @property + def yaw(self) -> float: + """Yaw angle in radians.""" + return self.pose.yaw + + @property + def covariance_matrix(self) -> np.ndarray: + """Get covariance as 6x6 matrix.""" + return self.covariance.reshape(6, 6) + + @covariance_matrix.setter + def covariance_matrix(self, value: np.ndarray) -> None: + """Set covariance from 6x6 matrix.""" + self.covariance = np.array(value).reshape(36) + + def __repr__(self) -> str: + return f"PoseWithCovariance(pose={self.pose!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" + + def __str__(self) -> str: + return ( + f"PoseWithCovariance(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " + f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " + f"cov_trace={np.trace(self.covariance_matrix):.3f})" + ) + + def __eq__(self, other) -> bool: + """Check if two PoseWithCovariance are equal.""" + if not isinstance(other, PoseWithCovariance): + return False + return self.pose == other.pose and np.allclose(self.covariance, other.covariance) + + def lcm_encode(self) -> bytes: + """Encode to LCM binary format.""" + lcm_msg = LCMPoseWithCovariance() + lcm_msg.pose = self.pose + # LCM expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + lcm_msg.covariance = self.covariance.tolist() + else: + lcm_msg.covariance = list(self.covariance) + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> "PoseWithCovariance": + """Decode from LCM binary format.""" + lcm_msg = LCMPoseWithCovariance.lcm_decode(data) + pose = Pose( + position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z], + orientation=[ + lcm_msg.pose.orientation.x, + lcm_msg.pose.orientation.y, + lcm_msg.pose.orientation.z, + lcm_msg.pose.orientation.w, + ], + ) + return cls(pose, lcm_msg.covariance) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSPoseWithCovariance) -> "PoseWithCovariance": + """Create a PoseWithCovariance from a ROS geometry_msgs/PoseWithCovariance message. + + Args: + ros_msg: ROS PoseWithCovariance message + + Returns: + PoseWithCovariance instance + """ + pose = Pose.from_ros_msg(ros_msg.pose) + return cls(pose, list(ros_msg.covariance)) + + def to_ros_msg(self) -> ROSPoseWithCovariance: + """Convert to a ROS geometry_msgs/PoseWithCovariance message. + + Returns: + ROS PoseWithCovariance message + """ + ros_msg = ROSPoseWithCovariance() + ros_msg.pose = self.pose.to_ros_msg() + # ROS expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + ros_msg.covariance = self.covariance.tolist() + else: + ros_msg.covariance = list(self.covariance) + return ros_msg diff --git a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py new file mode 100644 index 0000000000..9f48d8e2dc --- /dev/null +++ b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py @@ -0,0 +1,155 @@ +# 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 typing import TypeAlias + +import numpy as np +from dimos_lcm.geometry_msgs import PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped +from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped +from plum import dispatch + +from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from PoseWithCovarianceStamped +PoseWithCovarianceStampedConvertable: TypeAlias = ( + tuple[PoseConvertable, list[float] | np.ndarray] + | LCMPoseWithCovarianceStamped + | dict[str, PoseConvertable | list[float] | np.ndarray | float | str] +) + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class PoseWithCovarianceStamped(PoseWithCovariance, Timestamped): + msg_name = "geometry_msgs.PoseWithCovarianceStamped" + ts: float + frame_id: str + + @dispatch + def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None: + """Initialize with timestamp and frame_id.""" + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + super().__init__(**kwargs) + + @dispatch + def __init__( + self, + ts: float = 0.0, + frame_id: str = "", + pose: Pose | PoseConvertable | None = None, + covariance: list[float] | np.ndarray | None = None, + ) -> None: + """Initialize with timestamp, frame_id, pose and covariance.""" + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + if pose is None: + super().__init__() + else: + super().__init__(pose, covariance) + + def lcm_encode(self) -> bytes: + lcm_msg = LCMPoseWithCovarianceStamped() + lcm_msg.pose.pose = self.pose + # LCM expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + lcm_msg.pose.covariance = self.covariance.tolist() + else: + lcm_msg.pose.covariance = list(self.covariance) + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> PoseWithCovarianceStamped: + lcm_msg = LCMPoseWithCovarianceStamped.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, + pose=Pose( + position=[ + lcm_msg.pose.pose.position.x, + lcm_msg.pose.pose.position.y, + lcm_msg.pose.pose.position.z, + ], + orientation=[ + lcm_msg.pose.pose.orientation.x, + lcm_msg.pose.pose.orientation.y, + lcm_msg.pose.pose.orientation.z, + lcm_msg.pose.pose.orientation.w, + ], + ), + covariance=lcm_msg.pose.covariance, + ) + + def __str__(self) -> str: + return ( + f"PoseWithCovarianceStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " + f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " + f"cov_trace={np.trace(self.covariance_matrix):.3f})" + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSPoseWithCovarianceStamped) -> "PoseWithCovarianceStamped": + """Create a PoseWithCovarianceStamped from a ROS geometry_msgs/PoseWithCovarianceStamped message. + + Args: + ros_msg: ROS PoseWithCovarianceStamped message + + Returns: + PoseWithCovarianceStamped instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert pose with covariance + pose_with_cov = PoseWithCovariance.from_ros_msg(ros_msg.pose) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + pose=pose_with_cov.pose, + covariance=pose_with_cov.covariance, + ) + + def to_ros_msg(self) -> ROSPoseWithCovarianceStamped: + """Convert to a ROS geometry_msgs/PoseWithCovarianceStamped message. + + Returns: + ROS PoseWithCovarianceStamped message + """ + ros_msg = ROSPoseWithCovarianceStamped() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set pose with covariance + ros_msg.pose.pose = self.pose.to_ros_msg() + # ROS expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + ros_msg.pose.covariance = self.covariance.tolist() + else: + ros_msg.pose.covariance = list(self.covariance) + + return ros_msg diff --git a/dimos/msgs/geometry_msgs/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py new file mode 100644 index 0000000000..81be1c3874 --- /dev/null +++ b/dimos/msgs/geometry_msgs/TwistWithCovariance.py @@ -0,0 +1,219 @@ +# 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 TypeAlias + +import numpy as np +from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance +from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance +from plum import dispatch + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable + +# Types that can be converted to/from TwistWithCovariance +TwistWithCovarianceConvertable: TypeAlias = ( + tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] + | LCMTwistWithCovariance + | dict[str, Twist | tuple[VectorConvertable, VectorConvertable] | list[float] | np.ndarray] +) + + +class TwistWithCovariance(LCMTwistWithCovariance): + twist: Twist + msg_name = "geometry_msgs.TwistWithCovariance" + + @dispatch + def __init__(self) -> None: + """Initialize with default twist and zero covariance.""" + self.twist = Twist() + self.covariance = np.zeros(36) + + @dispatch + def __init__( + self, + twist: Twist | tuple[VectorConvertable, VectorConvertable], + covariance: list[float] | np.ndarray | None = None, + ) -> None: + """Initialize with twist and optional covariance.""" + if isinstance(twist, Twist): + self.twist = twist + else: + # Assume it's a tuple of (linear, angular) + self.twist = Twist(twist[0], twist[1]) + + if covariance is None: + self.covariance = np.zeros(36) + else: + self.covariance = np.array(covariance, dtype=float).reshape(36) + + @dispatch + def __init__(self, twist_with_cov: TwistWithCovariance) -> None: + """Initialize from another TwistWithCovariance (copy constructor).""" + self.twist = Twist(twist_with_cov.twist) + self.covariance = np.array(twist_with_cov.covariance).copy() + + @dispatch + def __init__(self, lcm_twist_with_cov: LCMTwistWithCovariance) -> None: + """Initialize from an LCM TwistWithCovariance.""" + self.twist = Twist(lcm_twist_with_cov.twist) + self.covariance = np.array(lcm_twist_with_cov.covariance) + + @dispatch + def __init__( + self, + twist_dict: dict[ + str, Twist | tuple[VectorConvertable, VectorConvertable] | list[float] | np.ndarray + ], + ) -> None: + """Initialize from a dictionary with 'twist' and 'covariance' keys.""" + twist = twist_dict["twist"] + if isinstance(twist, Twist): + self.twist = twist + else: + # Assume it's a tuple of (linear, angular) + self.twist = Twist(twist[0], twist[1]) + + covariance = twist_dict.get("covariance") + if covariance is None: + self.covariance = np.zeros(36) + else: + self.covariance = np.array(covariance, dtype=float).reshape(36) + + @dispatch + def __init__( + self, + twist_tuple: tuple[ + Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray + ], + ) -> None: + """Initialize from a tuple of (twist, covariance).""" + twist = twist_tuple[0] + if isinstance(twist, Twist): + self.twist = twist + else: + # Assume it's a tuple of (linear, angular) + self.twist = Twist(twist[0], twist[1]) + self.covariance = np.array(twist_tuple[1], dtype=float).reshape(36) + + def __getattribute__(self, name): + """Override to ensure covariance is always returned as numpy array.""" + if name == "covariance": + cov = object.__getattribute__(self, "covariance") + if not isinstance(cov, np.ndarray): + return np.array(cov, dtype=float) + return cov + return super().__getattribute__(name) + + def __setattr__(self, name, value): + """Override to ensure covariance is stored as numpy array.""" + if name == "covariance": + if not isinstance(value, np.ndarray): + value = np.array(value, dtype=float).reshape(36) + super().__setattr__(name, value) + + @property + def linear(self) -> Vector3: + """Linear velocity vector.""" + return self.twist.linear + + @property + def angular(self) -> Vector3: + """Angular velocity vector.""" + return self.twist.angular + + @property + def covariance_matrix(self) -> np.ndarray: + """Get covariance as 6x6 matrix.""" + return self.covariance.reshape(6, 6) + + @covariance_matrix.setter + def covariance_matrix(self, value: np.ndarray) -> None: + """Set covariance from 6x6 matrix.""" + self.covariance = np.array(value).reshape(36) + + def __repr__(self) -> str: + return f"TwistWithCovariance(twist={self.twist!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" + + def __str__(self) -> str: + return ( + f"TwistWithCovariance(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " + f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " + f"cov_trace={np.trace(self.covariance_matrix):.3f})" + ) + + def __eq__(self, other) -> bool: + """Check if two TwistWithCovariance are equal.""" + if not isinstance(other, TwistWithCovariance): + return False + return self.twist == other.twist and np.allclose(self.covariance, other.covariance) + + def is_zero(self) -> bool: + """Check if this is a zero twist (no linear or angular velocity).""" + return self.twist.is_zero() + + def __bool__(self) -> bool: + """Boolean conversion - False if zero twist, True otherwise.""" + return not self.is_zero() + + def lcm_encode(self) -> bytes: + """Encode to LCM binary format.""" + lcm_msg = LCMTwistWithCovariance() + lcm_msg.twist = self.twist + # LCM expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + lcm_msg.covariance = self.covariance.tolist() + else: + lcm_msg.covariance = list(self.covariance) + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> "TwistWithCovariance": + """Decode from LCM binary format.""" + lcm_msg = LCMTwistWithCovariance.lcm_decode(data) + twist = Twist( + linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], + angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], + ) + return cls(twist, lcm_msg.covariance) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSTwistWithCovariance) -> "TwistWithCovariance": + """Create a TwistWithCovariance from a ROS geometry_msgs/TwistWithCovariance message. + + Args: + ros_msg: ROS TwistWithCovariance message + + Returns: + TwistWithCovariance instance + """ + twist = Twist.from_ros_msg(ros_msg.twist) + return cls(twist, list(ros_msg.covariance)) + + def to_ros_msg(self) -> ROSTwistWithCovariance: + """Convert to a ROS geometry_msgs/TwistWithCovariance message. + + Returns: + ROS TwistWithCovariance message + """ + ros_msg = ROSTwistWithCovariance() + ros_msg.twist = self.twist.to_ros_msg() + # ROS expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + ros_msg.covariance = self.covariance.tolist() + else: + ros_msg.covariance = list(self.covariance) + return ros_msg diff --git a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py new file mode 100644 index 0000000000..f199eb60c9 --- /dev/null +++ b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py @@ -0,0 +1,163 @@ +# 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 typing import TypeAlias + +import numpy as np +from dimos_lcm.geometry_msgs import TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped +from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped +from plum import dispatch + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from TwistWithCovarianceStamped +TwistWithCovarianceStampedConvertable: TypeAlias = ( + tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] + | LCMTwistWithCovarianceStamped + | dict[ + str, + Twist + | tuple[VectorConvertable, VectorConvertable] + | list[float] + | np.ndarray + | float + | str, + ] +) + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class TwistWithCovarianceStamped(TwistWithCovariance, Timestamped): + msg_name = "geometry_msgs.TwistWithCovarianceStamped" + ts: float + frame_id: str + + @dispatch + def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None: + """Initialize with timestamp and frame_id.""" + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + super().__init__(**kwargs) + + @dispatch + def __init__( + self, + ts: float = 0.0, + frame_id: str = "", + twist: Twist | tuple[VectorConvertable, VectorConvertable] | None = None, + covariance: list[float] | np.ndarray | None = None, + ) -> None: + """Initialize with timestamp, frame_id, twist and covariance.""" + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + if twist is None: + super().__init__() + else: + super().__init__(twist, covariance) + + def lcm_encode(self) -> bytes: + lcm_msg = LCMTwistWithCovarianceStamped() + lcm_msg.twist.twist = self.twist + # LCM expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + lcm_msg.twist.covariance = self.covariance.tolist() + else: + lcm_msg.twist.covariance = list(self.covariance) + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> TwistWithCovarianceStamped: + lcm_msg = LCMTwistWithCovarianceStamped.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, + twist=Twist( + linear=[ + lcm_msg.twist.twist.linear.x, + lcm_msg.twist.twist.linear.y, + lcm_msg.twist.twist.linear.z, + ], + angular=[ + lcm_msg.twist.twist.angular.x, + lcm_msg.twist.twist.angular.y, + lcm_msg.twist.twist.angular.z, + ], + ), + covariance=lcm_msg.twist.covariance, + ) + + def __str__(self) -> str: + return ( + f"TwistWithCovarianceStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " + f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " + f"cov_trace={np.trace(self.covariance_matrix):.3f})" + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSTwistWithCovarianceStamped) -> "TwistWithCovarianceStamped": + """Create a TwistWithCovarianceStamped from a ROS geometry_msgs/TwistWithCovarianceStamped message. + + Args: + ros_msg: ROS TwistWithCovarianceStamped message + + Returns: + TwistWithCovarianceStamped instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert twist with covariance + twist_with_cov = TwistWithCovariance.from_ros_msg(ros_msg.twist) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + twist=twist_with_cov.twist, + covariance=twist_with_cov.covariance, + ) + + def to_ros_msg(self) -> ROSTwistWithCovarianceStamped: + """Convert to a ROS geometry_msgs/TwistWithCovarianceStamped message. + + Returns: + ROS TwistWithCovarianceStamped message + """ + ros_msg = ROSTwistWithCovarianceStamped() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set twist with covariance + ros_msg.twist.twist = self.twist.to_ros_msg() + # ROS expects list, not numpy array + if isinstance(self.covariance, np.ndarray): + ros_msg.twist.covariance = self.covariance.tolist() + else: + ros_msg.twist.covariance = list(self.covariance) + + return ros_msg diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py new file mode 100644 index 0000000000..d35946cc4a --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py @@ -0,0 +1,378 @@ +# Copyright 2025 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +import pytest +from dimos_lcm.geometry_msgs import PoseWithCovariance as LCMPoseWithCovariance +from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +def test_pose_with_covariance_default_init(): + """Test that default initialization creates a pose at origin with zero covariance.""" + pose_cov = PoseWithCovariance() + + # Pose should be at origin with identity orientation + assert pose_cov.pose.position.x == 0.0 + assert pose_cov.pose.position.y == 0.0 + assert pose_cov.pose.position.z == 0.0 + assert pose_cov.pose.orientation.x == 0.0 + assert pose_cov.pose.orientation.y == 0.0 + assert pose_cov.pose.orientation.z == 0.0 + assert pose_cov.pose.orientation.w == 1.0 + + # Covariance should be all zeros + assert np.all(pose_cov.covariance == 0.0) + assert pose_cov.covariance.shape == (36,) + + +def test_pose_with_covariance_pose_init(): + """Test initialization with a Pose object.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + pose_cov = PoseWithCovariance(pose) + + # Pose should match + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + assert pose_cov.pose.orientation.x == 0.1 + assert pose_cov.pose.orientation.y == 0.2 + assert pose_cov.pose.orientation.z == 0.3 + assert pose_cov.pose.orientation.w == 0.9 + + # Covariance should be zeros by default + assert np.all(pose_cov.covariance == 0.0) + + +def test_pose_with_covariance_pose_and_covariance_init(): + """Test initialization with pose and covariance.""" + pose = Pose(1.0, 2.0, 3.0) + covariance = np.arange(36, dtype=float) + pose_cov = PoseWithCovariance(pose, covariance) + + # Pose should match + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + + # Covariance should match + assert np.array_equal(pose_cov.covariance, covariance) + + +def test_pose_with_covariance_list_covariance(): + """Test initialization with covariance as a list.""" + pose = Pose(1.0, 2.0, 3.0) + covariance_list = list(range(36)) + pose_cov = PoseWithCovariance(pose, covariance_list) + + # Covariance should be converted to numpy array + assert isinstance(pose_cov.covariance, np.ndarray) + assert np.array_equal(pose_cov.covariance, np.array(covariance_list)) + + +def test_pose_with_covariance_copy_init(): + """Test copy constructor.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + original = PoseWithCovariance(pose, covariance) + copy = PoseWithCovariance(original) + + # Should be equal but not the same object + assert copy == original + assert copy is not original + assert copy.pose is not original.pose + assert copy.covariance is not original.covariance + + # Modify original to ensure they're independent + original.covariance[0] = 999.0 + assert copy.covariance[0] != 999.0 + + +def test_pose_with_covariance_lcm_init(): + """Test initialization from LCM message.""" + lcm_msg = LCMPoseWithCovariance() + lcm_msg.pose.position.x = 1.0 + lcm_msg.pose.position.y = 2.0 + lcm_msg.pose.position.z = 3.0 + lcm_msg.pose.orientation.x = 0.1 + lcm_msg.pose.orientation.y = 0.2 + lcm_msg.pose.orientation.z = 0.3 + lcm_msg.pose.orientation.w = 0.9 + lcm_msg.covariance = list(range(36)) + + pose_cov = PoseWithCovariance(lcm_msg) + + # Pose should match + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + assert pose_cov.pose.orientation.x == 0.1 + assert pose_cov.pose.orientation.y == 0.2 + assert pose_cov.pose.orientation.z == 0.3 + assert pose_cov.pose.orientation.w == 0.9 + + # Covariance should match + assert np.array_equal(pose_cov.covariance, np.arange(36)) + + +def test_pose_with_covariance_dict_init(): + """Test initialization from dictionary.""" + pose_dict = {"pose": Pose(1.0, 2.0, 3.0), "covariance": list(range(36))} + pose_cov = PoseWithCovariance(pose_dict) + + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + assert np.array_equal(pose_cov.covariance, np.arange(36)) + + +def test_pose_with_covariance_dict_init_no_covariance(): + """Test initialization from dictionary without covariance.""" + pose_dict = {"pose": Pose(1.0, 2.0, 3.0)} + pose_cov = PoseWithCovariance(pose_dict) + + assert pose_cov.pose.position.x == 1.0 + assert np.all(pose_cov.covariance == 0.0) + + +def test_pose_with_covariance_tuple_init(): + """Test initialization from tuple.""" + pose = Pose(1.0, 2.0, 3.0) + covariance = np.arange(36, dtype=float) + pose_tuple = (pose, covariance) + pose_cov = PoseWithCovariance(pose_tuple) + + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + assert np.array_equal(pose_cov.covariance, covariance) + + +def test_pose_with_covariance_properties(): + """Test convenience properties.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + pose_cov = PoseWithCovariance(pose) + + # Position properties + assert pose_cov.x == 1.0 + assert pose_cov.y == 2.0 + assert pose_cov.z == 3.0 + assert pose_cov.position.x == 1.0 + assert pose_cov.position.y == 2.0 + assert pose_cov.position.z == 3.0 + + # Orientation properties + assert pose_cov.orientation.x == 0.1 + assert pose_cov.orientation.y == 0.2 + assert pose_cov.orientation.z == 0.3 + assert pose_cov.orientation.w == 0.9 + + # Euler angle properties + assert pose_cov.roll == pose.roll + assert pose_cov.pitch == pose.pitch + assert pose_cov.yaw == pose.yaw + + +def test_pose_with_covariance_matrix_property(): + """Test covariance matrix property.""" + pose = Pose() + covariance_array = np.arange(36, dtype=float) + pose_cov = PoseWithCovariance(pose, covariance_array) + + # Get as matrix + cov_matrix = pose_cov.covariance_matrix + assert cov_matrix.shape == (6, 6) + assert cov_matrix[0, 0] == 0.0 + assert cov_matrix[5, 5] == 35.0 + + # Set from matrix + new_matrix = np.eye(6) * 2.0 + pose_cov.covariance_matrix = new_matrix + assert np.array_equal(pose_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + + +def test_pose_with_covariance_repr(): + """Test string representation.""" + pose = Pose(1.234, 2.567, 3.891) + pose_cov = PoseWithCovariance(pose) + + repr_str = repr(pose_cov) + assert "PoseWithCovariance" in repr_str + assert "pose=" in repr_str + assert "covariance=" in repr_str + assert "36 elements" in repr_str + + +def test_pose_with_covariance_str(): + """Test string formatting.""" + pose = Pose(1.234, 2.567, 3.891) + covariance = np.eye(6).flatten() + pose_cov = PoseWithCovariance(pose, covariance) + + str_repr = str(pose_cov) + assert "PoseWithCovariance" in str_repr + assert "1.234" in str_repr + assert "2.567" in str_repr + assert "3.891" in str_repr + assert "cov_trace" in str_repr + assert "6.000" in str_repr # Trace of identity matrix is 6 + + +def test_pose_with_covariance_equality(): + """Test equality comparison.""" + pose1 = Pose(1.0, 2.0, 3.0) + cov1 = np.arange(36, dtype=float) + pose_cov1 = PoseWithCovariance(pose1, cov1) + + pose2 = Pose(1.0, 2.0, 3.0) + cov2 = np.arange(36, dtype=float) + pose_cov2 = PoseWithCovariance(pose2, cov2) + + # Equal + assert pose_cov1 == pose_cov2 + + # Different pose + pose3 = Pose(1.1, 2.0, 3.0) + pose_cov3 = PoseWithCovariance(pose3, cov1) + assert pose_cov1 != pose_cov3 + + # Different covariance + cov3 = np.arange(36, dtype=float) + 1 + pose_cov4 = PoseWithCovariance(pose1, cov3) + assert pose_cov1 != pose_cov4 + + # Different type + assert pose_cov1 != "not a pose" + assert pose_cov1 != None + + +def test_pose_with_covariance_lcm_encode_decode(): + """Test LCM encoding and decoding.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + source = PoseWithCovariance(pose, covariance) + + # Encode and decode + binary_msg = source.lcm_encode() + decoded = PoseWithCovariance.lcm_decode(binary_msg) + + # Should be equal + assert decoded == source + assert isinstance(decoded, PoseWithCovariance) + assert isinstance(decoded.pose, Pose) + assert isinstance(decoded.covariance, np.ndarray) + + +def test_pose_with_covariance_from_ros_msg(): + """Test creating from ROS message.""" + ros_msg = ROSPoseWithCovariance() + ros_msg.pose.position = ROSPoint(x=1.0, y=2.0, z=3.0) + ros_msg.pose.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ros_msg.covariance = [float(i) for i in range(36)] + + pose_cov = PoseWithCovariance.from_ros_msg(ros_msg) + + assert pose_cov.pose.position.x == 1.0 + assert pose_cov.pose.position.y == 2.0 + assert pose_cov.pose.position.z == 3.0 + assert pose_cov.pose.orientation.x == 0.1 + assert pose_cov.pose.orientation.y == 0.2 + assert pose_cov.pose.orientation.z == 0.3 + assert pose_cov.pose.orientation.w == 0.9 + assert np.array_equal(pose_cov.covariance, np.arange(36)) + + +def test_pose_with_covariance_to_ros_msg(): + """Test converting to ROS message.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + pose_cov = PoseWithCovariance(pose, covariance) + + ros_msg = pose_cov.to_ros_msg() + + assert isinstance(ros_msg, ROSPoseWithCovariance) + assert ros_msg.pose.position.x == 1.0 + assert ros_msg.pose.position.y == 2.0 + assert ros_msg.pose.position.z == 3.0 + assert ros_msg.pose.orientation.x == 0.1 + assert ros_msg.pose.orientation.y == 0.2 + assert ros_msg.pose.orientation.z == 0.3 + assert ros_msg.pose.orientation.w == 0.9 + assert list(ros_msg.covariance) == list(range(36)) + + +def test_pose_with_covariance_ros_roundtrip(): + """Test round-trip conversion with ROS messages.""" + pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) + covariance = np.random.rand(36) + original = PoseWithCovariance(pose, covariance) + + ros_msg = original.to_ros_msg() + restored = PoseWithCovariance.from_ros_msg(ros_msg) + + assert restored == original + + +def test_pose_with_covariance_zero_covariance(): + """Test with zero covariance matrix.""" + pose = Pose(1.0, 2.0, 3.0) + pose_cov = PoseWithCovariance(pose) + + assert np.all(pose_cov.covariance == 0.0) + assert np.trace(pose_cov.covariance_matrix) == 0.0 + + +def test_pose_with_covariance_diagonal_covariance(): + """Test with diagonal covariance matrix.""" + pose = Pose() + covariance = np.zeros(36) + # Set diagonal elements + for i in range(6): + covariance[i * 6 + i] = i + 1 + + pose_cov = PoseWithCovariance(pose, covariance) + + cov_matrix = pose_cov.covariance_matrix + assert np.trace(cov_matrix) == sum(range(1, 7)) # 1+2+3+4+5+6 = 21 + + # Check diagonal elements + for i in range(6): + assert cov_matrix[i, i] == i + 1 + + # Check off-diagonal elements are zero + for i in range(6): + for j in range(6): + if i != j: + assert cov_matrix[i, j] == 0.0 + + +@pytest.mark.parametrize( + "x,y,z", + [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (100.0, -100.0, 0.0)], +) +def test_pose_with_covariance_parametrized_positions(x, y, z): + """Parametrized test for various position values.""" + pose = Pose(x, y, z) + pose_cov = PoseWithCovariance(pose) + + assert pose_cov.x == x + assert pose_cov.y == y + assert pose_cov.z == z diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py new file mode 100644 index 0000000000..f6b7560e16 --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py @@ -0,0 +1,342 @@ +# 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 PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped +from dimos_lcm.std_msgs import Header as LCMHeader +from dimos_lcm.std_msgs import Time as LCMTime +from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped +from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion +from std_msgs.msg import Header as ROSHeader +from builtin_interfaces.msg import Time as ROSTime + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import PoseWithCovarianceStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +def test_pose_with_covariance_stamped_default_init(): + """Test default initialization.""" + pose_cov_stamped = PoseWithCovarianceStamped() + + # Should have current timestamp + assert pose_cov_stamped.ts > 0 + assert pose_cov_stamped.frame_id == "" + + # Pose should be at origin with identity orientation + assert pose_cov_stamped.pose.position.x == 0.0 + assert pose_cov_stamped.pose.position.y == 0.0 + assert pose_cov_stamped.pose.position.z == 0.0 + assert pose_cov_stamped.pose.orientation.w == 1.0 + + # Covariance should be all zeros + assert np.all(pose_cov_stamped.covariance == 0.0) + + +def test_pose_with_covariance_stamped_with_timestamp(): + """Test initialization with specific timestamp.""" + ts = 1234567890.123456 + frame_id = "base_link" + pose_cov_stamped = PoseWithCovarianceStamped(ts=ts, frame_id=frame_id) + + assert pose_cov_stamped.ts == ts + assert pose_cov_stamped.frame_id == frame_id + + +def test_pose_with_covariance_stamped_with_pose(): + """Test initialization with pose.""" + ts = 1234567890.123456 + frame_id = "map" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + + pose_cov_stamped = PoseWithCovarianceStamped( + ts=ts, frame_id=frame_id, pose=pose, covariance=covariance + ) + + assert pose_cov_stamped.ts == ts + assert pose_cov_stamped.frame_id == frame_id + assert pose_cov_stamped.pose.position.x == 1.0 + assert pose_cov_stamped.pose.position.y == 2.0 + assert pose_cov_stamped.pose.position.z == 3.0 + assert np.array_equal(pose_cov_stamped.covariance, covariance) + + +def test_pose_with_covariance_stamped_properties(): + """Test convenience properties.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.eye(6).flatten() + pose_cov_stamped = PoseWithCovarianceStamped( + ts=1234567890.0, frame_id="odom", pose=pose, covariance=covariance + ) + + # Position properties + assert pose_cov_stamped.x == 1.0 + assert pose_cov_stamped.y == 2.0 + assert pose_cov_stamped.z == 3.0 + + # Orientation properties + assert pose_cov_stamped.orientation.x == 0.1 + assert pose_cov_stamped.orientation.y == 0.2 + assert pose_cov_stamped.orientation.z == 0.3 + assert pose_cov_stamped.orientation.w == 0.9 + + # Euler angles + assert pose_cov_stamped.roll == pose.roll + assert pose_cov_stamped.pitch == pose.pitch + assert pose_cov_stamped.yaw == pose.yaw + + # Covariance matrix + cov_matrix = pose_cov_stamped.covariance_matrix + assert cov_matrix.shape == (6, 6) + assert np.trace(cov_matrix) == 6.0 + + +def test_pose_with_covariance_stamped_str(): + """Test string representation.""" + pose = Pose(1.234, 2.567, 3.891) + covariance = np.eye(6).flatten() * 2.0 + pose_cov_stamped = PoseWithCovarianceStamped( + ts=1234567890.0, frame_id="world", pose=pose, covariance=covariance + ) + + str_repr = str(pose_cov_stamped) + assert "PoseWithCovarianceStamped" in str_repr + assert "1.234" in str_repr + assert "2.567" in str_repr + assert "3.891" in str_repr + assert "cov_trace" in str_repr + assert "12.000" in str_repr # Trace of 2*identity is 12 + + +def test_pose_with_covariance_stamped_lcm_encode_decode(): + """Test LCM encoding and decoding.""" + ts = 1234567890.123456 + frame_id = "camera_link" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + + source = PoseWithCovarianceStamped(ts=ts, frame_id=frame_id, pose=pose, covariance=covariance) + + # Encode and decode + binary_msg = source.lcm_encode() + decoded = PoseWithCovarianceStamped.lcm_decode(binary_msg) + + # Check timestamp (may lose some precision) + assert abs(decoded.ts - ts) < 1e-6 + assert decoded.frame_id == frame_id + + # Check pose + assert decoded.pose.position.x == 1.0 + assert decoded.pose.position.y == 2.0 + assert decoded.pose.position.z == 3.0 + assert decoded.pose.orientation.x == 0.1 + assert decoded.pose.orientation.y == 0.2 + assert decoded.pose.orientation.z == 0.3 + assert decoded.pose.orientation.w == 0.9 + + # Check covariance + assert np.array_equal(decoded.covariance, covariance) + + +def test_pose_with_covariance_stamped_from_ros_msg(): + """Test creating from ROS message.""" + ros_msg = ROSPoseWithCovarianceStamped() + + # Set header + ros_msg.header = ROSHeader() + ros_msg.header.stamp = ROSTime() + ros_msg.header.stamp.sec = 1234567890 + ros_msg.header.stamp.nanosec = 123456000 + ros_msg.header.frame_id = "laser" + + # Set pose with covariance + ros_msg.pose = ROSPoseWithCovariance() + ros_msg.pose.pose = ROSPose() + ros_msg.pose.pose.position = ROSPoint(x=1.0, y=2.0, z=3.0) + ros_msg.pose.pose.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ros_msg.pose.covariance = [float(i) for i in range(36)] + + pose_cov_stamped = PoseWithCovarianceStamped.from_ros_msg(ros_msg) + + assert pose_cov_stamped.ts == 1234567890.123456 + assert pose_cov_stamped.frame_id == "laser" + assert pose_cov_stamped.pose.position.x == 1.0 + assert pose_cov_stamped.pose.position.y == 2.0 + assert pose_cov_stamped.pose.position.z == 3.0 + assert pose_cov_stamped.pose.orientation.x == 0.1 + assert pose_cov_stamped.pose.orientation.y == 0.2 + assert pose_cov_stamped.pose.orientation.z == 0.3 + assert pose_cov_stamped.pose.orientation.w == 0.9 + assert np.array_equal(pose_cov_stamped.covariance, np.arange(36)) + + +def test_pose_with_covariance_stamped_to_ros_msg(): + """Test converting to ROS message.""" + ts = 1234567890.567890 + frame_id = "imu" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + covariance = np.arange(36, dtype=float) + + pose_cov_stamped = PoseWithCovarianceStamped( + ts=ts, frame_id=frame_id, pose=pose, covariance=covariance + ) + + ros_msg = pose_cov_stamped.to_ros_msg() + + assert isinstance(ros_msg, ROSPoseWithCovarianceStamped) + assert ros_msg.header.frame_id == frame_id + assert ros_msg.header.stamp.sec == 1234567890 + assert abs(ros_msg.header.stamp.nanosec - 567890000) < 100 # Allow small rounding error + + assert ros_msg.pose.pose.position.x == 1.0 + assert ros_msg.pose.pose.position.y == 2.0 + assert ros_msg.pose.pose.position.z == 3.0 + assert ros_msg.pose.pose.orientation.x == 0.1 + assert ros_msg.pose.pose.orientation.y == 0.2 + assert ros_msg.pose.pose.orientation.z == 0.3 + assert ros_msg.pose.pose.orientation.w == 0.9 + assert list(ros_msg.pose.covariance) == list(range(36)) + + +def test_pose_with_covariance_stamped_ros_roundtrip(): + """Test round-trip conversion with ROS messages.""" + ts = 2147483647.987654 # Max int32 value for ROS Time.sec + frame_id = "robot_base" + pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) + covariance = np.random.rand(36) + + original = PoseWithCovarianceStamped(ts=ts, frame_id=frame_id, pose=pose, covariance=covariance) + + ros_msg = original.to_ros_msg() + restored = PoseWithCovarianceStamped.from_ros_msg(ros_msg) + + # Check timestamp (loses some precision in conversion) + assert abs(restored.ts - ts) < 1e-6 + assert restored.frame_id == frame_id + + # Check pose + assert restored.pose.position.x == original.pose.position.x + assert restored.pose.position.y == original.pose.position.y + assert restored.pose.position.z == original.pose.position.z + assert restored.pose.orientation.x == original.pose.orientation.x + assert restored.pose.orientation.y == original.pose.orientation.y + assert restored.pose.orientation.z == original.pose.orientation.z + assert restored.pose.orientation.w == original.pose.orientation.w + + # Check covariance + assert np.allclose(restored.covariance, original.covariance) + + +def test_pose_with_covariance_stamped_zero_timestamp(): + """Test that zero timestamp gets replaced with current time.""" + pose_cov_stamped = PoseWithCovarianceStamped(ts=0.0) + + # Should have been replaced with current time + assert pose_cov_stamped.ts > 0 + assert pose_cov_stamped.ts <= time.time() + + +def test_pose_with_covariance_stamped_inheritance(): + """Test that it properly inherits from PoseWithCovariance and Timestamped.""" + pose = Pose(1.0, 2.0, 3.0) + covariance = np.eye(6).flatten() + pose_cov_stamped = PoseWithCovarianceStamped( + ts=1234567890.0, frame_id="test", pose=pose, covariance=covariance + ) + + # Should be instance of parent classes + assert isinstance(pose_cov_stamped, PoseWithCovariance) + + # Should have Timestamped attributes + assert hasattr(pose_cov_stamped, "ts") + assert hasattr(pose_cov_stamped, "frame_id") + + # Should have PoseWithCovariance attributes + assert hasattr(pose_cov_stamped, "pose") + assert hasattr(pose_cov_stamped, "covariance") + + +def test_pose_with_covariance_stamped_sec_nsec(): + """Test the sec_nsec helper function.""" + from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import sec_nsec + + # Test integer seconds + s, ns = sec_nsec(1234567890.0) + assert s == 1234567890 + assert ns == 0 + + # Test fractional seconds + s, ns = sec_nsec(1234567890.123456789) + assert s == 1234567890 + assert abs(ns - 123456789) < 100 # Allow small rounding error + + # Test small fractional seconds + s, ns = sec_nsec(0.000000001) + assert s == 0 + assert ns == 1 + + # Test large timestamp + s, ns = sec_nsec(9999999999.999999999) + # Due to floating point precision, this might round to 10000000000 + assert s in [9999999999, 10000000000] + if s == 9999999999: + assert abs(ns - 999999999) < 10 + else: + assert ns == 0 + + +@pytest.mark.parametrize( + "frame_id", + ["", "map", "odom", "base_link", "camera_optical_frame", "sensor/lidar/front"], +) +def test_pose_with_covariance_stamped_frame_ids(frame_id): + """Test various frame ID values.""" + pose_cov_stamped = PoseWithCovarianceStamped(frame_id=frame_id) + assert pose_cov_stamped.frame_id == frame_id + + # Test roundtrip through ROS + ros_msg = pose_cov_stamped.to_ros_msg() + assert ros_msg.header.frame_id == frame_id + + restored = PoseWithCovarianceStamped.from_ros_msg(ros_msg) + assert restored.frame_id == frame_id + + +def test_pose_with_covariance_stamped_different_covariances(): + """Test with different covariance patterns.""" + pose = Pose(1.0, 2.0, 3.0) + + # Zero covariance + zero_cov = np.zeros(36) + pose_cov1 = PoseWithCovarianceStamped(pose=pose, covariance=zero_cov) + assert np.all(pose_cov1.covariance == 0.0) + + # Identity covariance + identity_cov = np.eye(6).flatten() + pose_cov2 = PoseWithCovarianceStamped(pose=pose, covariance=identity_cov) + assert np.trace(pose_cov2.covariance_matrix) == 6.0 + + # Full covariance + full_cov = np.random.rand(36) + pose_cov3 = PoseWithCovarianceStamped(pose=pose, covariance=full_cov) + assert np.array_equal(pose_cov3.covariance, full_cov) diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py new file mode 100644 index 0000000000..0a2dbdff06 --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py @@ -0,0 +1,407 @@ +# Copyright 2025 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +import pytest +from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance +from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Vector3 as ROSVector3 + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +def test_twist_with_covariance_default_init(): + """Test that default initialization creates a zero twist with zero covariance.""" + twist_cov = TwistWithCovariance() + + # Twist should be zero + assert twist_cov.twist.linear.x == 0.0 + assert twist_cov.twist.linear.y == 0.0 + assert twist_cov.twist.linear.z == 0.0 + assert twist_cov.twist.angular.x == 0.0 + assert twist_cov.twist.angular.y == 0.0 + assert twist_cov.twist.angular.z == 0.0 + + # Covariance should be all zeros + assert np.all(twist_cov.covariance == 0.0) + assert twist_cov.covariance.shape == (36,) + + +def test_twist_with_covariance_twist_init(): + """Test initialization with a Twist object.""" + linear = Vector3(1.0, 2.0, 3.0) + angular = Vector3(0.1, 0.2, 0.3) + twist = Twist(linear, angular) + twist_cov = TwistWithCovariance(twist) + + # Twist should match + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert twist_cov.twist.angular.x == 0.1 + assert twist_cov.twist.angular.y == 0.2 + assert twist_cov.twist.angular.z == 0.3 + + # Covariance should be zeros by default + assert np.all(twist_cov.covariance == 0.0) + + +def test_twist_with_covariance_twist_and_covariance_init(): + """Test initialization with twist and covariance.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + twist_cov = TwistWithCovariance(twist, covariance) + + # Twist should match + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + + # Covariance should match + assert np.array_equal(twist_cov.covariance, covariance) + + +def test_twist_with_covariance_tuple_init(): + """Test initialization with tuple of (linear, angular) velocities.""" + linear = [1.0, 2.0, 3.0] + angular = [0.1, 0.2, 0.3] + covariance = np.arange(36, dtype=float) + twist_cov = TwistWithCovariance((linear, angular), covariance) + + # Twist should match + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert twist_cov.twist.angular.x == 0.1 + assert twist_cov.twist.angular.y == 0.2 + assert twist_cov.twist.angular.z == 0.3 + + # Covariance should match + assert np.array_equal(twist_cov.covariance, covariance) + + +def test_twist_with_covariance_list_covariance(): + """Test initialization with covariance as a list.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance_list = list(range(36)) + twist_cov = TwistWithCovariance(twist, covariance_list) + + # Covariance should be converted to numpy array + assert isinstance(twist_cov.covariance, np.ndarray) + assert np.array_equal(twist_cov.covariance, np.array(covariance_list)) + + +def test_twist_with_covariance_copy_init(): + """Test copy constructor.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + original = TwistWithCovariance(twist, covariance) + copy = TwistWithCovariance(original) + + # Should be equal but not the same object + assert copy == original + assert copy is not original + assert copy.twist is not original.twist + assert copy.covariance is not original.covariance + + # Modify original to ensure they're independent + original.covariance[0] = 999.0 + assert copy.covariance[0] != 999.0 + + +def test_twist_with_covariance_lcm_init(): + """Test initialization from LCM message.""" + lcm_msg = LCMTwistWithCovariance() + lcm_msg.twist.linear.x = 1.0 + lcm_msg.twist.linear.y = 2.0 + lcm_msg.twist.linear.z = 3.0 + lcm_msg.twist.angular.x = 0.1 + lcm_msg.twist.angular.y = 0.2 + lcm_msg.twist.angular.z = 0.3 + lcm_msg.covariance = list(range(36)) + + twist_cov = TwistWithCovariance(lcm_msg) + + # Twist should match + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert twist_cov.twist.angular.x == 0.1 + assert twist_cov.twist.angular.y == 0.2 + assert twist_cov.twist.angular.z == 0.3 + + # Covariance should match + assert np.array_equal(twist_cov.covariance, np.arange(36)) + + +def test_twist_with_covariance_dict_init(): + """Test initialization from dictionary.""" + twist_dict = { + "twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)), + "covariance": list(range(36)), + } + twist_cov = TwistWithCovariance(twist_dict) + + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert np.array_equal(twist_cov.covariance, np.arange(36)) + + +def test_twist_with_covariance_dict_init_no_covariance(): + """Test initialization from dictionary without covariance.""" + twist_dict = {"twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3))} + twist_cov = TwistWithCovariance(twist_dict) + + assert twist_cov.twist.linear.x == 1.0 + assert np.all(twist_cov.covariance == 0.0) + + +def test_twist_with_covariance_tuple_of_tuple_init(): + """Test initialization from tuple of (twist_tuple, covariance).""" + twist_tuple = ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]) + covariance = np.arange(36, dtype=float) + twist_cov = TwistWithCovariance((twist_tuple, covariance)) + + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert twist_cov.twist.angular.x == 0.1 + assert twist_cov.twist.angular.y == 0.2 + assert twist_cov.twist.angular.z == 0.3 + assert np.array_equal(twist_cov.covariance, covariance) + + +def test_twist_with_covariance_properties(): + """Test convenience properties.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + twist_cov = TwistWithCovariance(twist) + + # Linear and angular properties + assert twist_cov.linear.x == 1.0 + assert twist_cov.linear.y == 2.0 + assert twist_cov.linear.z == 3.0 + assert twist_cov.angular.x == 0.1 + assert twist_cov.angular.y == 0.2 + assert twist_cov.angular.z == 0.3 + + +def test_twist_with_covariance_matrix_property(): + """Test covariance matrix property.""" + twist = Twist() + covariance_array = np.arange(36, dtype=float) + twist_cov = TwistWithCovariance(twist, covariance_array) + + # Get as matrix + cov_matrix = twist_cov.covariance_matrix + assert cov_matrix.shape == (6, 6) + assert cov_matrix[0, 0] == 0.0 + assert cov_matrix[5, 5] == 35.0 + + # Set from matrix + new_matrix = np.eye(6) * 2.0 + twist_cov.covariance_matrix = new_matrix + assert np.array_equal(twist_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + + +def test_twist_with_covariance_repr(): + """Test string representation.""" + twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) + twist_cov = TwistWithCovariance(twist) + + repr_str = repr(twist_cov) + assert "TwistWithCovariance" in repr_str + assert "twist=" in repr_str + assert "covariance=" in repr_str + assert "36 elements" in repr_str + + +def test_twist_with_covariance_str(): + """Test string formatting.""" + twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) + covariance = np.eye(6).flatten() + twist_cov = TwistWithCovariance(twist, covariance) + + str_repr = str(twist_cov) + assert "TwistWithCovariance" in str_repr + assert "1.234" in str_repr + assert "2.567" in str_repr + assert "3.891" in str_repr + assert "cov_trace" in str_repr + assert "6.000" in str_repr # Trace of identity matrix is 6 + + +def test_twist_with_covariance_equality(): + """Test equality comparison.""" + twist1 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + cov1 = np.arange(36, dtype=float) + twist_cov1 = TwistWithCovariance(twist1, cov1) + + twist2 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + cov2 = np.arange(36, dtype=float) + twist_cov2 = TwistWithCovariance(twist2, cov2) + + # Equal + assert twist_cov1 == twist_cov2 + + # Different twist + twist3 = Twist(Vector3(1.1, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + twist_cov3 = TwistWithCovariance(twist3, cov1) + assert twist_cov1 != twist_cov3 + + # Different covariance + cov3 = np.arange(36, dtype=float) + 1 + twist_cov4 = TwistWithCovariance(twist1, cov3) + assert twist_cov1 != twist_cov4 + + # Different type + assert twist_cov1 != "not a twist" + assert twist_cov1 != None + + +def test_twist_with_covariance_is_zero(): + """Test is_zero method.""" + # Zero twist + twist_cov1 = TwistWithCovariance() + assert twist_cov1.is_zero() + assert not twist_cov1 # Boolean conversion + + # Non-zero twist + twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) + twist_cov2 = TwistWithCovariance(twist) + assert not twist_cov2.is_zero() + assert twist_cov2 # Boolean conversion + + +def test_twist_with_covariance_lcm_encode_decode(): + """Test LCM encoding and decoding.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + source = TwistWithCovariance(twist, covariance) + + # Encode and decode + binary_msg = source.lcm_encode() + decoded = TwistWithCovariance.lcm_decode(binary_msg) + + # Should be equal + assert decoded == source + assert isinstance(decoded, TwistWithCovariance) + assert isinstance(decoded.twist, Twist) + assert isinstance(decoded.covariance, np.ndarray) + + +def test_twist_with_covariance_from_ros_msg(): + """Test creating from ROS message.""" + ros_msg = ROSTwistWithCovariance() + ros_msg.twist.linear = ROSVector3(x=1.0, y=2.0, z=3.0) + ros_msg.twist.angular = ROSVector3(x=0.1, y=0.2, z=0.3) + ros_msg.covariance = [float(i) for i in range(36)] + + twist_cov = TwistWithCovariance.from_ros_msg(ros_msg) + + assert twist_cov.twist.linear.x == 1.0 + assert twist_cov.twist.linear.y == 2.0 + assert twist_cov.twist.linear.z == 3.0 + assert twist_cov.twist.angular.x == 0.1 + assert twist_cov.twist.angular.y == 0.2 + assert twist_cov.twist.angular.z == 0.3 + assert np.array_equal(twist_cov.covariance, np.arange(36)) + + +def test_twist_with_covariance_to_ros_msg(): + """Test converting to ROS message.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + twist_cov = TwistWithCovariance(twist, covariance) + + ros_msg = twist_cov.to_ros_msg() + + assert isinstance(ros_msg, ROSTwistWithCovariance) + assert ros_msg.twist.linear.x == 1.0 + assert ros_msg.twist.linear.y == 2.0 + assert ros_msg.twist.linear.z == 3.0 + assert ros_msg.twist.angular.x == 0.1 + assert ros_msg.twist.angular.y == 0.2 + assert ros_msg.twist.angular.z == 0.3 + assert list(ros_msg.covariance) == list(range(36)) + + +def test_twist_with_covariance_ros_roundtrip(): + """Test round-trip conversion with ROS messages.""" + twist = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.15, 0.25, 0.35)) + covariance = np.random.rand(36) + original = TwistWithCovariance(twist, covariance) + + ros_msg = original.to_ros_msg() + restored = TwistWithCovariance.from_ros_msg(ros_msg) + + assert restored == original + + +def test_twist_with_covariance_zero_covariance(): + """Test with zero covariance matrix.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + twist_cov = TwistWithCovariance(twist) + + assert np.all(twist_cov.covariance == 0.0) + assert np.trace(twist_cov.covariance_matrix) == 0.0 + + +def test_twist_with_covariance_diagonal_covariance(): + """Test with diagonal covariance matrix.""" + twist = Twist() + covariance = np.zeros(36) + # Set diagonal elements + for i in range(6): + covariance[i * 6 + i] = i + 1 + + twist_cov = TwistWithCovariance(twist, covariance) + + cov_matrix = twist_cov.covariance_matrix + assert np.trace(cov_matrix) == sum(range(1, 7)) # 1+2+3+4+5+6 = 21 + + # Check diagonal elements + for i in range(6): + assert cov_matrix[i, i] == i + 1 + + # Check off-diagonal elements are zero + for i in range(6): + for j in range(6): + if i != j: + assert cov_matrix[i, j] == 0.0 + + +@pytest.mark.parametrize( + "linear,angular", + [ + ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), + ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]), + ([-1.0, -2.0, -3.0], [-0.1, -0.2, -0.3]), + ([100.0, -100.0, 0.0], [3.14, -3.14, 0.0]), + ], +) +def test_twist_with_covariance_parametrized_velocities(linear, angular): + """Parametrized test for various velocity values.""" + twist = Twist(linear, angular) + twist_cov = TwistWithCovariance(twist) + + assert twist_cov.linear.x == linear[0] + assert twist_cov.linear.y == linear[1] + assert twist_cov.linear.z == linear[2] + assert twist_cov.angular.x == angular[0] + assert twist_cov.angular.y == angular[1] + assert twist_cov.angular.z == angular[2] diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py new file mode 100644 index 0000000000..7abc3f689b --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py @@ -0,0 +1,367 @@ +# 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 TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped +from dimos_lcm.std_msgs import Header as LCMHeader +from dimos_lcm.std_msgs import Time as LCMTime +from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped +from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Vector3 as ROSVector3 +from std_msgs.msg import Header as ROSHeader +from builtin_interfaces.msg import Time as ROSTime + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import TwistWithCovarianceStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +def test_twist_with_covariance_stamped_default_init(): + """Test default initialization.""" + twist_cov_stamped = TwistWithCovarianceStamped() + + # Should have current timestamp + assert twist_cov_stamped.ts > 0 + assert twist_cov_stamped.frame_id == "" + + # Twist should be zero + assert twist_cov_stamped.twist.linear.x == 0.0 + assert twist_cov_stamped.twist.linear.y == 0.0 + assert twist_cov_stamped.twist.linear.z == 0.0 + assert twist_cov_stamped.twist.angular.x == 0.0 + assert twist_cov_stamped.twist.angular.y == 0.0 + assert twist_cov_stamped.twist.angular.z == 0.0 + + # Covariance should be all zeros + assert np.all(twist_cov_stamped.covariance == 0.0) + + +def test_twist_with_covariance_stamped_with_timestamp(): + """Test initialization with specific timestamp.""" + ts = 1234567890.123456 + frame_id = "base_link" + twist_cov_stamped = TwistWithCovarianceStamped(ts=ts, frame_id=frame_id) + + assert twist_cov_stamped.ts == ts + assert twist_cov_stamped.frame_id == frame_id + + +def test_twist_with_covariance_stamped_with_twist(): + """Test initialization with twist.""" + ts = 1234567890.123456 + frame_id = "odom" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + + twist_cov_stamped = TwistWithCovarianceStamped( + ts=ts, frame_id=frame_id, twist=twist, covariance=covariance + ) + + assert twist_cov_stamped.ts == ts + assert twist_cov_stamped.frame_id == frame_id + assert twist_cov_stamped.twist.linear.x == 1.0 + assert twist_cov_stamped.twist.linear.y == 2.0 + assert twist_cov_stamped.twist.linear.z == 3.0 + assert np.array_equal(twist_cov_stamped.covariance, covariance) + + +def test_twist_with_covariance_stamped_with_tuple(): + """Test initialization with tuple of velocities.""" + ts = 1234567890.123456 + frame_id = "robot_base" + linear = [1.0, 2.0, 3.0] + angular = [0.1, 0.2, 0.3] + covariance = np.arange(36, dtype=float) + + twist_cov_stamped = TwistWithCovarianceStamped( + ts=ts, frame_id=frame_id, twist=(linear, angular), covariance=covariance + ) + + assert twist_cov_stamped.ts == ts + assert twist_cov_stamped.frame_id == frame_id + assert twist_cov_stamped.twist.linear.x == 1.0 + assert twist_cov_stamped.twist.angular.x == 0.1 + assert np.array_equal(twist_cov_stamped.covariance, covariance) + + +def test_twist_with_covariance_stamped_properties(): + """Test convenience properties.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.eye(6).flatten() + twist_cov_stamped = TwistWithCovarianceStamped( + ts=1234567890.0, frame_id="cmd_vel", twist=twist, covariance=covariance + ) + + # Linear and angular properties + assert twist_cov_stamped.linear.x == 1.0 + assert twist_cov_stamped.linear.y == 2.0 + assert twist_cov_stamped.linear.z == 3.0 + assert twist_cov_stamped.angular.x == 0.1 + assert twist_cov_stamped.angular.y == 0.2 + assert twist_cov_stamped.angular.z == 0.3 + + # Covariance matrix + cov_matrix = twist_cov_stamped.covariance_matrix + assert cov_matrix.shape == (6, 6) + assert np.trace(cov_matrix) == 6.0 + + +def test_twist_with_covariance_stamped_str(): + """Test string representation.""" + twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.111, 0.222, 0.333)) + covariance = np.eye(6).flatten() * 2.0 + twist_cov_stamped = TwistWithCovarianceStamped( + ts=1234567890.0, frame_id="world", twist=twist, covariance=covariance + ) + + str_repr = str(twist_cov_stamped) + assert "TwistWithCovarianceStamped" in str_repr + assert "1.234" in str_repr + assert "2.567" in str_repr + assert "3.891" in str_repr + assert "cov_trace" in str_repr + assert "12.000" in str_repr # Trace of 2*identity is 12 + + +def test_twist_with_covariance_stamped_lcm_encode_decode(): + """Test LCM encoding and decoding.""" + ts = 1234567890.123456 + frame_id = "camera_link" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + + source = TwistWithCovarianceStamped( + ts=ts, frame_id=frame_id, twist=twist, covariance=covariance + ) + + # Encode and decode + binary_msg = source.lcm_encode() + decoded = TwistWithCovarianceStamped.lcm_decode(binary_msg) + + # Check timestamp (may lose some precision) + assert abs(decoded.ts - ts) < 1e-6 + assert decoded.frame_id == frame_id + + # Check twist + assert decoded.twist.linear.x == 1.0 + assert decoded.twist.linear.y == 2.0 + assert decoded.twist.linear.z == 3.0 + assert decoded.twist.angular.x == 0.1 + assert decoded.twist.angular.y == 0.2 + assert decoded.twist.angular.z == 0.3 + + # Check covariance + assert np.array_equal(decoded.covariance, covariance) + + +def test_twist_with_covariance_stamped_from_ros_msg(): + """Test creating from ROS message.""" + ros_msg = ROSTwistWithCovarianceStamped() + + # Set header + ros_msg.header = ROSHeader() + ros_msg.header.stamp = ROSTime() + ros_msg.header.stamp.sec = 1234567890 + ros_msg.header.stamp.nanosec = 123456000 + ros_msg.header.frame_id = "laser" + + # Set twist with covariance + ros_msg.twist = ROSTwistWithCovariance() + ros_msg.twist.twist = ROSTwist() + ros_msg.twist.twist.linear = ROSVector3(x=1.0, y=2.0, z=3.0) + ros_msg.twist.twist.angular = ROSVector3(x=0.1, y=0.2, z=0.3) + ros_msg.twist.covariance = [float(i) for i in range(36)] + + twist_cov_stamped = TwistWithCovarianceStamped.from_ros_msg(ros_msg) + + assert twist_cov_stamped.ts == 1234567890.123456 + assert twist_cov_stamped.frame_id == "laser" + assert twist_cov_stamped.twist.linear.x == 1.0 + assert twist_cov_stamped.twist.linear.y == 2.0 + assert twist_cov_stamped.twist.linear.z == 3.0 + assert twist_cov_stamped.twist.angular.x == 0.1 + assert twist_cov_stamped.twist.angular.y == 0.2 + assert twist_cov_stamped.twist.angular.z == 0.3 + assert np.array_equal(twist_cov_stamped.covariance, np.arange(36)) + + +def test_twist_with_covariance_stamped_to_ros_msg(): + """Test converting to ROS message.""" + ts = 1234567890.567890 + frame_id = "imu" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.arange(36, dtype=float) + + twist_cov_stamped = TwistWithCovarianceStamped( + ts=ts, frame_id=frame_id, twist=twist, covariance=covariance + ) + + ros_msg = twist_cov_stamped.to_ros_msg() + + assert isinstance(ros_msg, ROSTwistWithCovarianceStamped) + assert ros_msg.header.frame_id == frame_id + assert ros_msg.header.stamp.sec == 1234567890 + assert abs(ros_msg.header.stamp.nanosec - 567890000) < 100 # Allow small rounding error + + assert ros_msg.twist.twist.linear.x == 1.0 + assert ros_msg.twist.twist.linear.y == 2.0 + assert ros_msg.twist.twist.linear.z == 3.0 + assert ros_msg.twist.twist.angular.x == 0.1 + assert ros_msg.twist.twist.angular.y == 0.2 + assert ros_msg.twist.twist.angular.z == 0.3 + assert list(ros_msg.twist.covariance) == list(range(36)) + + +def test_twist_with_covariance_stamped_ros_roundtrip(): + """Test round-trip conversion with ROS messages.""" + ts = 2147483647.987654 # Max int32 value for ROS Time.sec + frame_id = "robot_base" + twist = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.15, 0.25, 0.35)) + covariance = np.random.rand(36) + + original = TwistWithCovarianceStamped( + ts=ts, frame_id=frame_id, twist=twist, covariance=covariance + ) + + ros_msg = original.to_ros_msg() + restored = TwistWithCovarianceStamped.from_ros_msg(ros_msg) + + # Check timestamp (loses some precision in conversion) + assert abs(restored.ts - ts) < 1e-6 + assert restored.frame_id == frame_id + + # Check twist + assert restored.twist.linear.x == original.twist.linear.x + assert restored.twist.linear.y == original.twist.linear.y + assert restored.twist.linear.z == original.twist.linear.z + assert restored.twist.angular.x == original.twist.angular.x + assert restored.twist.angular.y == original.twist.angular.y + assert restored.twist.angular.z == original.twist.angular.z + + # Check covariance + assert np.allclose(restored.covariance, original.covariance) + + +def test_twist_with_covariance_stamped_zero_timestamp(): + """Test that zero timestamp gets replaced with current time.""" + twist_cov_stamped = TwistWithCovarianceStamped(ts=0.0) + + # Should have been replaced with current time + assert twist_cov_stamped.ts > 0 + assert twist_cov_stamped.ts <= time.time() + + +def test_twist_with_covariance_stamped_inheritance(): + """Test that it properly inherits from TwistWithCovariance and Timestamped.""" + twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) + covariance = np.eye(6).flatten() + twist_cov_stamped = TwistWithCovarianceStamped( + ts=1234567890.0, frame_id="test", twist=twist, covariance=covariance + ) + + # Should be instance of parent classes + assert isinstance(twist_cov_stamped, TwistWithCovariance) + + # Should have Timestamped attributes + assert hasattr(twist_cov_stamped, "ts") + assert hasattr(twist_cov_stamped, "frame_id") + + # Should have TwistWithCovariance attributes + assert hasattr(twist_cov_stamped, "twist") + assert hasattr(twist_cov_stamped, "covariance") + + +def test_twist_with_covariance_stamped_is_zero(): + """Test is_zero method inheritance.""" + # Zero twist + twist_cov_stamped1 = TwistWithCovarianceStamped() + assert twist_cov_stamped1.is_zero() + assert not twist_cov_stamped1 # Boolean conversion + + # Non-zero twist + twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) + twist_cov_stamped2 = TwistWithCovarianceStamped(twist=twist) + assert not twist_cov_stamped2.is_zero() + assert twist_cov_stamped2 # Boolean conversion + + +def test_twist_with_covariance_stamped_sec_nsec(): + """Test the sec_nsec helper function.""" + from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import sec_nsec + + # Test integer seconds + s, ns = sec_nsec(1234567890.0) + assert s == 1234567890 + assert ns == 0 + + # Test fractional seconds + s, ns = sec_nsec(1234567890.123456789) + assert s == 1234567890 + assert abs(ns - 123456789) < 100 # Allow small rounding error + + # Test small fractional seconds + s, ns = sec_nsec(0.000000001) + assert s == 0 + assert ns == 1 + + # Test large timestamp + s, ns = sec_nsec(9999999999.999999999) + # Due to floating point precision, this might round to 10000000000 + assert s in [9999999999, 10000000000] + if s == 9999999999: + assert abs(ns - 999999999) < 10 + else: + assert ns == 0 + + +@pytest.mark.parametrize( + "frame_id", + ["", "map", "odom", "base_link", "cmd_vel", "sensor/velocity/front"], +) +def test_twist_with_covariance_stamped_frame_ids(frame_id): + """Test various frame ID values.""" + twist_cov_stamped = TwistWithCovarianceStamped(frame_id=frame_id) + assert twist_cov_stamped.frame_id == frame_id + + # Test roundtrip through ROS + ros_msg = twist_cov_stamped.to_ros_msg() + assert ros_msg.header.frame_id == frame_id + + restored = TwistWithCovarianceStamped.from_ros_msg(ros_msg) + assert restored.frame_id == frame_id + + +def test_twist_with_covariance_stamped_different_covariances(): + """Test with different covariance patterns.""" + twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.5)) + + # Zero covariance + zero_cov = np.zeros(36) + twist_cov1 = TwistWithCovarianceStamped(twist=twist, covariance=zero_cov) + assert np.all(twist_cov1.covariance == 0.0) + + # Identity covariance + identity_cov = np.eye(6).flatten() + twist_cov2 = TwistWithCovarianceStamped(twist=twist, covariance=identity_cov) + assert np.trace(twist_cov2.covariance_matrix) == 6.0 + + # Full covariance + full_cov = np.random.rand(36) + twist_cov3 = TwistWithCovarianceStamped(twist=twist, covariance=full_cov) + assert np.array_equal(twist_cov3.covariance, full_cov) diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py new file mode 100644 index 0000000000..d5c875db20 --- /dev/null +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -0,0 +1,373 @@ +# 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 typing import TypeAlias + +import numpy as np +from dimos_lcm.nav_msgs import Odometry as LCMOdometry +from nav_msgs.msg import Odometry as ROSOdometry +from plum import dispatch + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from Odometry +OdometryConvertable: TypeAlias = ( + LCMOdometry | dict[str, float | str | PoseWithCovariance | TwistWithCovariance | Pose | Twist] +) + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class Odometry(LCMOdometry, Timestamped): + pose: PoseWithCovariance + twist: TwistWithCovariance + msg_name = "nav_msgs.Odometry" + ts: float + frame_id: str + child_frame_id: str + + @dispatch + def __init__( + self, + ts: float = 0.0, + frame_id: str = "", + child_frame_id: str = "", + pose: PoseWithCovariance | Pose | None = None, + twist: TwistWithCovariance | Twist | None = None, + ) -> None: + """Initialize with timestamp, frame IDs, pose and twist. + + Args: + ts: Timestamp in seconds (defaults to current time if 0) + frame_id: Reference frame ID (e.g., "odom", "map") + child_frame_id: Child frame ID (e.g., "base_link", "base_footprint") + pose: Pose with covariance (or just Pose, covariance will be zero) + twist: Twist with covariance (or just Twist, covariance will be zero) + """ + self.ts = ts if ts != 0 else time.time() + self.frame_id = frame_id + self.child_frame_id = child_frame_id + + # Handle pose + if pose is None: + self.pose = PoseWithCovariance() + elif isinstance(pose, PoseWithCovariance): + self.pose = pose + elif isinstance(pose, Pose): + self.pose = PoseWithCovariance(pose) + else: + self.pose = PoseWithCovariance(Pose(pose)) + + # Handle twist + if twist is None: + self.twist = TwistWithCovariance() + elif isinstance(twist, TwistWithCovariance): + self.twist = twist + elif isinstance(twist, Twist): + self.twist = TwistWithCovariance(twist) + else: + self.twist = TwistWithCovariance(Twist(twist)) + + @dispatch + def __init__(self, odometry: Odometry) -> None: + """Initialize from another Odometry (copy constructor).""" + self.ts = odometry.ts + self.frame_id = odometry.frame_id + self.child_frame_id = odometry.child_frame_id + self.pose = PoseWithCovariance(odometry.pose) + self.twist = TwistWithCovariance(odometry.twist) + + @dispatch + def __init__(self, lcm_odometry: LCMOdometry) -> None: + """Initialize from an LCM Odometry.""" + self.ts = lcm_odometry.header.stamp.sec + (lcm_odometry.header.stamp.nsec / 1_000_000_000) + self.frame_id = lcm_odometry.header.frame_id + self.child_frame_id = lcm_odometry.child_frame_id + self.pose = PoseWithCovariance(lcm_odometry.pose) + self.twist = TwistWithCovariance(lcm_odometry.twist) + + @dispatch + def __init__( + self, + odometry_dict: dict[ + str, float | str | PoseWithCovariance | TwistWithCovariance | Pose | Twist + ], + ) -> None: + """Initialize from a dictionary.""" + self.ts = odometry_dict.get("ts", odometry_dict.get("timestamp", time.time())) + self.frame_id = odometry_dict.get("frame_id", "") + self.child_frame_id = odometry_dict.get("child_frame_id", "") + + # Handle pose + pose = odometry_dict.get("pose") + if pose is None: + self.pose = PoseWithCovariance() + elif isinstance(pose, PoseWithCovariance): + self.pose = pose + elif isinstance(pose, Pose): + self.pose = PoseWithCovariance(pose) + else: + self.pose = PoseWithCovariance(Pose(pose)) + + # Handle twist + twist = odometry_dict.get("twist") + if twist is None: + self.twist = TwistWithCovariance() + elif isinstance(twist, TwistWithCovariance): + self.twist = twist + elif isinstance(twist, Twist): + self.twist = TwistWithCovariance(twist) + else: + self.twist = TwistWithCovariance(Twist(twist)) + + @property + def position(self) -> Vector3: + """Get position from pose.""" + return self.pose.position + + @property + def orientation(self): + """Get orientation from pose.""" + return self.pose.orientation + + @property + def linear_velocity(self) -> Vector3: + """Get linear velocity from twist.""" + return self.twist.linear + + @property + def angular_velocity(self) -> Vector3: + """Get angular velocity from twist.""" + return self.twist.angular + + @property + def x(self) -> float: + """X position.""" + return self.pose.x + + @property + def y(self) -> float: + """Y position.""" + return self.pose.y + + @property + def z(self) -> float: + """Z position.""" + return self.pose.z + + @property + def vx(self) -> float: + """Linear velocity in X.""" + return self.twist.linear.x + + @property + def vy(self) -> float: + """Linear velocity in Y.""" + return self.twist.linear.y + + @property + def vz(self) -> float: + """Linear velocity in Z.""" + return self.twist.linear.z + + @property + def wx(self) -> float: + """Angular velocity around X (roll rate).""" + return self.twist.angular.x + + @property + def wy(self) -> float: + """Angular velocity around Y (pitch rate).""" + return self.twist.angular.y + + @property + def wz(self) -> float: + """Angular velocity around Z (yaw rate).""" + return self.twist.angular.z + + @property + def roll(self) -> float: + """Roll angle in radians.""" + return self.pose.roll + + @property + def pitch(self) -> float: + """Pitch angle in radians.""" + return self.pose.pitch + + @property + def yaw(self) -> float: + """Yaw angle in radians.""" + return self.pose.yaw + + def __repr__(self) -> str: + return ( + f"Odometry(ts={self.ts:.6f}, frame_id='{self.frame_id}', " + f"child_frame_id='{self.child_frame_id}', pose={self.pose!r}, twist={self.twist!r})" + ) + + def __str__(self) -> str: + return ( + f"Odometry:\n" + f" Timestamp: {self.ts:.6f}\n" + f" Frame: {self.frame_id} -> {self.child_frame_id}\n" + f" Position: [{self.x:.3f}, {self.y:.3f}, {self.z:.3f}]\n" + f" Orientation: [roll={self.roll:.3f}, pitch={self.pitch:.3f}, yaw={self.yaw:.3f}]\n" + f" Linear Velocity: [{self.vx:.3f}, {self.vy:.3f}, {self.vz:.3f}]\n" + f" Angular Velocity: [{self.wx:.3f}, {self.wy:.3f}, {self.wz:.3f}]" + ) + + def __eq__(self, other) -> bool: + """Check if two Odometry messages are equal.""" + if not isinstance(other, Odometry): + return False + return ( + abs(self.ts - other.ts) < 1e-6 + and self.frame_id == other.frame_id + and self.child_frame_id == other.child_frame_id + and self.pose == other.pose + and self.twist == other.twist + ) + + def lcm_encode(self) -> bytes: + """Encode to LCM binary format.""" + lcm_msg = LCMOdometry() + + # Set header + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + lcm_msg.child_frame_id = self.child_frame_id + + # Set pose with covariance + lcm_msg.pose.pose = self.pose.pose + if isinstance(self.pose.covariance, np.ndarray): + lcm_msg.pose.covariance = self.pose.covariance.tolist() + else: + lcm_msg.pose.covariance = list(self.pose.covariance) + + # Set twist with covariance + lcm_msg.twist.twist = self.twist.twist + if isinstance(self.twist.covariance, np.ndarray): + lcm_msg.twist.covariance = self.twist.covariance.tolist() + else: + lcm_msg.twist.covariance = list(self.twist.covariance) + + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> "Odometry": + """Decode from LCM binary format.""" + lcm_msg = LCMOdometry.lcm_decode(data) + + # Extract timestamp + ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) + + # Create pose with covariance + pose = Pose( + position=[ + lcm_msg.pose.pose.position.x, + lcm_msg.pose.pose.position.y, + lcm_msg.pose.pose.position.z, + ], + orientation=[ + lcm_msg.pose.pose.orientation.x, + lcm_msg.pose.pose.orientation.y, + lcm_msg.pose.pose.orientation.z, + lcm_msg.pose.pose.orientation.w, + ], + ) + pose_with_cov = PoseWithCovariance(pose, lcm_msg.pose.covariance) + + # Create twist with covariance + twist = Twist( + linear=[ + lcm_msg.twist.twist.linear.x, + lcm_msg.twist.twist.linear.y, + lcm_msg.twist.twist.linear.z, + ], + angular=[ + lcm_msg.twist.twist.angular.x, + lcm_msg.twist.twist.angular.y, + lcm_msg.twist.twist.angular.z, + ], + ) + twist_with_cov = TwistWithCovariance(twist, lcm_msg.twist.covariance) + + return cls( + ts=ts, + frame_id=lcm_msg.header.frame_id, + child_frame_id=lcm_msg.child_frame_id, + pose=pose_with_cov, + twist=twist_with_cov, + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSOdometry) -> "Odometry": + """Create an Odometry from a ROS nav_msgs/Odometry message. + + Args: + ros_msg: ROS Odometry message + + Returns: + Odometry instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + # Convert pose and twist with covariance + pose_with_cov = PoseWithCovariance.from_ros_msg(ros_msg.pose) + twist_with_cov = TwistWithCovariance.from_ros_msg(ros_msg.twist) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + child_frame_id=ros_msg.child_frame_id, + pose=pose_with_cov, + twist=twist_with_cov, + ) + + def to_ros_msg(self) -> ROSOdometry: + """Convert to a ROS nav_msgs/Odometry message. + + Returns: + ROS Odometry message + """ + ros_msg = ROSOdometry() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set child frame ID + ros_msg.child_frame_id = self.child_frame_id + + # Set pose with covariance + ros_msg.pose = self.pose.to_ros_msg() + + # Set twist with covariance + ros_msg.twist = self.twist.to_ros_msg() + + return ros_msg diff --git a/dimos/msgs/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py new file mode 100644 index 0000000000..6961cc2bed --- /dev/null +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -0,0 +1,466 @@ +# 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.nav_msgs import Odometry as LCMOdometry +from nav_msgs.msg import Odometry as ROSOdometry +from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance +from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance +from geometry_msgs.msg import Pose as ROSPose +from geometry_msgs.msg import Twist as ROSTwist +from geometry_msgs.msg import Point as ROSPoint +from geometry_msgs.msg import Quaternion as ROSQuaternion +from geometry_msgs.msg import Vector3 as ROSVector3 +from std_msgs.msg import Header as ROSHeader +from builtin_interfaces.msg import Time as ROSTime + +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry_msgs.Quaternion import Quaternion + + +def test_odometry_default_init(): + """Test default initialization.""" + odom = Odometry() + + # Should have current timestamp + assert odom.ts > 0 + assert odom.frame_id == "" + assert odom.child_frame_id == "" + + # Pose should be at origin with identity orientation + assert odom.pose.position.x == 0.0 + assert odom.pose.position.y == 0.0 + assert odom.pose.position.z == 0.0 + assert odom.pose.orientation.w == 1.0 + + # Twist should be zero + assert odom.twist.linear.x == 0.0 + assert odom.twist.linear.y == 0.0 + assert odom.twist.linear.z == 0.0 + assert odom.twist.angular.x == 0.0 + assert odom.twist.angular.y == 0.0 + assert odom.twist.angular.z == 0.0 + + # Covariances should be zero + assert np.all(odom.pose.covariance == 0.0) + assert np.all(odom.twist.covariance == 0.0) + + +def test_odometry_with_frames(): + """Test initialization with frame IDs.""" + ts = 1234567890.123456 + frame_id = "odom" + child_frame_id = "base_link" + + odom = Odometry(ts=ts, frame_id=frame_id, child_frame_id=child_frame_id) + + assert odom.ts == ts + assert odom.frame_id == frame_id + assert odom.child_frame_id == child_frame_id + + +def test_odometry_with_pose_and_twist(): + """Test initialization with pose and twist.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) + + odom = Odometry(ts=1000.0, frame_id="odom", child_frame_id="base_link", pose=pose, twist=twist) + + assert odom.pose.pose.position.x == 1.0 + assert odom.pose.pose.position.y == 2.0 + assert odom.pose.pose.position.z == 3.0 + assert odom.twist.twist.linear.x == 0.5 + assert odom.twist.twist.angular.z == 0.1 + + +def test_odometry_with_covariances(): + """Test initialization with pose and twist with covariances.""" + pose = Pose(1.0, 2.0, 3.0) + pose_cov = np.arange(36, dtype=float) + pose_with_cov = PoseWithCovariance(pose, pose_cov) + + twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) + twist_cov = np.arange(36, 72, dtype=float) + twist_with_cov = TwistWithCovariance(twist, twist_cov) + + odom = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_link", + pose=pose_with_cov, + twist=twist_with_cov, + ) + + assert odom.pose.position.x == 1.0 + assert np.array_equal(odom.pose.covariance, pose_cov) + assert odom.twist.linear.x == 0.5 + assert np.array_equal(odom.twist.covariance, twist_cov) + + +def test_odometry_copy_constructor(): + """Test copy constructor.""" + original = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_link", + pose=Pose(1.0, 2.0, 3.0), + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + ) + + copy = Odometry(original) + + assert copy == original + assert copy is not original + assert copy.pose is not original.pose + assert copy.twist is not original.twist + + +def test_odometry_dict_init(): + """Test initialization from dictionary.""" + odom_dict = { + "ts": 1000.0, + "frame_id": "odom", + "child_frame_id": "base_link", + "pose": Pose(1.0, 2.0, 3.0), + "twist": Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + } + + odom = Odometry(odom_dict) + + assert odom.ts == 1000.0 + assert odom.frame_id == "odom" + assert odom.child_frame_id == "base_link" + assert odom.pose.position.x == 1.0 + assert odom.twist.linear.x == 0.5 + + +def test_odometry_properties(): + """Test convenience properties.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) + + odom = Odometry(ts=1000.0, frame_id="odom", child_frame_id="base_link", pose=pose, twist=twist) + + # Position properties + assert odom.x == 1.0 + assert odom.y == 2.0 + assert odom.z == 3.0 + assert odom.position.x == 1.0 + assert odom.position.y == 2.0 + assert odom.position.z == 3.0 + + # Orientation properties + assert odom.orientation.x == 0.1 + assert odom.orientation.y == 0.2 + assert odom.orientation.z == 0.3 + assert odom.orientation.w == 0.9 + + # Velocity properties + assert odom.vx == 0.5 + assert odom.vy == 0.6 + assert odom.vz == 0.7 + assert odom.linear_velocity.x == 0.5 + assert odom.linear_velocity.y == 0.6 + assert odom.linear_velocity.z == 0.7 + + # Angular velocity properties + assert odom.wx == 0.1 + assert odom.wy == 0.2 + assert odom.wz == 0.3 + assert odom.angular_velocity.x == 0.1 + assert odom.angular_velocity.y == 0.2 + assert odom.angular_velocity.z == 0.3 + + # Euler angles + assert odom.roll == pose.roll + assert odom.pitch == pose.pitch + assert odom.yaw == pose.yaw + + +def test_odometry_str_repr(): + """Test string representations.""" + odom = Odometry( + ts=1234567890.123456, + frame_id="odom", + child_frame_id="base_link", + pose=Pose(1.234, 2.567, 3.891), + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + ) + + repr_str = repr(odom) + assert "Odometry" in repr_str + assert "1234567890.123456" in repr_str + assert "odom" in repr_str + assert "base_link" in repr_str + + str_repr = str(odom) + assert "Odometry" in str_repr + assert "odom -> base_link" in str_repr + assert "1.234" in str_repr + assert "0.500" in str_repr + + +def test_odometry_equality(): + """Test equality comparison.""" + odom1 = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_link", + pose=Pose(1.0, 2.0, 3.0), + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + ) + + odom2 = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_link", + pose=Pose(1.0, 2.0, 3.0), + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + ) + + odom3 = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_link", + pose=Pose(1.1, 2.0, 3.0), # Different position + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + ) + + assert odom1 == odom2 + assert odom1 != odom3 + assert odom1 != "not an odometry" + + +def test_odometry_lcm_encode_decode(): + """Test LCM encoding and decoding.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + pose_cov = np.arange(36, dtype=float) + twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) + twist_cov = np.arange(36, 72, dtype=float) + + source = Odometry( + ts=1234567890.123456, + frame_id="odom", + child_frame_id="base_link", + pose=PoseWithCovariance(pose, pose_cov), + twist=TwistWithCovariance(twist, twist_cov), + ) + + # Encode and decode + binary_msg = source.lcm_encode() + decoded = Odometry.lcm_decode(binary_msg) + + # Check values (allowing for timestamp precision loss) + assert abs(decoded.ts - source.ts) < 1e-6 + assert decoded.frame_id == source.frame_id + assert decoded.child_frame_id == source.child_frame_id + assert decoded.pose == source.pose + assert decoded.twist == source.twist + + +def test_odometry_from_ros_msg(): + """Test creating from ROS message.""" + ros_msg = ROSOdometry() + + # Set header + ros_msg.header = ROSHeader() + ros_msg.header.stamp = ROSTime() + ros_msg.header.stamp.sec = 1234567890 + ros_msg.header.stamp.nanosec = 123456000 + ros_msg.header.frame_id = "odom" + ros_msg.child_frame_id = "base_link" + + # Set pose with covariance + ros_msg.pose = ROSPoseWithCovariance() + ros_msg.pose.pose = ROSPose() + ros_msg.pose.pose.position = ROSPoint(x=1.0, y=2.0, z=3.0) + ros_msg.pose.pose.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ros_msg.pose.covariance = [float(i) for i in range(36)] + + # Set twist with covariance + ros_msg.twist = ROSTwistWithCovariance() + ros_msg.twist.twist = ROSTwist() + ros_msg.twist.twist.linear = ROSVector3(x=0.5, y=0.6, z=0.7) + ros_msg.twist.twist.angular = ROSVector3(x=0.1, y=0.2, z=0.3) + ros_msg.twist.covariance = [float(i) for i in range(36, 72)] + + odom = Odometry.from_ros_msg(ros_msg) + + assert odom.ts == 1234567890.123456 + assert odom.frame_id == "odom" + assert odom.child_frame_id == "base_link" + assert odom.pose.position.x == 1.0 + assert odom.twist.linear.x == 0.5 + assert np.array_equal(odom.pose.covariance, np.arange(36)) + assert np.array_equal(odom.twist.covariance, np.arange(36, 72)) + + +def test_odometry_to_ros_msg(): + """Test converting to ROS message.""" + pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + pose_cov = np.arange(36, dtype=float) + twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) + twist_cov = np.arange(36, 72, dtype=float) + + odom = Odometry( + ts=1234567890.567890, + frame_id="odom", + child_frame_id="base_link", + pose=PoseWithCovariance(pose, pose_cov), + twist=TwistWithCovariance(twist, twist_cov), + ) + + ros_msg = odom.to_ros_msg() + + assert isinstance(ros_msg, ROSOdometry) + assert ros_msg.header.frame_id == "odom" + assert ros_msg.header.stamp.sec == 1234567890 + assert abs(ros_msg.header.stamp.nanosec - 567890000) < 100 # Allow small rounding error + assert ros_msg.child_frame_id == "base_link" + + # Check pose + assert ros_msg.pose.pose.position.x == 1.0 + assert ros_msg.pose.pose.position.y == 2.0 + assert ros_msg.pose.pose.position.z == 3.0 + assert ros_msg.pose.pose.orientation.x == 0.1 + assert ros_msg.pose.pose.orientation.y == 0.2 + assert ros_msg.pose.pose.orientation.z == 0.3 + assert ros_msg.pose.pose.orientation.w == 0.9 + assert list(ros_msg.pose.covariance) == list(range(36)) + + # Check twist + assert ros_msg.twist.twist.linear.x == 0.5 + assert ros_msg.twist.twist.linear.y == 0.6 + assert ros_msg.twist.twist.linear.z == 0.7 + assert ros_msg.twist.twist.angular.x == 0.1 + assert ros_msg.twist.twist.angular.y == 0.2 + assert ros_msg.twist.twist.angular.z == 0.3 + assert list(ros_msg.twist.covariance) == list(range(36, 72)) + + +def test_odometry_ros_roundtrip(): + """Test round-trip conversion with ROS messages.""" + pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) + pose_cov = np.random.rand(36) + twist = Twist(Vector3(0.55, 0.65, 0.75), Vector3(0.15, 0.25, 0.35)) + twist_cov = np.random.rand(36) + + original = Odometry( + ts=2147483647.987654, # Max int32 value for ROS Time.sec + frame_id="world", + child_frame_id="robot", + pose=PoseWithCovariance(pose, pose_cov), + twist=TwistWithCovariance(twist, twist_cov), + ) + + ros_msg = original.to_ros_msg() + restored = Odometry.from_ros_msg(ros_msg) + + # Check values (allowing for timestamp precision loss) + assert abs(restored.ts - original.ts) < 1e-6 + assert restored.frame_id == original.frame_id + assert restored.child_frame_id == original.child_frame_id + assert restored.pose == original.pose + assert restored.twist == original.twist + + +def test_odometry_zero_timestamp(): + """Test that zero timestamp gets replaced with current time.""" + odom = Odometry(ts=0.0) + + # Should have been replaced with current time + assert odom.ts > 0 + assert odom.ts <= time.time() + + +def test_odometry_with_just_pose(): + """Test initialization with just a Pose (no covariance).""" + pose = Pose(1.0, 2.0, 3.0) + + odom = Odometry(pose=pose) + + assert odom.pose.position.x == 1.0 + assert odom.pose.position.y == 2.0 + assert odom.pose.position.z == 3.0 + assert np.all(odom.pose.covariance == 0.0) # Should have zero covariance + assert np.all(odom.twist.covariance == 0.0) # Twist should also be zero + + +def test_odometry_with_just_twist(): + """Test initialization with just a Twist (no covariance).""" + twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) + + odom = Odometry(twist=twist) + + assert odom.twist.linear.x == 0.5 + assert odom.twist.angular.z == 0.1 + assert np.all(odom.twist.covariance == 0.0) # Should have zero covariance + assert np.all(odom.pose.covariance == 0.0) # Pose should also be zero + + +@pytest.mark.parametrize( + "frame_id,child_frame_id", + [ + ("odom", "base_link"), + ("map", "odom"), + ("world", "robot"), + ("base_link", "camera_link"), + ("", ""), # Empty frames + ], +) +def test_odometry_frame_combinations(frame_id, child_frame_id): + """Test various frame ID combinations.""" + odom = Odometry(frame_id=frame_id, child_frame_id=child_frame_id) + + assert odom.frame_id == frame_id + assert odom.child_frame_id == child_frame_id + + # Test roundtrip through ROS + ros_msg = odom.to_ros_msg() + assert ros_msg.header.frame_id == frame_id + assert ros_msg.child_frame_id == child_frame_id + + restored = Odometry.from_ros_msg(ros_msg) + assert restored.frame_id == frame_id + assert restored.child_frame_id == child_frame_id + + +def test_odometry_typical_robot_scenario(): + """Test a typical robot odometry scenario.""" + # Robot moving forward at 0.5 m/s with slight rotation + odom = Odometry( + ts=1000.0, + frame_id="odom", + child_frame_id="base_footprint", + pose=Pose(10.0, 5.0, 0.0, 0.0, 0.0, np.sin(0.1), np.cos(0.1)), # 0.2 rad yaw + twist=Twist( + Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.05) + ), # Moving forward, turning slightly + ) + + # Check we can access all the typical properties + assert odom.x == 10.0 + assert odom.y == 5.0 + assert odom.z == 0.0 + assert abs(odom.yaw - 0.2) < 0.01 # Approximately 0.2 radians + assert odom.vx == 0.5 # Forward velocity + assert odom.wz == 0.05 # Yaw rate From a77b8c007b0943e6c8a7c2d23e8cffd0bafe37ae Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Thu, 11 Sep 2025 10:21:10 -0700 Subject: [PATCH 06/21] added topic remapping to ros_bridge --- dimos/robot/ros_bridge.py | 32 ++++- dimos/robot/test_ros_bridge.py | 253 +++++++++++++++++++++++++++++++++ 2 files changed, 279 insertions(+), 6 deletions(-) diff --git a/dimos/robot/ros_bridge.py b/dimos/robot/ros_bridge.py index 450b3eb317..55dea29442 100644 --- a/dimos/robot/ros_bridge.py +++ b/dimos/robot/ros_bridge.py @@ -77,7 +77,12 @@ def _ros_spin(self): self._executor.shutdown() def add_topic( - self, topic_name: str, dimos_type: Type, ros_type: Type, direction: BridgeDirection + self, + topic_name: str, + dimos_type: Type, + ros_type: Type, + direction: BridgeDirection, + remap_topic: Optional[str] = None, ) -> None: """Add unidirectional bridging for a topic. @@ -86,13 +91,24 @@ def add_topic( dimos_type: DIMOS message type (e.g., dimos.msgs.geometry_msgs.Twist) ros_type: ROS message type (e.g., geometry_msgs.msg.Twist) direction: Direction of bridging (ROS_TO_DIMOS or DIMOS_TO_ROS) + remap_topic: Optional remapped topic name for the other side """ if topic_name in self._bridges: logger.warning(f"Topic {topic_name} already bridged") return + # Determine actual topic names for each side + ros_topic_name = topic_name + dimos_topic_name = topic_name + + if remap_topic: + if direction == BridgeDirection.ROS_TO_DIMOS: + dimos_topic_name = remap_topic + else: # DIMOS_TO_ROS + ros_topic_name = remap_topic + # Create DIMOS/LCM topic - dimos_topic = Topic(topic_name, dimos_type) + dimos_topic = Topic(dimos_topic_name, dimos_type) ros_subscription = None ros_publisher = None @@ -104,18 +120,18 @@ def ros_callback(msg): self._ros_to_dimos(msg, dimos_topic, dimos_type, topic_name) ros_subscription = self.node.create_subscription( - ros_type, topic_name, ros_callback, self._qos + ros_type, ros_topic_name, ros_callback, self._qos ) - logger.info(f" ROS → DIMOS: Subscribing to ROS topic {topic_name}") + logger.info(f" ROS → DIMOS: Subscribing to ROS topic {ros_topic_name}") elif direction == BridgeDirection.DIMOS_TO_ROS: - ros_publisher = self.node.create_publisher(ros_type, topic_name, self._qos) + ros_publisher = self.node.create_publisher(ros_type, ros_topic_name, self._qos) def dimos_callback(msg, _topic): self._dimos_to_ros(msg, ros_publisher, topic_name) dimos_subscription = self.lcm.subscribe(dimos_topic, dimos_callback) - logger.info(f" DIMOS → ROS: Subscribing to DIMOS topic {topic_name}") + logger.info(f" DIMOS → ROS: Subscribing to DIMOS topic {dimos_topic_name}") else: raise ValueError(f"Invalid bridge direction: {direction}") @@ -127,6 +143,8 @@ def dimos_callback(msg, _topic): "ros_publisher": ros_publisher, "dimos_subscription": dimos_subscription, "direction": direction, + "ros_topic_name": ros_topic_name, + "dimos_topic_name": dimos_topic_name, } direction_str = { @@ -135,6 +153,8 @@ def dimos_callback(msg, _topic): }[direction] logger.info(f"Bridged topic: {topic_name} ({direction_str})") + if remap_topic: + logger.info(f" Remapped: ROS '{ros_topic_name}' ↔ DIMOS '{dimos_topic_name}'") logger.info(f" DIMOS type: {dimos_type.__name__}, ROS type: {ros_type.__name__}") def _ros_to_dimos( diff --git a/dimos/robot/test_ros_bridge.py b/dimos/robot/test_ros_bridge.py index bd49238c77..a02967e62a 100644 --- a/dimos/robot/test_ros_bridge.py +++ b/dimos/robot/test_ros_bridge.py @@ -475,6 +475,192 @@ def test_multiple_message_types(bridge): assert bridge.lcm.subscribe.call_count == dimos_to_ros_count +def test_topic_remapping_ros_to_dimos(bridge): + """Test remapping topic names for ROS to DIMOS direction.""" + ros_topic = "/cmd_vel" + dimos_remapped = "/robot/velocity_command" + + bridge.add_topic( + ros_topic, + Twist, + ROSTwist, + direction=BridgeDirection.ROS_TO_DIMOS, + remap_topic=dimos_remapped, + ) + + # Verify ROS subscribes to original topic + bridge.node.create_subscription.assert_called_once() + call_args = bridge.node.create_subscription.call_args + assert call_args[0][1] == ros_topic # ROS side uses original topic + + # Verify bridge metadata contains both names + assert ros_topic in bridge._bridges + bridge_info = bridge._bridges[ros_topic] + assert bridge_info["ros_topic_name"] == ros_topic + assert bridge_info["dimos_topic_name"] == dimos_remapped + assert bridge_info["dimos_topic"].topic == dimos_remapped + + +def test_topic_remapping_dimos_to_ros(bridge): + """Test remapping topic names for DIMOS to ROS direction.""" + dimos_topic = "/velocity_command" + ros_remapped = "/mobile_base/cmd_vel" + + bridge.add_topic( + dimos_topic, + Twist, + ROSTwist, + direction=BridgeDirection.DIMOS_TO_ROS, + remap_topic=ros_remapped, + ) + + # Verify ROS publishes to remapped topic + bridge.node.create_publisher.assert_called_once_with(ROSTwist, ros_remapped, bridge._qos) + + # Verify DIMOS subscribes to original topic + bridge.lcm.subscribe.assert_called_once() + dimos_topic_arg = bridge.lcm.subscribe.call_args[0][0] + assert dimos_topic_arg.topic == dimos_topic + + # Verify bridge metadata + assert dimos_topic in bridge._bridges + bridge_info = bridge._bridges[dimos_topic] + assert bridge_info["ros_topic_name"] == ros_remapped + assert bridge_info["dimos_topic_name"] == dimos_topic + + +def test_remapped_message_flow_ros_to_dimos(bridge): + """Test message flow with remapped topics from ROS to DIMOS.""" + ros_topic = "/ros/cmd_vel" + dimos_remapped = "/dimos/velocity" + + bridge.add_topic( + ros_topic, + Twist, + ROSTwist, + direction=BridgeDirection.ROS_TO_DIMOS, + remap_topic=dimos_remapped, + ) + + # Get the ROS callback + ros_callback = bridge.node.create_subscription.call_args[0][2] + + # Send a ROS message + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=2.0, y=3.0, z=4.0) + ros_msg.angular = ROSVector3(x=0.2, y=0.3, z=0.4) + + ros_callback(ros_msg) + + # Verify DIMOS publishes to remapped topic + bridge.lcm.publish.assert_called_once() + published_topic, published_msg = bridge.lcm.publish.call_args[0] + assert published_topic.topic == dimos_remapped + assert isinstance(published_msg, Twist) + assert published_msg.linear.x == 2.0 + + +def test_remapped_message_flow_dimos_to_ros(bridge): + """Test message flow with remapped topics from DIMOS to ROS.""" + dimos_topic = "/dimos/velocity" + ros_remapped = "/ros/cmd_vel" + + bridge.add_topic( + dimos_topic, + Twist, + ROSTwist, + direction=BridgeDirection.DIMOS_TO_ROS, + remap_topic=ros_remapped, + ) + + # Get the DIMOS callback + dimos_callback = bridge.lcm.subscribe.call_args[0][1] + + # Send a DIMOS message + dimos_msg = Twist(linear=Vector3(5.0, 6.0, 7.0), angular=Vector3(0.5, 0.6, 0.7)) + + ros_publisher = bridge.node.create_publisher.return_value + dimos_callback(dimos_msg, None) + + # Verify ROS publishes to remapped topic + ros_publisher.publish.assert_called_once() + published_msg = ros_publisher.publish.call_args[0][0] + assert isinstance(published_msg, ROSTwist) + assert published_msg.linear.x == 5.0 + + +def test_multiple_remapped_topics(bridge): + """Test multiple topics with remapping.""" + topic_configs = [ + # (original_name, dimos_type, ros_type, direction, remap_name) + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS, "/robot/velocity"), + ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/odometry"), + ("/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/navigation/global_path"), + ( + "/goal", + PoseStamped, + ROSPoseStamped, + BridgeDirection.DIMOS_TO_ROS, + "/navigation/goal_pose", + ), + ] + + for topic, dimos_type, ros_type, direction, remap in topic_configs: + bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) + + assert len(bridge._bridges) == 4 + + # Verify ROS to DIMOS remapping + assert bridge._bridges["/cmd_vel"]["dimos_topic_name"] == "/robot/velocity" + assert bridge._bridges["/odom"]["dimos_topic_name"] == "/robot/odometry" + + # Verify DIMOS to ROS remapping + assert bridge._bridges["/path"]["ros_topic_name"] == "/navigation/global_path" + assert bridge._bridges["/goal"]["ros_topic_name"] == "/navigation/goal_pose" + + +def test_no_remapping_when_none(bridge): + """Test that topics work normally when remap_topic is None.""" + topic = "/cmd_vel" + + bridge.add_topic( + topic, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS, remap_topic=None + ) + + bridge_info = bridge._bridges[topic] + assert bridge_info["ros_topic_name"] == topic + assert bridge_info["dimos_topic_name"] == topic + + +def test_stress_remapped_topics(bridge): + """Test stress scenario with remapped topics.""" + num_messages = 100 + ros_topic = "/ros/high_freq" + dimos_remapped = "/dimos/data_stream" + + bridge.add_topic( + ros_topic, + Twist, + ROSTwist, + direction=BridgeDirection.ROS_TO_DIMOS, + remap_topic=dimos_remapped, + ) + + ros_callback = bridge.node.create_subscription.call_args[0][2] + + for i in range(num_messages): + ros_msg = ROSTwist() + ros_msg.linear = ROSVector3(x=float(i), y=float(i * 2), z=float(i * 3)) + ros_callback(ros_msg) + + assert bridge.lcm.publish.call_count == num_messages + + # Verify all published to remapped topic + for call in bridge.lcm.publish.call_args_list: + topic, _ = call[0] + assert topic.topic == dimos_remapped + + def test_navigation_stack_topics(bridge): """Test common navigation stack topics.""" nav_topics = [ @@ -654,3 +840,70 @@ def test_autonomous_vehicle_topics(bridge): assert len(control_topics) == 4 # steering, throttle, brake, planned_trajectory assert len(feedback_topics) == 2 # pose, current_path + + +def test_remapping_with_navigation_stack(bridge): + """Test remapping with common navigation stack patterns.""" + # Map ROS2 Nav2 topics to custom DIMOS topics + nav_remapping = [ + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS, "/nav2/cmd_vel"), + ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/odometry"), + ("/global_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/nav2/plan"), + ("/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/nav2/local_plan"), + ("/goal_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/goal"), + ] + + for topic, dimos_type, ros_type, direction, remap in nav_remapping: + bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) + + # Verify DIMOS to ROS remappings + assert bridge._bridges["/cmd_vel"]["ros_topic_name"] == "/nav2/cmd_vel" + assert bridge._bridges["/global_plan"]["ros_topic_name"] == "/nav2/plan" + assert bridge._bridges["/local_plan"]["ros_topic_name"] == "/nav2/local_plan" + + # Verify ROS to DIMOS remappings + assert bridge._bridges["/odom"]["dimos_topic_name"] == "/robot/odometry" + assert bridge._bridges["/goal_pose"]["dimos_topic_name"] == "/robot/goal" + + +def test_remapping_with_robot_namespace(bridge): + """Test remapping for multi-robot systems with namespaces.""" + robot_id = "robot1" + + # Remap topics to include robot namespace + topics_with_namespace = [ + ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS, f"/{robot_id}/cmd_vel"), + ("/pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, f"/{robot_id}/pose"), + ("/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, f"/{robot_id}/path"), + ] + + for topic, dimos_type, ros_type, direction, remap in topics_with_namespace: + bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) + + # Verify all topics are properly namespaced + assert bridge._bridges["/cmd_vel"]["ros_topic_name"] == "/robot1/cmd_vel" + assert bridge._bridges["/pose"]["dimos_topic_name"] == "/robot1/pose" + assert bridge._bridges["/path"]["ros_topic_name"] == "/robot1/path" + + +def test_remapping_preserves_original_key(bridge): + """Test that remapping preserves the original topic name as the key.""" + original_topic = "/original_topic" + remapped_name = "/remapped_topic" + + bridge.add_topic( + original_topic, + Twist, + ROSTwist, + direction=BridgeDirection.ROS_TO_DIMOS, + remap_topic=remapped_name, + ) + + # Original topic name should be the key + assert original_topic in bridge._bridges + assert remapped_name not in bridge._bridges + + # Bridge info should contain both names + bridge_info = bridge._bridges[original_topic] + assert bridge_info["ros_topic_name"] == original_topic + assert bridge_info["dimos_topic_name"] == remapped_name From 5e9153ee57f812ae34d3cff24a4fdf1a7932f1ce Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Thu, 11 Sep 2025 14:46:35 -0700 Subject: [PATCH 07/21] removed unsed functions from connection module --- dimos/robot/unitree_webrtc/unitree_g1.py | 57 ++++++++---------------- 1 file changed, 19 insertions(+), 38 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index b108661878..38fd0b2899 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -24,11 +24,10 @@ from typing import Optional from dimos import core -from dimos.core import Module, In, Out, rpc -from dimos.msgs.geometry_msgs import PoseStamped, Transform, Twist, TwistStamped +from dimos.core import Module, In, rpc +from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped from dimos.protocol import pubsub from dimos.protocol.pubsub.lcmpubsub import LCM -from dimos.protocol.tf import TF from dimos.robot.foxglove_bridge import FoxgloveBridge from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection @@ -55,16 +54,12 @@ class G1ConnectionModule(Module): """Simplified connection module for G1 - uses WebRTC for control.""" movecmd: In[TwistStamped] = None - odom: Out[PoseStamped] = None ip: str connection_type: str = "webrtc" - _odom: PoseStamped = None - def __init__(self, ip: str = None, connection_type: str = "webrtc", *args, **kwargs): self.ip = ip self.connection_type = connection_type - self.tf = TF() self.connection = None Module.__init__(self, *args, **kwargs) @@ -73,38 +68,14 @@ def start(self): """Start the connection and subscribe to sensor streams.""" # Use the exact same UnitreeWebRTCConnection as Go2 self.connection = UnitreeWebRTCConnection(self.ip) - - # Subscribe only to odometry (no video/lidar for G1) - self.connection.odom_stream().subscribe(self._publish_tf) self.movecmd.subscribe(self.move) - def _publish_tf(self, msg): - """Publish odometry and TF transforms.""" - self._odom = msg - self.odom.publish(msg) - self.tf.publish(Transform.from_pose("base_link", msg)) - - @rpc - def get_odom(self) -> Optional[PoseStamped]: - """Get the robot's odometry.""" - return self._odom - @rpc def move(self, twist_stamped: TwistStamped, duration: float = 0.0): """Send movement command to robot.""" twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular) self.connection.move(twist, duration) - @rpc - def standup(self): - """Make the robot stand up.""" - return self.connection.standup() - - @rpc - def liedown(self): - """Make the robot lie down.""" - return self.connection.liedown() - @rpc def publish_request(self, topic: str, data: dict): """Forward WebRTC publish requests to connection.""" @@ -123,6 +94,8 @@ def __init__( recording_path: str = None, replay_path: str = None, enable_joystick: bool = False, + enable_connection: bool = True, + enable_ros_bridge: bool = True, ): """Initialize the G1 robot. @@ -134,6 +107,8 @@ def __init__( recording_path: Path to save recordings (if recording) replay_path: Path to replay recordings from (if replaying) enable_joystick: Enable pygame joystick control + enable_connection: Enable robot connection module + enable_ros_bridge: Enable ROS bridge """ super().__init__() self.ip = ip @@ -141,6 +116,8 @@ def __init__( self.recording_path = recording_path self.replay_path = replay_path self.enable_joystick = enable_joystick + self.enable_connection = enable_connection + self.enable_ros_bridge = enable_ros_bridge self.websocket_port = websocket_port self.lcm = LCM() @@ -173,13 +150,17 @@ def start(self): """Start the robot system with all modules.""" self.dimos = core.start(4) # 2 workers for connection and visualization - self._deploy_connection() + if self.enable_connection: + self._deploy_connection() + self._deploy_visualization() if self.enable_joystick: self._deploy_joystick() - self._deploy_ros_bridge() + if self.enable_ros_bridge: + self._deploy_ros_bridge() + self._start_modules() self.lcm.start() @@ -192,7 +173,6 @@ def _deploy_connection(self): self.connection = self.dimos.deploy(G1ConnectionModule, self.ip) # Configure LCM transports - self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped) def _deploy_camera(self): @@ -239,8 +219,7 @@ def _deploy_visualization(self): self.websocket_vis = self.dimos.deploy(WebsocketVisModule, port=self.websocket_port) self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) - # Connect to robot pose - self.websocket_vis.robot_pose.connect(self.connection.odom) + # Note: robot_pose connection removed since odom was removed from G1ConnectionModule # Deploy Foxglove bridge self.foxglove_bridge = FoxgloveBridge() @@ -267,7 +246,8 @@ def _deploy_ros_bridge(self): def _start_modules(self): """Start all deployed modules.""" - self.connection.start() + if self.connection: + self.connection.start() self.websocket_vis.start() self.foxglove_bridge.start() @@ -290,7 +270,8 @@ def move(self, twist_stamped: TwistStamped, duration: float = 0.0): def get_odom(self) -> PoseStamped: """Get the robot's odometry.""" - return self.connection.get_odom() + # Note: odom functionality removed from G1ConnectionModule + return None def shutdown(self): """Shutdown the robot and clean up resources.""" From 93da41f301dce73e2d67faf81e10d4bfcd12df65 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Thu, 11 Sep 2025 18:24:47 -0700 Subject: [PATCH 08/21] added tf message passthrough --- dimos/msgs/nav_msgs/__init__.py | 3 +- dimos/msgs/tf2_msgs/TFMessage.py | 1 + dimos/robot/unitree_webrtc/unitree_g1.py | 47 ++++++++++++++++++++++-- 3 files changed, 47 insertions(+), 4 deletions(-) diff --git a/dimos/msgs/nav_msgs/__init__.py b/dimos/msgs/nav_msgs/__init__.py index 3e4241daa0..9ea87f3f78 100644 --- a/dimos/msgs/nav_msgs/__init__.py +++ b/dimos/msgs/nav_msgs/__init__.py @@ -1,4 +1,5 @@ from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, MapMetaData, OccupancyGrid from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.nav_msgs.Odometry import Odometry -__all__ = ["Path", "OccupancyGrid", "MapMetaData", "CostValues"] +__all__ = ["Path", "OccupancyGrid", "MapMetaData", "CostValues", "Odometry"] diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index 33576a72dc..3d61c37a16 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -35,6 +35,7 @@ from dimos_lcm.std_msgs import Time as LCMTime from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage from tf2_msgs.msg import TFMessage as ROSTFMessage +from geometry_msgs.msg import TransformStamped as ROSTransformStamped from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 38fd0b2899..a833ad07a8 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -24,8 +24,12 @@ from typing import Optional from dimos import core -from dimos.core import Module, In, rpc +from dimos.core import Module, In, Out, rpc from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs import Image +from dimos_lcm.sensor_msgs import CameraInfo +from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol import pubsub from dimos.protocol.pubsub.lcmpubsub import LCM from dimos.robot.foxglove_bridge import FoxgloveBridge @@ -34,9 +38,11 @@ from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills from dimos.robot.ros_bridge import ROSBridge, BridgeDirection from geometry_msgs.msg import TwistStamped as ROSTwistStamped +from nav_msgs.msg import Odometry as ROSOdometry +from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot -from dimos.hardware.zed_camera import ZEDModule +#from dimos.hardware.zed_camera import ZEDModule from dimos.types.robot_capabilities import RobotCapability from dimos.utils.logging_config import setup_logger @@ -54,6 +60,9 @@ class G1ConnectionModule(Module): """Simplified connection module for G1 - uses WebRTC for control.""" movecmd: In[TwistStamped] = None + odom_in: In[Odometry] = None + + odom_pose: Out[PoseStamped] = None ip: str connection_type: str = "webrtc" @@ -69,6 +78,15 @@ def start(self): # Use the exact same UnitreeWebRTCConnection as Go2 self.connection = UnitreeWebRTCConnection(self.ip) self.movecmd.subscribe(self.move) + self.odom_in.subscribe(self._publish_odom_pose) + + def _publish_odom_pose(self, msg: Odometry): + self.odom_pose.publish(PoseStamped( + ts=msg.ts, + frame_id=msg.frame_id, + position=msg.pose.pose.position, + orientation=msg.pose.orientation + )) @rpc def move(self, twist_stamped: TwistStamped, duration: float = 0.0): @@ -96,6 +114,7 @@ def __init__( enable_joystick: bool = False, enable_connection: bool = True, enable_ros_bridge: bool = True, + enable_camera: bool = False, ): """Initialize the G1 robot. @@ -109,6 +128,7 @@ def __init__( enable_joystick: Enable pygame joystick control enable_connection: Enable robot connection module enable_ros_bridge: Enable ROS bridge + enable_camera: Enable ZED camera module """ super().__init__() self.ip = ip @@ -118,6 +138,7 @@ def __init__( self.enable_joystick = enable_joystick self.enable_connection = enable_connection self.enable_ros_bridge = enable_ros_bridge + self.enable_camera = enable_camera self.websocket_port = websocket_port self.lcm = LCM() @@ -138,6 +159,7 @@ def __init__( self.foxglove_bridge = None self.joystick = None self.ros_bridge = None + self.zed_camera = None self._setup_directories() @@ -155,6 +177,9 @@ def start(self): self._deploy_visualization() + if self.enable_camera: + self._deploy_camera() + if self.enable_joystick: self._deploy_joystick() @@ -174,6 +199,8 @@ def _deploy_connection(self): # Configure LCM transports self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped) + self.connection.odom_in.transport = core.LCMTransport("/state_estimation", Odometry) + self.connection.odom_pose.transport = core.LCMTransport("/odom", PoseStamped) def _deploy_camera(self): """Deploy and configure the ZED camera module (real or fake based on replay_path).""" @@ -242,7 +269,17 @@ def _deploy_ros_bridge(self): "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS ) - logger.info("ROS bridge deployed: /cmd_vel (ROS → DIMOS)") + # Add /state_estimation topic from ROS to DIMOS + self.ros_bridge.add_topic( + "/state_estimation", Odometry, ROSOdometry, direction=BridgeDirection.ROS_TO_DIMOS + ) + + # Add /tf topic from ROS to DIMOS + self.ros_bridge.add_topic( + "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS + ) + + logger.info("ROS bridge deployed: /cmd_vel, /state_estimation, /tf (ROS → DIMOS)") def _start_modules(self): """Start all deployed modules.""" @@ -306,6 +343,7 @@ def main(): parser = argparse.ArgumentParser(description="Unitree G1 Humanoid Robot Control") parser.add_argument("--ip", default=os.getenv("ROBOT_IP"), help="Robot IP address") parser.add_argument("--joystick", action="store_true", help="Enable pygame joystick control") + parser.add_argument("--camera", action="store_true", help="Enable ZED camera module") parser.add_argument("--output-dir", help="Output directory for logs/data") parser.add_argument("--record", help="Path to save recording") parser.add_argument("--replay", help="Path to replay recording from") @@ -324,6 +362,9 @@ def main(): recording_path=args.record, replay_path=args.replay, enable_joystick=args.joystick, + enable_camera=args.camera, + enable_connection=False, + enable_ros_bridge=True ) robot.start() From e0fe9b9cadd212806b5e1050fe695072cc7e63a9 Mon Sep 17 00:00:00 2001 From: alexlin2 <44330195+alexlin2@users.noreply.github.com> Date: Fri, 12 Sep 2025 01:25:31 +0000 Subject: [PATCH 09/21] CI code cleanup --- dimos/robot/unitree_webrtc/unitree_g1.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index a833ad07a8..1ef2709dc5 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -42,7 +42,8 @@ from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot -#from dimos.hardware.zed_camera import ZEDModule + +# from dimos.hardware.zed_camera import ZEDModule from dimos.types.robot_capabilities import RobotCapability from dimos.utils.logging_config import setup_logger @@ -81,12 +82,14 @@ def start(self): self.odom_in.subscribe(self._publish_odom_pose) def _publish_odom_pose(self, msg: Odometry): - self.odom_pose.publish(PoseStamped( - ts=msg.ts, - frame_id=msg.frame_id, - position=msg.pose.pose.position, - orientation=msg.pose.orientation - )) + self.odom_pose.publish( + PoseStamped( + ts=msg.ts, + frame_id=msg.frame_id, + position=msg.pose.pose.position, + orientation=msg.pose.orientation, + ) + ) @rpc def move(self, twist_stamped: TwistStamped, duration: float = 0.0): @@ -364,7 +367,7 @@ def main(): enable_joystick=args.joystick, enable_camera=args.camera, enable_connection=False, - enable_ros_bridge=True + enable_ros_bridge=True, ) robot.start() From 00e1b216ad3d85991445cb4e5c34a861d0808280 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Mon, 15 Sep 2025 09:15:07 -0700 Subject: [PATCH 10/21] added pointcloud2 ros conversion and cameraInfo dimos msgs, tested g1 bridge --- dimos/msgs/sensor_msgs/CameraInfo.py | 323 +++++++++++++++++++ dimos/msgs/sensor_msgs/PointCloud2.py | 139 ++++++++ dimos/msgs/sensor_msgs/__init__.py | 1 + dimos/msgs/sensor_msgs/test_CameraInfo.py | 349 +++++++++++++++++++++ dimos/msgs/sensor_msgs/test_PointCloud2.py | 142 +++++++++ dimos/robot/unitree_webrtc/unitree_g1.py | 18 +- 6 files changed, 968 insertions(+), 4 deletions(-) create mode 100644 dimos/msgs/sensor_msgs/CameraInfo.py create mode 100644 dimos/msgs/sensor_msgs/test_CameraInfo.py diff --git a/dimos/msgs/sensor_msgs/CameraInfo.py b/dimos/msgs/sensor_msgs/CameraInfo.py new file mode 100644 index 0000000000..f759735e16 --- /dev/null +++ b/dimos/msgs/sensor_msgs/CameraInfo.py @@ -0,0 +1,323 @@ +# 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 typing import List, Optional + +import numpy as np + +# Import LCM types +from dimos_lcm.sensor_msgs import CameraInfo as LCMCameraInfo +from dimos_lcm.std_msgs.Header import Header + +# Import ROS types +try: + from sensor_msgs.msg import CameraInfo as ROSCameraInfo + from std_msgs.msg import Header as ROSHeader + from sensor_msgs.msg import RegionOfInterest as ROSRegionOfInterest + ROS_AVAILABLE = True +except ImportError: + ROS_AVAILABLE = False + +from dimos.types.timestamped import Timestamped + + +class CameraInfo(Timestamped): + """Camera calibration information message.""" + + msg_name = "sensor_msgs.CameraInfo" + + def __init__( + self, + height: int = 0, + width: int = 0, + distortion_model: str = "", + D: Optional[List[float]] = None, + K: Optional[List[float]] = None, + R: Optional[List[float]] = None, + P: Optional[List[float]] = None, + binning_x: int = 0, + binning_y: int = 0, + frame_id: str = "", + ts: Optional[float] = None, + ): + """Initialize CameraInfo. + + Args: + height: Image height + width: Image width + distortion_model: Name of distortion model (e.g., "plumb_bob") + D: Distortion coefficients + K: 3x3 intrinsic camera matrix + R: 3x3 rectification matrix + P: 3x4 projection matrix + binning_x: Horizontal binning + binning_y: Vertical binning + frame_id: Frame ID + ts: Timestamp + """ + self.ts = ts if ts is not None else time.time() + self.frame_id = frame_id + self.height = height + self.width = width + self.distortion_model = distortion_model + + # Initialize distortion coefficients + self.D = D if D is not None else [] + + # Initialize 3x3 intrinsic camera matrix (row-major) + self.K = K if K is not None else [0.0] * 9 + + # Initialize 3x3 rectification matrix (row-major) + self.R = R if R is not None else [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + + # Initialize 3x4 projection matrix (row-major) + self.P = P if P is not None else [0.0] * 12 + + self.binning_x = binning_x + self.binning_y = binning_y + + # Region of interest (not used in basic implementation) + self.roi_x_offset = 0 + self.roi_y_offset = 0 + self.roi_height = 0 + self.roi_width = 0 + self.roi_do_rectify = False + + def get_K_matrix(self) -> np.ndarray: + """Get intrinsic matrix as numpy array.""" + return np.array(self.K, dtype=np.float64).reshape(3, 3) + + def get_P_matrix(self) -> np.ndarray: + """Get projection matrix as numpy array.""" + return np.array(self.P, dtype=np.float64).reshape(3, 4) + + def get_R_matrix(self) -> np.ndarray: + """Get rectification matrix as numpy array.""" + return np.array(self.R, dtype=np.float64).reshape(3, 3) + + def get_D_coeffs(self) -> np.ndarray: + """Get distortion coefficients as numpy array.""" + return np.array(self.D, dtype=np.float64) + + def set_K_matrix(self, K: np.ndarray): + """Set intrinsic matrix from numpy array.""" + if K.shape != (3, 3): + raise ValueError(f"K matrix must be 3x3, got {K.shape}") + self.K = K.flatten().tolist() + + def set_P_matrix(self, P: np.ndarray): + """Set projection matrix from numpy array.""" + if P.shape != (3, 4): + raise ValueError(f"P matrix must be 3x4, got {P.shape}") + self.P = P.flatten().tolist() + + def set_R_matrix(self, R: np.ndarray): + """Set rectification matrix from numpy array.""" + if R.shape != (3, 3): + raise ValueError(f"R matrix must be 3x3, got {R.shape}") + self.R = R.flatten().tolist() + + def set_D_coeffs(self, D: np.ndarray): + """Set distortion coefficients from numpy array.""" + self.D = D.flatten().tolist() + + def lcm_encode(self) -> bytes: + """Convert to LCM CameraInfo message.""" + msg = LCMCameraInfo() + + # Header + msg.header = Header() + msg.header.seq = 0 + msg.header.frame_id = self.frame_id + msg.header.stamp.sec = int(self.ts) + msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) + + # Image dimensions + msg.height = self.height + msg.width = self.width + + # Distortion model + msg.distortion_model = self.distortion_model + + # Distortion coefficients + msg.D_length = len(self.D) + msg.D = self.D + + # Camera matrices (all stored as row-major) + msg.K = self.K + msg.R = self.R + msg.P = self.P + + # Binning + msg.binning_x = self.binning_x + msg.binning_y = self.binning_y + + # ROI + msg.roi.x_offset = self.roi_x_offset + msg.roi.y_offset = self.roi_y_offset + msg.roi.height = self.roi_height + msg.roi.width = self.roi_width + msg.roi.do_rectify = self.roi_do_rectify + + return msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> "CameraInfo": + """Decode from LCM CameraInfo bytes.""" + msg = LCMCameraInfo.lcm_decode(data) + + # Extract timestamp + ts = msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 if hasattr(msg, "header") else None + + camera_info = cls( + height=msg.height, + width=msg.width, + distortion_model=msg.distortion_model, + D=list(msg.D) if msg.D_length > 0 else [], + K=list(msg.K), + R=list(msg.R), + P=list(msg.P), + binning_x=msg.binning_x, + binning_y=msg.binning_y, + frame_id=msg.header.frame_id if hasattr(msg, "header") else "", + ts=ts, + ) + + # Set ROI if present + if hasattr(msg, "roi"): + camera_info.roi_x_offset = msg.roi.x_offset + camera_info.roi_y_offset = msg.roi.y_offset + camera_info.roi_height = msg.roi.height + camera_info.roi_width = msg.roi.width + camera_info.roi_do_rectify = msg.roi.do_rectify + + return camera_info + + @classmethod + def from_ros_msg(cls, ros_msg: "ROSCameraInfo") -> "CameraInfo": + """Create CameraInfo from ROS sensor_msgs/CameraInfo message. + + Args: + ros_msg: ROS CameraInfo message + + Returns: + CameraInfo instance + """ + if not ROS_AVAILABLE: + raise ImportError("ROS packages not available. Cannot convert from ROS message.") + + # Extract timestamp + ts = ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9 + + camera_info = cls( + height=ros_msg.height, + width=ros_msg.width, + distortion_model=ros_msg.distortion_model, + D=list(ros_msg.d), + K=list(ros_msg.k), + R=list(ros_msg.r), + P=list(ros_msg.p), + binning_x=ros_msg.binning_x, + binning_y=ros_msg.binning_y, + frame_id=ros_msg.header.frame_id, + ts=ts, + ) + + # Set ROI + camera_info.roi_x_offset = ros_msg.roi.x_offset + camera_info.roi_y_offset = ros_msg.roi.y_offset + camera_info.roi_height = ros_msg.roi.height + camera_info.roi_width = ros_msg.roi.width + camera_info.roi_do_rectify = ros_msg.roi.do_rectify + + return camera_info + + def to_ros_msg(self) -> "ROSCameraInfo": + """Convert to ROS sensor_msgs/CameraInfo message. + + Returns: + ROS CameraInfo message + """ + if not ROS_AVAILABLE: + raise ImportError("ROS packages not available. Cannot convert to ROS message.") + + ros_msg = ROSCameraInfo() + + # Set header + ros_msg.header = ROSHeader() + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1e9) + + # Image dimensions + ros_msg.height = self.height + ros_msg.width = self.width + + # Distortion model and coefficients + ros_msg.distortion_model = self.distortion_model + ros_msg.d = self.D + + # Camera matrices (all row-major) + ros_msg.k = self.K + ros_msg.r = self.R + ros_msg.p = self.P + + # Binning + ros_msg.binning_x = self.binning_x + ros_msg.binning_y = self.binning_y + + # ROI + ros_msg.roi = ROSRegionOfInterest() + ros_msg.roi.x_offset = self.roi_x_offset + ros_msg.roi.y_offset = self.roi_y_offset + ros_msg.roi.height = self.roi_height + ros_msg.roi.width = self.roi_width + ros_msg.roi.do_rectify = self.roi_do_rectify + + return ros_msg + + def __repr__(self) -> str: + """String representation.""" + return (f"CameraInfo(height={self.height}, width={self.width}, " + f"distortion_model='{self.distortion_model}', " + f"frame_id='{self.frame_id}', ts={self.ts})") + + def __str__(self) -> str: + """Human-readable string.""" + return (f"CameraInfo:\n" + f" Resolution: {self.width}x{self.height}\n" + f" Distortion model: {self.distortion_model}\n" + f" Frame ID: {self.frame_id}\n" + f" Binning: {self.binning_x}x{self.binning_y}") + + def __eq__(self, other) -> bool: + """Check if two CameraInfo messages are equal.""" + if not isinstance(other, CameraInfo): + return False + + return ( + self.height == other.height + and self.width == other.width + and self.distortion_model == other.distortion_model + and self.D == other.D + and self.K == other.K + and self.R == other.R + and self.P == other.P + and self.binning_x == other.binning_x + and self.binning_y == other.binning_y + and self.frame_id == other.frame_id + ) \ No newline at end of file diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 2238b31025..dc93f96f70 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -28,6 +28,15 @@ from dimos_lcm.sensor_msgs.PointField import PointField from dimos_lcm.std_msgs.Header import Header +# Import ROS types +try: + from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 + from sensor_msgs.msg import PointField as ROSPointField + from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True +except ImportError: + ROS_AVAILABLE = False + from dimos.types.timestamped import Timestamped @@ -211,3 +220,133 @@ def __len__(self) -> int: def __repr__(self) -> str: """String representation.""" return f"PointCloud(points={len(self)}, frame_id='{self.frame_id}', ts={self.ts})" + + @classmethod + def from_ros_msg(cls, ros_msg: "ROSPointCloud2") -> "PointCloud2": + """Convert from ROS sensor_msgs/PointCloud2 message. + + Args: + ros_msg: ROS PointCloud2 message + + Returns: + PointCloud2 instance + """ + if not ROS_AVAILABLE: + raise ImportError("ROS packages not available. Cannot convert from ROS message.") + + # Handle empty point cloud + if ros_msg.width == 0 or ros_msg.height == 0: + pc = o3d.geometry.PointCloud() + return cls( + pointcloud=pc, + frame_id=ros_msg.header.frame_id, + ts=ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9, + ) + + # Parse field information to find X, Y, Z offsets + x_offset = y_offset = z_offset = None + for field in ros_msg.fields: + if field.name == "x": + x_offset = field.offset + elif field.name == "y": + y_offset = field.offset + elif field.name == "z": + z_offset = field.offset + + if any(offset is None for offset in [x_offset, y_offset, z_offset]): + raise ValueError("PointCloud2 message missing X, Y, or Z fields") + + # Extract points from binary data + num_points = ros_msg.width * ros_msg.height + points = np.zeros((num_points, 3), dtype=np.float32) + + data = ros_msg.data + point_step = ros_msg.point_step + + # Determine byte order + byte_order = ">" if ros_msg.is_bigendian else "<" + + for i in range(num_points): + base_offset = i * point_step + + # Extract X, Y, Z (assuming float32) + x_bytes = data[base_offset + x_offset : base_offset + x_offset + 4] + y_bytes = data[base_offset + y_offset : base_offset + y_offset + 4] + z_bytes = data[base_offset + z_offset : base_offset + z_offset + 4] + + points[i, 0] = struct.unpack(f"{byte_order}f", x_bytes)[0] + points[i, 1] = struct.unpack(f"{byte_order}f", y_bytes)[0] + points[i, 2] = struct.unpack(f"{byte_order}f", z_bytes)[0] + + # Filter out NaN and Inf values if not dense + if not ros_msg.is_dense: + mask = np.isfinite(points).all(axis=1) + points = points[mask] + + # Create Open3D point cloud + pc = o3d.geometry.PointCloud() + pc.points = o3d.utility.Vector3dVector(points) + + # Extract timestamp + ts = ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9 + + return cls( + pointcloud=pc, + frame_id=ros_msg.header.frame_id, + ts=ts, + ) + + def to_ros_msg(self) -> "ROSPointCloud2": + """Convert to ROS sensor_msgs/PointCloud2 message. + + Returns: + ROS PointCloud2 message + """ + if not ROS_AVAILABLE: + raise ImportError("ROS packages not available. Cannot convert to ROS message.") + + ros_msg = ROSPointCloud2() + + # Set header + ros_msg.header = ROSHeader() + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1e9) + + points = self.as_numpy() + + if len(points) == 0: + # Empty point cloud + ros_msg.height = 0 + ros_msg.width = 0 + ros_msg.fields = [] + ros_msg.is_bigendian = False + ros_msg.point_step = 0 + ros_msg.row_step = 0 + ros_msg.data = b"" + ros_msg.is_dense = True + return ros_msg + + # Set dimensions + ros_msg.height = 1 # Unorganized point cloud + ros_msg.width = len(points) + + # Define fields (X, Y, Z as float32) + ros_msg.fields = [ + ROSPointField(name="x", offset=0, datatype=ROSPointField.FLOAT32, count=1), + ROSPointField(name="y", offset=4, datatype=ROSPointField.FLOAT32, count=1), + ROSPointField(name="z", offset=8, datatype=ROSPointField.FLOAT32, count=1), + ] + + # Set point step and row step + ros_msg.point_step = 12 # 3 floats * 4 bytes each + ros_msg.row_step = ros_msg.point_step * ros_msg.width + + # Convert points to bytes (little endian float32) + ros_msg.data = points.astype(np.float32).tobytes() + + # Set properties + ros_msg.is_bigendian = False # Little endian + ros_msg.is_dense = True # No invalid points + + return ros_msg diff --git a/dimos/msgs/sensor_msgs/__init__.py b/dimos/msgs/sensor_msgs/__init__.py index 434ec75afb..a7afafe2f2 100644 --- a/dimos/msgs/sensor_msgs/__init__.py +++ b/dimos/msgs/sensor_msgs/__init__.py @@ -1,2 +1,3 @@ from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py new file mode 100644 index 0000000000..180e230529 --- /dev/null +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -0,0 +1,349 @@ +#!/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 numpy as np + +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + +# Try to import ROS types for testing +try: + from sensor_msgs.msg import CameraInfo as ROSCameraInfo + from sensor_msgs.msg import RegionOfInterest as ROSRegionOfInterest + from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True +except ImportError: + ROS_AVAILABLE = False + + +def test_lcm_encode_decode(): + """Test LCM encode/decode preserves CameraInfo data.""" + print("Testing CameraInfo LCM encode/decode...") + + # Create test camera info with sample calibration data + original = CameraInfo( + height=480, + width=640, + distortion_model="plumb_bob", + D=[-0.1, 0.05, 0.001, -0.002, 0.0], # 5 distortion coefficients + K=[500.0, 0.0, 320.0, # fx, 0, cx + 0.0, 500.0, 240.0, # 0, fy, cy + 0.0, 0.0, 1.0], # 0, 0, 1 + R=[1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0], + P=[500.0, 0.0, 320.0, 0.0, # fx, 0, cx, Tx + 0.0, 500.0, 240.0, 0.0, # 0, fy, cy, Ty + 0.0, 0.0, 1.0, 0.0], # 0, 0, 1, 0 + binning_x=2, + binning_y=2, + frame_id="camera_optical_frame", + ts=1234567890.123456, + ) + + # Set ROI + original.roi_x_offset = 100 + original.roi_y_offset = 50 + original.roi_height = 200 + original.roi_width = 300 + original.roi_do_rectify = True + + # Encode and decode + binary_msg = original.lcm_encode() + decoded = CameraInfo.lcm_decode(binary_msg) + + # Check basic properties + assert original.height == decoded.height, f"Height mismatch: {original.height} vs {decoded.height}" + assert original.width == decoded.width, f"Width mismatch: {original.width} vs {decoded.width}" + print(f"✓ Image dimensions preserved: {decoded.width}x{decoded.height}") + + assert original.distortion_model == decoded.distortion_model, ( + f"Distortion model mismatch: '{original.distortion_model}' vs '{decoded.distortion_model}'" + ) + print(f"✓ Distortion model preserved: '{decoded.distortion_model}'") + + # Check distortion coefficients + assert len(original.D) == len(decoded.D), f"D length mismatch: {len(original.D)} vs {len(decoded.D)}" + np.testing.assert_allclose( + original.D, decoded.D, rtol=1e-9, atol=1e-9, + err_msg="Distortion coefficients don't match" + ) + print(f"✓ Distortion coefficients preserved: {len(decoded.D)} coefficients") + + # Check camera matrices + np.testing.assert_allclose( + original.K, decoded.K, rtol=1e-9, atol=1e-9, + err_msg="K matrix doesn't match" + ) + print("✓ Intrinsic matrix K preserved") + + np.testing.assert_allclose( + original.R, decoded.R, rtol=1e-9, atol=1e-9, + err_msg="R matrix doesn't match" + ) + print("✓ Rectification matrix R preserved") + + np.testing.assert_allclose( + original.P, decoded.P, rtol=1e-9, atol=1e-9, + err_msg="P matrix doesn't match" + ) + print("✓ Projection matrix P preserved") + + # Check binning + assert original.binning_x == decoded.binning_x, f"Binning X mismatch: {original.binning_x} vs {decoded.binning_x}" + assert original.binning_y == decoded.binning_y, f"Binning Y mismatch: {original.binning_y} vs {decoded.binning_y}" + print(f"✓ Binning preserved: {decoded.binning_x}x{decoded.binning_y}") + + # Check ROI + assert original.roi_x_offset == decoded.roi_x_offset, "ROI x_offset mismatch" + assert original.roi_y_offset == decoded.roi_y_offset, "ROI y_offset mismatch" + assert original.roi_height == decoded.roi_height, "ROI height mismatch" + assert original.roi_width == decoded.roi_width, "ROI width mismatch" + assert original.roi_do_rectify == decoded.roi_do_rectify, "ROI do_rectify mismatch" + print("✓ ROI preserved") + + # Check metadata + assert original.frame_id == decoded.frame_id, ( + f"Frame ID mismatch: '{original.frame_id}' vs '{decoded.frame_id}'" + ) + print(f"✓ Frame ID preserved: '{decoded.frame_id}'") + + assert abs(original.ts - decoded.ts) < 1e-6, ( + f"Timestamp mismatch: {original.ts} vs {decoded.ts}" + ) + print(f"✓ Timestamp preserved: {decoded.ts}") + + print("✓ LCM encode/decode test passed - all properties preserved!") + + +def test_numpy_matrix_operations(): + """Test numpy matrix getter/setter operations.""" + print("\nTesting numpy matrix operations...") + + camera_info = CameraInfo() + + # Test K matrix + K = np.array([[525.0, 0.0, 319.5], + [0.0, 525.0, 239.5], + [0.0, 0.0, 1.0]]) + camera_info.set_K_matrix(K) + K_retrieved = camera_info.get_K_matrix() + np.testing.assert_allclose(K, K_retrieved, rtol=1e-9, atol=1e-9) + print("✓ K matrix setter/getter works") + + # Test P matrix + P = np.array([[525.0, 0.0, 319.5, 0.0], + [0.0, 525.0, 239.5, 0.0], + [0.0, 0.0, 1.0, 0.0]]) + camera_info.set_P_matrix(P) + P_retrieved = camera_info.get_P_matrix() + np.testing.assert_allclose(P, P_retrieved, rtol=1e-9, atol=1e-9) + print("✓ P matrix setter/getter works") + + # Test R matrix + R = np.eye(3) + camera_info.set_R_matrix(R) + R_retrieved = camera_info.get_R_matrix() + np.testing.assert_allclose(R, R_retrieved, rtol=1e-9, atol=1e-9) + print("✓ R matrix setter/getter works") + + # Test D coefficients + D = np.array([-0.2, 0.1, 0.001, -0.002, 0.05]) + camera_info.set_D_coeffs(D) + D_retrieved = camera_info.get_D_coeffs() + np.testing.assert_allclose(D, D_retrieved, rtol=1e-9, atol=1e-9) + print("✓ D coefficients setter/getter works") + + print("✓ All numpy matrix operations passed!") + + +def test_ros_conversion(): + """Test ROS message conversion preserves CameraInfo data.""" + if not ROS_AVAILABLE: + print("\nROS packages not available - skipping ROS conversion test") + return + + print("\nTesting ROS CameraInfo conversion...") + + # Create test camera info + original = CameraInfo( + height=720, + width=1280, + distortion_model="rational_polynomial", + D=[0.1, -0.2, 0.001, 0.002, -0.05, 0.01, -0.02, 0.003], # 8 coefficients + K=[600.0, 0.0, 640.0, + 0.0, 600.0, 360.0, + 0.0, 0.0, 1.0], + R=[0.999, -0.01, 0.02, + 0.01, 0.999, -0.01, + -0.02, 0.01, 0.999], + P=[600.0, 0.0, 640.0, -60.0, # Stereo baseline of 0.1m + 0.0, 600.0, 360.0, 0.0, + 0.0, 0.0, 1.0, 0.0], + binning_x=1, + binning_y=1, + frame_id="left_camera_optical", + ts=9876543210.987654, + ) + + # Set ROI + original.roi_x_offset = 200 + original.roi_y_offset = 100 + original.roi_height = 400 + original.roi_width = 800 + original.roi_do_rectify = False + + # Test 1: Convert to ROS and back + ros_msg = original.to_ros_msg() + converted = CameraInfo.from_ros_msg(ros_msg) + + # Check all properties + assert original.height == converted.height, f"Height mismatch: {original.height} vs {converted.height}" + assert original.width == converted.width, f"Width mismatch: {original.width} vs {converted.width}" + print(f"✓ Dimensions preserved: {converted.width}x{converted.height}") + + assert original.distortion_model == converted.distortion_model, ( + f"Distortion model mismatch: '{original.distortion_model}' vs '{converted.distortion_model}'" + ) + print(f"✓ Distortion model preserved: '{converted.distortion_model}'") + + np.testing.assert_allclose( + original.D, converted.D, rtol=1e-9, atol=1e-9, + err_msg="D coefficients don't match after ROS conversion" + ) + print(f"✓ Distortion coefficients preserved: {len(converted.D)} coefficients") + + np.testing.assert_allclose( + original.K, converted.K, rtol=1e-9, atol=1e-9, + err_msg="K matrix doesn't match after ROS conversion" + ) + print("✓ K matrix preserved") + + np.testing.assert_allclose( + original.R, converted.R, rtol=1e-9, atol=1e-9, + err_msg="R matrix doesn't match after ROS conversion" + ) + print("✓ R matrix preserved") + + np.testing.assert_allclose( + original.P, converted.P, rtol=1e-9, atol=1e-9, + err_msg="P matrix doesn't match after ROS conversion" + ) + print("✓ P matrix preserved") + + assert original.binning_x == converted.binning_x, "Binning X mismatch" + assert original.binning_y == converted.binning_y, "Binning Y mismatch" + print(f"✓ Binning preserved: {converted.binning_x}x{converted.binning_y}") + + assert original.roi_x_offset == converted.roi_x_offset, "ROI x_offset mismatch" + assert original.roi_y_offset == converted.roi_y_offset, "ROI y_offset mismatch" + assert original.roi_height == converted.roi_height, "ROI height mismatch" + assert original.roi_width == converted.roi_width, "ROI width mismatch" + assert original.roi_do_rectify == converted.roi_do_rectify, "ROI do_rectify mismatch" + print("✓ ROI preserved") + + assert original.frame_id == converted.frame_id, ( + f"Frame ID mismatch: '{original.frame_id}' vs '{converted.frame_id}'" + ) + print(f"✓ Frame ID preserved: '{converted.frame_id}'") + + assert abs(original.ts - converted.ts) < 1e-6, ( + f"Timestamp mismatch: {original.ts} vs {converted.ts}" + ) + print(f"✓ Timestamp preserved: {converted.ts}") + + # Test 2: Create ROS message directly and convert to DIMOS + ros_msg2 = ROSCameraInfo() + ros_msg2.header = ROSHeader() + ros_msg2.header.frame_id = "test_camera" + ros_msg2.header.stamp.sec = 1234567890 + ros_msg2.header.stamp.nanosec = 500000000 + + ros_msg2.height = 1080 + ros_msg2.width = 1920 + ros_msg2.distortion_model = "plumb_bob" + ros_msg2.d = [-0.3, 0.15, 0.0, 0.0, 0.0] + ros_msg2.k = [1000.0, 0.0, 960.0, 0.0, 1000.0, 540.0, 0.0, 0.0, 1.0] + ros_msg2.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ros_msg2.p = [1000.0, 0.0, 960.0, 0.0, 0.0, 1000.0, 540.0, 0.0, 0.0, 0.0, 1.0, 0.0] + ros_msg2.binning_x = 4 + ros_msg2.binning_y = 4 + + ros_msg2.roi = ROSRegionOfInterest() + ros_msg2.roi.x_offset = 10 + ros_msg2.roi.y_offset = 20 + ros_msg2.roi.height = 100 + ros_msg2.roi.width = 200 + ros_msg2.roi.do_rectify = True + + # Convert to DIMOS + dimos_info = CameraInfo.from_ros_msg(ros_msg2) + + assert dimos_info.height == 1080, f"Height not preserved: expected 1080, got {dimos_info.height}" + assert dimos_info.width == 1920, f"Width not preserved: expected 1920, got {dimos_info.width}" + assert dimos_info.frame_id == "test_camera", f"Frame ID not preserved: expected 'test_camera', got '{dimos_info.frame_id}'" + assert dimos_info.distortion_model == "plumb_bob", f"Distortion model not preserved" + assert len(dimos_info.D) == 5, f"Wrong number of distortion coefficients: expected 5, got {len(dimos_info.D)}" + print("✓ ROS to DIMOS conversion works correctly") + + # Test 3: Empty/minimal CameraInfo + minimal = CameraInfo(frame_id="minimal_camera", ts=1234567890.0) + minimal_ros = minimal.to_ros_msg() + minimal_converted = CameraInfo.from_ros_msg(minimal_ros) + + assert minimal.frame_id == minimal_converted.frame_id, "Minimal CameraInfo frame_id not preserved" + assert len(minimal_converted.D) == 0, "Minimal CameraInfo should have empty D" + print("✓ Minimal CameraInfo handling works") + + print("\n✓ All ROS conversion tests passed!") + + +def test_equality(): + """Test CameraInfo equality comparison.""" + print("\nTesting CameraInfo equality...") + + info1 = CameraInfo( + height=480, width=640, + distortion_model="plumb_bob", + D=[-0.1, 0.05, 0.0, 0.0, 0.0], + frame_id="camera1", + ) + + info2 = CameraInfo( + height=480, width=640, + distortion_model="plumb_bob", + D=[-0.1, 0.05, 0.0, 0.0, 0.0], + frame_id="camera1", + ) + + info3 = CameraInfo( + height=720, width=1280, # Different resolution + distortion_model="plumb_bob", + D=[-0.1, 0.05, 0.0, 0.0, 0.0], + frame_id="camera1", + ) + + assert info1 == info2, "Identical CameraInfo objects should be equal" + assert info1 != info3, "Different CameraInfo objects should not be equal" + assert info1 != "not_camera_info", "CameraInfo should not equal non-CameraInfo object" + + print("✓ Equality comparison works correctly") + + +if __name__ == "__main__": + test_lcm_encode_decode() + test_numpy_matrix_operations() + test_ros_conversion() + test_equality() + print("\n✓✓✓ All CameraInfo tests passed! ✓✓✓") \ No newline at end of file diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index eee1778680..c019b4b89f 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -14,11 +14,21 @@ # limitations under the License. import numpy as np +import struct from dimos.msgs.sensor_msgs import PointCloud2 from dimos.robot.unitree_webrtc.type.lidar import LidarMessage from dimos.utils.testing import SensorReplay +# Try to import ROS types for testing +try: + from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 + from sensor_msgs.msg import PointField as ROSPointField + from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True +except ImportError: + ROS_AVAILABLE = False + def test_lcm_encode_decode(): """Test LCM encode/decode preserves pointcloud data.""" @@ -79,3 +89,135 @@ def test_lcm_encode_decode(): print(f" - Mean: {decoded_points.mean(axis=0)}") print("✓ LCM encode/decode test passed - all properties preserved!") + + +def test_ros_conversion(): + """Test ROS message conversion preserves pointcloud data.""" + if not ROS_AVAILABLE: + print("ROS packages not available - skipping ROS conversion test") + return + + print("\nTesting ROS PointCloud2 conversion...") + + # Create a simple test point cloud + import open3d as o3d + points = np.array([ + [1.0, 2.0, 3.0], + [4.0, 5.0, 6.0], + [-1.0, -2.0, -3.0], + [0.5, 0.5, 0.5], + ], dtype=np.float32) + + pc = o3d.geometry.PointCloud() + pc.points = o3d.utility.Vector3dVector(points) + + # Create DIMOS PointCloud2 + original = PointCloud2( + pointcloud=pc, + frame_id="test_frame", + ts=1234567890.123456, + ) + + # Test 1: Convert to ROS and back + ros_msg = original.to_ros_msg() + converted = PointCloud2.from_ros_msg(ros_msg) + + # Check points are preserved + original_points = original.as_numpy() + converted_points = converted.as_numpy() + + assert len(original_points) == len(converted_points), ( + f"Point count mismatch: {len(original_points)} vs {len(converted_points)}" + ) + + np.testing.assert_allclose( + original_points, + converted_points, + rtol=1e-6, + atol=1e-6, + err_msg="Points don't match after ROS conversion", + ) + print(f"✓ Points preserved: {len(converted_points)} points match") + + # Check metadata + assert original.frame_id == converted.frame_id, ( + f"Frame ID mismatch: '{original.frame_id}' vs '{converted.frame_id}'" + ) + print(f"✓ Frame ID preserved: '{converted.frame_id}'") + + assert abs(original.ts - converted.ts) < 1e-6, ( + f"Timestamp mismatch: {original.ts} vs {converted.ts}" + ) + print(f"✓ Timestamp preserved: {converted.ts}") + + # Test 2: Create ROS message directly and convert to DIMOS + ros_msg2 = ROSPointCloud2() + ros_msg2.header = ROSHeader() + ros_msg2.header.frame_id = "ros_test_frame" + ros_msg2.header.stamp.sec = 1234567890 + ros_msg2.header.stamp.nanosec = 123456000 + + # Set up point cloud data + ros_msg2.height = 1 + ros_msg2.width = 3 + ros_msg2.fields = [ + ROSPointField(name="x", offset=0, datatype=ROSPointField.FLOAT32, count=1), + ROSPointField(name="y", offset=4, datatype=ROSPointField.FLOAT32, count=1), + ROSPointField(name="z", offset=8, datatype=ROSPointField.FLOAT32, count=1), + ] + ros_msg2.is_bigendian = False + ros_msg2.point_step = 12 + ros_msg2.row_step = 36 + + # Pack test points + test_points = np.array([ + [1.0, 2.0, 3.0], + [4.0, 5.0, 6.0], + [7.0, 8.0, 9.0], + ], dtype=np.float32) + ros_msg2.data = test_points.tobytes() + ros_msg2.is_dense = True + + # Convert to DIMOS + dimos_pc = PointCloud2.from_ros_msg(ros_msg2) + + assert dimos_pc.frame_id == "ros_test_frame", ( + f"Frame ID not preserved: expected 'ros_test_frame', got '{dimos_pc.frame_id}'" + ) + + decoded_points = dimos_pc.as_numpy() + assert len(decoded_points) == 3, ( + f"Wrong number of points: expected 3, got {len(decoded_points)}" + ) + + np.testing.assert_allclose( + test_points, + decoded_points, + rtol=1e-6, + atol=1e-6, + err_msg="Points from ROS message don't match", + ) + print("✓ ROS to DIMOS conversion works correctly") + + # Test 3: Empty point cloud + empty_pc = PointCloud2( + pointcloud=o3d.geometry.PointCloud(), + frame_id="empty_frame", + ts=1234567890.0, + ) + + empty_ros = empty_pc.to_ros_msg() + assert empty_ros.width == 0, "Empty cloud should have width 0" + assert empty_ros.height == 0, "Empty cloud should have height 0" + assert len(empty_ros.data) == 0, "Empty cloud should have no data" + + empty_converted = PointCloud2.from_ros_msg(empty_ros) + assert len(empty_converted) == 0, "Empty cloud conversion failed" + print("✓ Empty point cloud handling works") + + print("\n✓ All ROS conversion tests passed!") + + +if __name__ == "__main__": + test_lcm_encode_decode() + test_ros_conversion() diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 1ef2709dc5..cb807be3a5 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -27,8 +27,7 @@ from dimos.core import Module, In, Out, rpc from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs import Image -from dimos_lcm.sensor_msgs import CameraInfo +from dimos.msgs.sensor_msgs import Image, CameraInfo, PointCloud2 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol import pubsub from dimos.protocol.pubsub.lcmpubsub import LCM @@ -39,16 +38,22 @@ from dimos.robot.ros_bridge import ROSBridge, BridgeDirection from geometry_msgs.msg import TwistStamped as ROSTwistStamped from nav_msgs.msg import Odometry as ROSOdometry +from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot -# from dimos.hardware.zed_camera import ZEDModule from dimos.types.robot_capabilities import RobotCapability from dimos.utils.logging_config import setup_logger logger = setup_logger("dimos.robot.unitree_webrtc.unitree_g1", level=logging.INFO) +# try: +# from dimos.hardware.zed_camera import ZEDModule +# except ImportError: +# logger.warning("ZEDModule not found. Please install pyzed to use ZED camera functionality.") +# ZEDModule = None + # Suppress verbose loggers logging.getLogger("aiortc.codecs.h264").setLevel(logging.ERROR) logging.getLogger("lcm_foxglove_bridge").setLevel(logging.ERROR) @@ -282,7 +287,12 @@ def _deploy_ros_bridge(self): "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS ) - logger.info("ROS bridge deployed: /cmd_vel, /state_estimation, /tf (ROS → DIMOS)") + # Add /registered_scan topic from ROS to DIMOS + self.ros_bridge.add_topic( + "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS + ) + + logger.info("ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)") def _start_modules(self): """Start all deployed modules.""" From 5cdc0611b7589715345d436a46453bb6db752edc Mon Sep 17 00:00:00 2001 From: alexlin2 <44330195+alexlin2@users.noreply.github.com> Date: Mon, 15 Sep 2025 16:15:38 +0000 Subject: [PATCH 11/21] CI code cleanup --- dimos/msgs/sensor_msgs/CameraInfo.py | 127 ++++++----- dimos/msgs/sensor_msgs/PointCloud2.py | 55 ++--- dimos/msgs/sensor_msgs/test_CameraInfo.py | 254 +++++++++++++-------- dimos/msgs/sensor_msgs/test_PointCloud2.py | 72 +++--- dimos/robot/unitree_webrtc/unitree_g1.py | 4 +- 5 files changed, 290 insertions(+), 222 deletions(-) diff --git a/dimos/msgs/sensor_msgs/CameraInfo.py b/dimos/msgs/sensor_msgs/CameraInfo.py index f759735e16..70a99e35ec 100644 --- a/dimos/msgs/sensor_msgs/CameraInfo.py +++ b/dimos/msgs/sensor_msgs/CameraInfo.py @@ -28,6 +28,7 @@ from sensor_msgs.msg import CameraInfo as ROSCameraInfo from std_msgs.msg import Header as ROSHeader from sensor_msgs.msg import RegionOfInterest as ROSRegionOfInterest + ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False @@ -37,9 +38,9 @@ class CameraInfo(Timestamped): """Camera calibration information message.""" - + msg_name = "sensor_msgs.CameraInfo" - + def __init__( self, height: int = 0, @@ -55,7 +56,7 @@ def __init__( ts: Optional[float] = None, ): """Initialize CameraInfo. - + Args: height: Image height width: Image width @@ -74,115 +75,115 @@ def __init__( self.height = height self.width = width self.distortion_model = distortion_model - + # Initialize distortion coefficients self.D = D if D is not None else [] - + # Initialize 3x3 intrinsic camera matrix (row-major) self.K = K if K is not None else [0.0] * 9 - + # Initialize 3x3 rectification matrix (row-major) self.R = R if R is not None else [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - + # Initialize 3x4 projection matrix (row-major) self.P = P if P is not None else [0.0] * 12 - + self.binning_x = binning_x self.binning_y = binning_y - + # Region of interest (not used in basic implementation) self.roi_x_offset = 0 self.roi_y_offset = 0 self.roi_height = 0 self.roi_width = 0 self.roi_do_rectify = False - + def get_K_matrix(self) -> np.ndarray: """Get intrinsic matrix as numpy array.""" return np.array(self.K, dtype=np.float64).reshape(3, 3) - + def get_P_matrix(self) -> np.ndarray: """Get projection matrix as numpy array.""" return np.array(self.P, dtype=np.float64).reshape(3, 4) - + def get_R_matrix(self) -> np.ndarray: """Get rectification matrix as numpy array.""" return np.array(self.R, dtype=np.float64).reshape(3, 3) - + def get_D_coeffs(self) -> np.ndarray: """Get distortion coefficients as numpy array.""" return np.array(self.D, dtype=np.float64) - + def set_K_matrix(self, K: np.ndarray): """Set intrinsic matrix from numpy array.""" if K.shape != (3, 3): raise ValueError(f"K matrix must be 3x3, got {K.shape}") self.K = K.flatten().tolist() - + def set_P_matrix(self, P: np.ndarray): """Set projection matrix from numpy array.""" if P.shape != (3, 4): raise ValueError(f"P matrix must be 3x4, got {P.shape}") self.P = P.flatten().tolist() - + def set_R_matrix(self, R: np.ndarray): """Set rectification matrix from numpy array.""" if R.shape != (3, 3): raise ValueError(f"R matrix must be 3x3, got {R.shape}") self.R = R.flatten().tolist() - + def set_D_coeffs(self, D: np.ndarray): """Set distortion coefficients from numpy array.""" self.D = D.flatten().tolist() - + def lcm_encode(self) -> bytes: """Convert to LCM CameraInfo message.""" msg = LCMCameraInfo() - + # Header msg.header = Header() msg.header.seq = 0 msg.header.frame_id = self.frame_id msg.header.stamp.sec = int(self.ts) msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - + # Image dimensions msg.height = self.height msg.width = self.width - + # Distortion model msg.distortion_model = self.distortion_model - + # Distortion coefficients msg.D_length = len(self.D) msg.D = self.D - + # Camera matrices (all stored as row-major) msg.K = self.K msg.R = self.R msg.P = self.P - + # Binning msg.binning_x = self.binning_x msg.binning_y = self.binning_y - + # ROI msg.roi.x_offset = self.roi_x_offset msg.roi.y_offset = self.roi_y_offset msg.roi.height = self.roi_height msg.roi.width = self.roi_width msg.roi.do_rectify = self.roi_do_rectify - + return msg.lcm_encode() - + @classmethod def lcm_decode(cls, data: bytes) -> "CameraInfo": """Decode from LCM CameraInfo bytes.""" msg = LCMCameraInfo.lcm_decode(data) - + # Extract timestamp ts = msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 if hasattr(msg, "header") else None - + camera_info = cls( height=msg.height, width=msg.width, @@ -196,7 +197,7 @@ def lcm_decode(cls, data: bytes) -> "CameraInfo": frame_id=msg.header.frame_id if hasattr(msg, "header") else "", ts=ts, ) - + # Set ROI if present if hasattr(msg, "roi"): camera_info.roi_x_offset = msg.roi.x_offset @@ -204,25 +205,25 @@ def lcm_decode(cls, data: bytes) -> "CameraInfo": camera_info.roi_height = msg.roi.height camera_info.roi_width = msg.roi.width camera_info.roi_do_rectify = msg.roi.do_rectify - + return camera_info - + @classmethod def from_ros_msg(cls, ros_msg: "ROSCameraInfo") -> "CameraInfo": """Create CameraInfo from ROS sensor_msgs/CameraInfo message. - + Args: ros_msg: ROS CameraInfo message - + Returns: CameraInfo instance """ if not ROS_AVAILABLE: raise ImportError("ROS packages not available. Cannot convert from ROS message.") - + # Extract timestamp ts = ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9 - + camera_info = cls( height=ros_msg.height, width=ros_msg.width, @@ -236,50 +237,50 @@ def from_ros_msg(cls, ros_msg: "ROSCameraInfo") -> "CameraInfo": frame_id=ros_msg.header.frame_id, ts=ts, ) - + # Set ROI camera_info.roi_x_offset = ros_msg.roi.x_offset camera_info.roi_y_offset = ros_msg.roi.y_offset camera_info.roi_height = ros_msg.roi.height camera_info.roi_width = ros_msg.roi.width camera_info.roi_do_rectify = ros_msg.roi.do_rectify - + return camera_info - + def to_ros_msg(self) -> "ROSCameraInfo": """Convert to ROS sensor_msgs/CameraInfo message. - + Returns: ROS CameraInfo message """ if not ROS_AVAILABLE: raise ImportError("ROS packages not available. Cannot convert to ROS message.") - + ros_msg = ROSCameraInfo() - + # Set header ros_msg.header = ROSHeader() ros_msg.header.frame_id = self.frame_id ros_msg.header.stamp.sec = int(self.ts) ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1e9) - + # Image dimensions ros_msg.height = self.height ros_msg.width = self.width - + # Distortion model and coefficients ros_msg.distortion_model = self.distortion_model ros_msg.d = self.D - + # Camera matrices (all row-major) ros_msg.k = self.K ros_msg.r = self.R ros_msg.p = self.P - + # Binning ros_msg.binning_x = self.binning_x ros_msg.binning_y = self.binning_y - + # ROI ros_msg.roi = ROSRegionOfInterest() ros_msg.roi.x_offset = self.roi_x_offset @@ -287,28 +288,32 @@ def to_ros_msg(self) -> "ROSCameraInfo": ros_msg.roi.height = self.roi_height ros_msg.roi.width = self.roi_width ros_msg.roi.do_rectify = self.roi_do_rectify - + return ros_msg - + def __repr__(self) -> str: """String representation.""" - return (f"CameraInfo(height={self.height}, width={self.width}, " - f"distortion_model='{self.distortion_model}', " - f"frame_id='{self.frame_id}', ts={self.ts})") - + return ( + f"CameraInfo(height={self.height}, width={self.width}, " + f"distortion_model='{self.distortion_model}', " + f"frame_id='{self.frame_id}', ts={self.ts})" + ) + def __str__(self) -> str: """Human-readable string.""" - return (f"CameraInfo:\n" - f" Resolution: {self.width}x{self.height}\n" - f" Distortion model: {self.distortion_model}\n" - f" Frame ID: {self.frame_id}\n" - f" Binning: {self.binning_x}x{self.binning_y}") - + return ( + f"CameraInfo:\n" + f" Resolution: {self.width}x{self.height}\n" + f" Distortion model: {self.distortion_model}\n" + f" Frame ID: {self.frame_id}\n" + f" Binning: {self.binning_x}x{self.binning_y}" + ) + def __eq__(self, other) -> bool: """Check if two CameraInfo messages are equal.""" if not isinstance(other, CameraInfo): return False - + return ( self.height == other.height and self.width == other.width @@ -320,4 +325,4 @@ def __eq__(self, other) -> bool: and self.binning_x == other.binning_x and self.binning_y == other.binning_y and self.frame_id == other.frame_id - ) \ No newline at end of file + ) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index dc93f96f70..c3a0c49bab 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -33,6 +33,7 @@ from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 from sensor_msgs.msg import PointField as ROSPointField from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False @@ -224,16 +225,16 @@ def __repr__(self) -> str: @classmethod def from_ros_msg(cls, ros_msg: "ROSPointCloud2") -> "PointCloud2": """Convert from ROS sensor_msgs/PointCloud2 message. - + Args: ros_msg: ROS PointCloud2 message - + Returns: PointCloud2 instance """ if not ROS_AVAILABLE: raise ImportError("ROS packages not available. Cannot convert from ROS message.") - + # Handle empty point cloud if ros_msg.width == 0 or ros_msg.height == 0: pc = o3d.geometry.PointCloud() @@ -242,7 +243,7 @@ def from_ros_msg(cls, ros_msg: "ROSPointCloud2") -> "PointCloud2": frame_id=ros_msg.header.frame_id, ts=ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9, ) - + # Parse field information to find X, Y, Z offsets x_offset = y_offset = z_offset = None for field in ros_msg.fields: @@ -252,69 +253,69 @@ def from_ros_msg(cls, ros_msg: "ROSPointCloud2") -> "PointCloud2": y_offset = field.offset elif field.name == "z": z_offset = field.offset - + if any(offset is None for offset in [x_offset, y_offset, z_offset]): raise ValueError("PointCloud2 message missing X, Y, or Z fields") - + # Extract points from binary data num_points = ros_msg.width * ros_msg.height points = np.zeros((num_points, 3), dtype=np.float32) - + data = ros_msg.data point_step = ros_msg.point_step - + # Determine byte order byte_order = ">" if ros_msg.is_bigendian else "<" - + for i in range(num_points): base_offset = i * point_step - + # Extract X, Y, Z (assuming float32) x_bytes = data[base_offset + x_offset : base_offset + x_offset + 4] y_bytes = data[base_offset + y_offset : base_offset + y_offset + 4] z_bytes = data[base_offset + z_offset : base_offset + z_offset + 4] - + points[i, 0] = struct.unpack(f"{byte_order}f", x_bytes)[0] points[i, 1] = struct.unpack(f"{byte_order}f", y_bytes)[0] points[i, 2] = struct.unpack(f"{byte_order}f", z_bytes)[0] - + # Filter out NaN and Inf values if not dense if not ros_msg.is_dense: mask = np.isfinite(points).all(axis=1) points = points[mask] - + # Create Open3D point cloud pc = o3d.geometry.PointCloud() pc.points = o3d.utility.Vector3dVector(points) - + # Extract timestamp ts = ros_msg.header.stamp.sec + ros_msg.header.stamp.nanosec / 1e9 - + return cls( pointcloud=pc, frame_id=ros_msg.header.frame_id, ts=ts, ) - + def to_ros_msg(self) -> "ROSPointCloud2": """Convert to ROS sensor_msgs/PointCloud2 message. - + Returns: ROS PointCloud2 message """ if not ROS_AVAILABLE: raise ImportError("ROS packages not available. Cannot convert to ROS message.") - + ros_msg = ROSPointCloud2() - + # Set header ros_msg.header = ROSHeader() ros_msg.header.frame_id = self.frame_id ros_msg.header.stamp.sec = int(self.ts) ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1e9) - + points = self.as_numpy() - + if len(points) == 0: # Empty point cloud ros_msg.height = 0 @@ -326,27 +327,27 @@ def to_ros_msg(self) -> "ROSPointCloud2": ros_msg.data = b"" ros_msg.is_dense = True return ros_msg - + # Set dimensions ros_msg.height = 1 # Unorganized point cloud ros_msg.width = len(points) - + # Define fields (X, Y, Z as float32) ros_msg.fields = [ ROSPointField(name="x", offset=0, datatype=ROSPointField.FLOAT32, count=1), ROSPointField(name="y", offset=4, datatype=ROSPointField.FLOAT32, count=1), ROSPointField(name="z", offset=8, datatype=ROSPointField.FLOAT32, count=1), ] - + # Set point step and row step ros_msg.point_step = 12 # 3 floats * 4 bytes each ros_msg.row_step = ros_msg.point_step * ros_msg.width - + # Convert points to bytes (little endian float32) ros_msg.data = points.astype(np.float32).tobytes() - + # Set properties ros_msg.is_bigendian = False # Little endian ros_msg.is_dense = True # No invalid points - + return ros_msg diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index 180e230529..e29cb2e95e 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -22,6 +22,7 @@ from sensor_msgs.msg import CameraInfo as ROSCameraInfo from sensor_msgs.msg import RegionOfInterest as ROSRegionOfInterest from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False @@ -30,81 +31,102 @@ def test_lcm_encode_decode(): """Test LCM encode/decode preserves CameraInfo data.""" print("Testing CameraInfo LCM encode/decode...") - + # Create test camera info with sample calibration data original = CameraInfo( height=480, width=640, distortion_model="plumb_bob", D=[-0.1, 0.05, 0.001, -0.002, 0.0], # 5 distortion coefficients - K=[500.0, 0.0, 320.0, # fx, 0, cx - 0.0, 500.0, 240.0, # 0, fy, cy - 0.0, 0.0, 1.0], # 0, 0, 1 - R=[1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0], - P=[500.0, 0.0, 320.0, 0.0, # fx, 0, cx, Tx - 0.0, 500.0, 240.0, 0.0, # 0, fy, cy, Ty - 0.0, 0.0, 1.0, 0.0], # 0, 0, 1, 0 + K=[ + 500.0, + 0.0, + 320.0, # fx, 0, cx + 0.0, + 500.0, + 240.0, # 0, fy, cy + 0.0, + 0.0, + 1.0, + ], # 0, 0, 1 + R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + P=[ + 500.0, + 0.0, + 320.0, + 0.0, # fx, 0, cx, Tx + 0.0, + 500.0, + 240.0, + 0.0, # 0, fy, cy, Ty + 0.0, + 0.0, + 1.0, + 0.0, + ], # 0, 0, 1, 0 binning_x=2, binning_y=2, frame_id="camera_optical_frame", ts=1234567890.123456, ) - + # Set ROI original.roi_x_offset = 100 original.roi_y_offset = 50 original.roi_height = 200 original.roi_width = 300 original.roi_do_rectify = True - + # Encode and decode binary_msg = original.lcm_encode() decoded = CameraInfo.lcm_decode(binary_msg) - + # Check basic properties - assert original.height == decoded.height, f"Height mismatch: {original.height} vs {decoded.height}" + assert original.height == decoded.height, ( + f"Height mismatch: {original.height} vs {decoded.height}" + ) assert original.width == decoded.width, f"Width mismatch: {original.width} vs {decoded.width}" print(f"✓ Image dimensions preserved: {decoded.width}x{decoded.height}") - + assert original.distortion_model == decoded.distortion_model, ( f"Distortion model mismatch: '{original.distortion_model}' vs '{decoded.distortion_model}'" ) print(f"✓ Distortion model preserved: '{decoded.distortion_model}'") - + # Check distortion coefficients - assert len(original.D) == len(decoded.D), f"D length mismatch: {len(original.D)} vs {len(decoded.D)}" + assert len(original.D) == len(decoded.D), ( + f"D length mismatch: {len(original.D)} vs {len(decoded.D)}" + ) np.testing.assert_allclose( - original.D, decoded.D, rtol=1e-9, atol=1e-9, - err_msg="Distortion coefficients don't match" + original.D, decoded.D, rtol=1e-9, atol=1e-9, err_msg="Distortion coefficients don't match" ) print(f"✓ Distortion coefficients preserved: {len(decoded.D)} coefficients") - + # Check camera matrices np.testing.assert_allclose( - original.K, decoded.K, rtol=1e-9, atol=1e-9, - err_msg="K matrix doesn't match" + original.K, decoded.K, rtol=1e-9, atol=1e-9, err_msg="K matrix doesn't match" ) print("✓ Intrinsic matrix K preserved") - + np.testing.assert_allclose( - original.R, decoded.R, rtol=1e-9, atol=1e-9, - err_msg="R matrix doesn't match" + original.R, decoded.R, rtol=1e-9, atol=1e-9, err_msg="R matrix doesn't match" ) print("✓ Rectification matrix R preserved") - + np.testing.assert_allclose( - original.P, decoded.P, rtol=1e-9, atol=1e-9, - err_msg="P matrix doesn't match" + original.P, decoded.P, rtol=1e-9, atol=1e-9, err_msg="P matrix doesn't match" ) print("✓ Projection matrix P preserved") - + # Check binning - assert original.binning_x == decoded.binning_x, f"Binning X mismatch: {original.binning_x} vs {decoded.binning_x}" - assert original.binning_y == decoded.binning_y, f"Binning Y mismatch: {original.binning_y} vs {decoded.binning_y}" + assert original.binning_x == decoded.binning_x, ( + f"Binning X mismatch: {original.binning_x} vs {decoded.binning_x}" + ) + assert original.binning_y == decoded.binning_y, ( + f"Binning Y mismatch: {original.binning_y} vs {decoded.binning_y}" + ) print(f"✓ Binning preserved: {decoded.binning_x}x{decoded.binning_y}") - + # Check ROI assert original.roi_x_offset == decoded.roi_x_offset, "ROI x_offset mismatch" assert original.roi_y_offset == decoded.roi_y_offset, "ROI y_offset mismatch" @@ -112,59 +134,55 @@ def test_lcm_encode_decode(): assert original.roi_width == decoded.roi_width, "ROI width mismatch" assert original.roi_do_rectify == decoded.roi_do_rectify, "ROI do_rectify mismatch" print("✓ ROI preserved") - + # Check metadata assert original.frame_id == decoded.frame_id, ( f"Frame ID mismatch: '{original.frame_id}' vs '{decoded.frame_id}'" ) print(f"✓ Frame ID preserved: '{decoded.frame_id}'") - + assert abs(original.ts - decoded.ts) < 1e-6, ( f"Timestamp mismatch: {original.ts} vs {decoded.ts}" ) print(f"✓ Timestamp preserved: {decoded.ts}") - + print("✓ LCM encode/decode test passed - all properties preserved!") def test_numpy_matrix_operations(): """Test numpy matrix getter/setter operations.""" print("\nTesting numpy matrix operations...") - + camera_info = CameraInfo() - + # Test K matrix - K = np.array([[525.0, 0.0, 319.5], - [0.0, 525.0, 239.5], - [0.0, 0.0, 1.0]]) + K = np.array([[525.0, 0.0, 319.5], [0.0, 525.0, 239.5], [0.0, 0.0, 1.0]]) camera_info.set_K_matrix(K) K_retrieved = camera_info.get_K_matrix() np.testing.assert_allclose(K, K_retrieved, rtol=1e-9, atol=1e-9) print("✓ K matrix setter/getter works") - + # Test P matrix - P = np.array([[525.0, 0.0, 319.5, 0.0], - [0.0, 525.0, 239.5, 0.0], - [0.0, 0.0, 1.0, 0.0]]) + P = np.array([[525.0, 0.0, 319.5, 0.0], [0.0, 525.0, 239.5, 0.0], [0.0, 0.0, 1.0, 0.0]]) camera_info.set_P_matrix(P) P_retrieved = camera_info.get_P_matrix() np.testing.assert_allclose(P, P_retrieved, rtol=1e-9, atol=1e-9) print("✓ P matrix setter/getter works") - + # Test R matrix R = np.eye(3) camera_info.set_R_matrix(R) R_retrieved = camera_info.get_R_matrix() np.testing.assert_allclose(R, R_retrieved, rtol=1e-9, atol=1e-9) print("✓ R matrix setter/getter works") - + # Test D coefficients D = np.array([-0.2, 0.1, 0.001, -0.002, 0.05]) camera_info.set_D_coeffs(D) D_retrieved = camera_info.get_D_coeffs() np.testing.assert_allclose(D, D_retrieved, rtol=1e-9, atol=1e-9) print("✓ D coefficients setter/getter works") - + print("✓ All numpy matrix operations passed!") @@ -173,103 +191,126 @@ def test_ros_conversion(): if not ROS_AVAILABLE: print("\nROS packages not available - skipping ROS conversion test") return - + print("\nTesting ROS CameraInfo conversion...") - + # Create test camera info original = CameraInfo( height=720, width=1280, distortion_model="rational_polynomial", D=[0.1, -0.2, 0.001, 0.002, -0.05, 0.01, -0.02, 0.003], # 8 coefficients - K=[600.0, 0.0, 640.0, - 0.0, 600.0, 360.0, - 0.0, 0.0, 1.0], - R=[0.999, -0.01, 0.02, - 0.01, 0.999, -0.01, - -0.02, 0.01, 0.999], - P=[600.0, 0.0, 640.0, -60.0, # Stereo baseline of 0.1m - 0.0, 600.0, 360.0, 0.0, - 0.0, 0.0, 1.0, 0.0], + K=[600.0, 0.0, 640.0, 0.0, 600.0, 360.0, 0.0, 0.0, 1.0], + R=[0.999, -0.01, 0.02, 0.01, 0.999, -0.01, -0.02, 0.01, 0.999], + P=[ + 600.0, + 0.0, + 640.0, + -60.0, # Stereo baseline of 0.1m + 0.0, + 600.0, + 360.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + ], binning_x=1, binning_y=1, frame_id="left_camera_optical", ts=9876543210.987654, ) - + # Set ROI original.roi_x_offset = 200 original.roi_y_offset = 100 original.roi_height = 400 original.roi_width = 800 original.roi_do_rectify = False - + # Test 1: Convert to ROS and back ros_msg = original.to_ros_msg() converted = CameraInfo.from_ros_msg(ros_msg) - + # Check all properties - assert original.height == converted.height, f"Height mismatch: {original.height} vs {converted.height}" - assert original.width == converted.width, f"Width mismatch: {original.width} vs {converted.width}" + assert original.height == converted.height, ( + f"Height mismatch: {original.height} vs {converted.height}" + ) + assert original.width == converted.width, ( + f"Width mismatch: {original.width} vs {converted.width}" + ) print(f"✓ Dimensions preserved: {converted.width}x{converted.height}") - + assert original.distortion_model == converted.distortion_model, ( f"Distortion model mismatch: '{original.distortion_model}' vs '{converted.distortion_model}'" ) print(f"✓ Distortion model preserved: '{converted.distortion_model}'") - + np.testing.assert_allclose( - original.D, converted.D, rtol=1e-9, atol=1e-9, - err_msg="D coefficients don't match after ROS conversion" + original.D, + converted.D, + rtol=1e-9, + atol=1e-9, + err_msg="D coefficients don't match after ROS conversion", ) print(f"✓ Distortion coefficients preserved: {len(converted.D)} coefficients") - + np.testing.assert_allclose( - original.K, converted.K, rtol=1e-9, atol=1e-9, - err_msg="K matrix doesn't match after ROS conversion" + original.K, + converted.K, + rtol=1e-9, + atol=1e-9, + err_msg="K matrix doesn't match after ROS conversion", ) print("✓ K matrix preserved") - + np.testing.assert_allclose( - original.R, converted.R, rtol=1e-9, atol=1e-9, - err_msg="R matrix doesn't match after ROS conversion" + original.R, + converted.R, + rtol=1e-9, + atol=1e-9, + err_msg="R matrix doesn't match after ROS conversion", ) print("✓ R matrix preserved") - + np.testing.assert_allclose( - original.P, converted.P, rtol=1e-9, atol=1e-9, - err_msg="P matrix doesn't match after ROS conversion" + original.P, + converted.P, + rtol=1e-9, + atol=1e-9, + err_msg="P matrix doesn't match after ROS conversion", ) print("✓ P matrix preserved") - + assert original.binning_x == converted.binning_x, "Binning X mismatch" assert original.binning_y == converted.binning_y, "Binning Y mismatch" print(f"✓ Binning preserved: {converted.binning_x}x{converted.binning_y}") - + assert original.roi_x_offset == converted.roi_x_offset, "ROI x_offset mismatch" assert original.roi_y_offset == converted.roi_y_offset, "ROI y_offset mismatch" assert original.roi_height == converted.roi_height, "ROI height mismatch" assert original.roi_width == converted.roi_width, "ROI width mismatch" assert original.roi_do_rectify == converted.roi_do_rectify, "ROI do_rectify mismatch" print("✓ ROI preserved") - + assert original.frame_id == converted.frame_id, ( f"Frame ID mismatch: '{original.frame_id}' vs '{converted.frame_id}'" ) print(f"✓ Frame ID preserved: '{converted.frame_id}'") - + assert abs(original.ts - converted.ts) < 1e-6, ( f"Timestamp mismatch: {original.ts} vs {converted.ts}" ) print(f"✓ Timestamp preserved: {converted.ts}") - + # Test 2: Create ROS message directly and convert to DIMOS ros_msg2 = ROSCameraInfo() ros_msg2.header = ROSHeader() ros_msg2.header.frame_id = "test_camera" ros_msg2.header.stamp.sec = 1234567890 ros_msg2.header.stamp.nanosec = 500000000 - + ros_msg2.height = 1080 ros_msg2.width = 1920 ros_msg2.distortion_model = "plumb_bob" @@ -279,65 +320,76 @@ def test_ros_conversion(): ros_msg2.p = [1000.0, 0.0, 960.0, 0.0, 0.0, 1000.0, 540.0, 0.0, 0.0, 0.0, 1.0, 0.0] ros_msg2.binning_x = 4 ros_msg2.binning_y = 4 - + ros_msg2.roi = ROSRegionOfInterest() ros_msg2.roi.x_offset = 10 ros_msg2.roi.y_offset = 20 ros_msg2.roi.height = 100 ros_msg2.roi.width = 200 ros_msg2.roi.do_rectify = True - + # Convert to DIMOS dimos_info = CameraInfo.from_ros_msg(ros_msg2) - - assert dimos_info.height == 1080, f"Height not preserved: expected 1080, got {dimos_info.height}" + + assert dimos_info.height == 1080, ( + f"Height not preserved: expected 1080, got {dimos_info.height}" + ) assert dimos_info.width == 1920, f"Width not preserved: expected 1920, got {dimos_info.width}" - assert dimos_info.frame_id == "test_camera", f"Frame ID not preserved: expected 'test_camera', got '{dimos_info.frame_id}'" + assert dimos_info.frame_id == "test_camera", ( + f"Frame ID not preserved: expected 'test_camera', got '{dimos_info.frame_id}'" + ) assert dimos_info.distortion_model == "plumb_bob", f"Distortion model not preserved" - assert len(dimos_info.D) == 5, f"Wrong number of distortion coefficients: expected 5, got {len(dimos_info.D)}" + assert len(dimos_info.D) == 5, ( + f"Wrong number of distortion coefficients: expected 5, got {len(dimos_info.D)}" + ) print("✓ ROS to DIMOS conversion works correctly") - + # Test 3: Empty/minimal CameraInfo minimal = CameraInfo(frame_id="minimal_camera", ts=1234567890.0) minimal_ros = minimal.to_ros_msg() minimal_converted = CameraInfo.from_ros_msg(minimal_ros) - - assert minimal.frame_id == minimal_converted.frame_id, "Minimal CameraInfo frame_id not preserved" + + assert minimal.frame_id == minimal_converted.frame_id, ( + "Minimal CameraInfo frame_id not preserved" + ) assert len(minimal_converted.D) == 0, "Minimal CameraInfo should have empty D" print("✓ Minimal CameraInfo handling works") - + print("\n✓ All ROS conversion tests passed!") def test_equality(): """Test CameraInfo equality comparison.""" print("\nTesting CameraInfo equality...") - + info1 = CameraInfo( - height=480, width=640, + height=480, + width=640, distortion_model="plumb_bob", D=[-0.1, 0.05, 0.0, 0.0, 0.0], frame_id="camera1", ) - + info2 = CameraInfo( - height=480, width=640, + height=480, + width=640, distortion_model="plumb_bob", D=[-0.1, 0.05, 0.0, 0.0, 0.0], frame_id="camera1", ) - + info3 = CameraInfo( - height=720, width=1280, # Different resolution + height=720, + width=1280, # Different resolution distortion_model="plumb_bob", D=[-0.1, 0.05, 0.0, 0.0, 0.0], frame_id="camera1", ) - + assert info1 == info2, "Identical CameraInfo objects should be equal" assert info1 != info3, "Different CameraInfo objects should not be equal" assert info1 != "not_camera_info", "CameraInfo should not equal non-CameraInfo object" - + print("✓ Equality comparison works correctly") @@ -346,4 +398,4 @@ def test_equality(): test_numpy_matrix_operations() test_ros_conversion() test_equality() - print("\n✓✓✓ All CameraInfo tests passed! ✓✓✓") \ No newline at end of file + print("\n✓✓✓ All CameraInfo tests passed! ✓✓✓") diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index c019b4b89f..776100ec2e 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -25,6 +25,7 @@ from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 from sensor_msgs.msg import PointField as ROSPointField from std_msgs.msg import Header as ROSHeader + ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False @@ -96,40 +97,44 @@ def test_ros_conversion(): if not ROS_AVAILABLE: print("ROS packages not available - skipping ROS conversion test") return - + print("\nTesting ROS PointCloud2 conversion...") - + # Create a simple test point cloud import open3d as o3d - points = np.array([ - [1.0, 2.0, 3.0], - [4.0, 5.0, 6.0], - [-1.0, -2.0, -3.0], - [0.5, 0.5, 0.5], - ], dtype=np.float32) - + + points = np.array( + [ + [1.0, 2.0, 3.0], + [4.0, 5.0, 6.0], + [-1.0, -2.0, -3.0], + [0.5, 0.5, 0.5], + ], + dtype=np.float32, + ) + pc = o3d.geometry.PointCloud() pc.points = o3d.utility.Vector3dVector(points) - + # Create DIMOS PointCloud2 original = PointCloud2( pointcloud=pc, frame_id="test_frame", ts=1234567890.123456, ) - + # Test 1: Convert to ROS and back ros_msg = original.to_ros_msg() converted = PointCloud2.from_ros_msg(ros_msg) - + # Check points are preserved original_points = original.as_numpy() converted_points = converted.as_numpy() - + assert len(original_points) == len(converted_points), ( f"Point count mismatch: {len(original_points)} vs {len(converted_points)}" ) - + np.testing.assert_allclose( original_points, converted_points, @@ -138,25 +143,25 @@ def test_ros_conversion(): err_msg="Points don't match after ROS conversion", ) print(f"✓ Points preserved: {len(converted_points)} points match") - + # Check metadata assert original.frame_id == converted.frame_id, ( f"Frame ID mismatch: '{original.frame_id}' vs '{converted.frame_id}'" ) print(f"✓ Frame ID preserved: '{converted.frame_id}'") - + assert abs(original.ts - converted.ts) < 1e-6, ( f"Timestamp mismatch: {original.ts} vs {converted.ts}" ) print(f"✓ Timestamp preserved: {converted.ts}") - + # Test 2: Create ROS message directly and convert to DIMOS ros_msg2 = ROSPointCloud2() ros_msg2.header = ROSHeader() ros_msg2.header.frame_id = "ros_test_frame" ros_msg2.header.stamp.sec = 1234567890 ros_msg2.header.stamp.nanosec = 123456000 - + # Set up point cloud data ros_msg2.height = 1 ros_msg2.width = 3 @@ -168,28 +173,31 @@ def test_ros_conversion(): ros_msg2.is_bigendian = False ros_msg2.point_step = 12 ros_msg2.row_step = 36 - + # Pack test points - test_points = np.array([ - [1.0, 2.0, 3.0], - [4.0, 5.0, 6.0], - [7.0, 8.0, 9.0], - ], dtype=np.float32) + test_points = np.array( + [ + [1.0, 2.0, 3.0], + [4.0, 5.0, 6.0], + [7.0, 8.0, 9.0], + ], + dtype=np.float32, + ) ros_msg2.data = test_points.tobytes() ros_msg2.is_dense = True - + # Convert to DIMOS dimos_pc = PointCloud2.from_ros_msg(ros_msg2) - + assert dimos_pc.frame_id == "ros_test_frame", ( f"Frame ID not preserved: expected 'ros_test_frame', got '{dimos_pc.frame_id}'" ) - + decoded_points = dimos_pc.as_numpy() assert len(decoded_points) == 3, ( f"Wrong number of points: expected 3, got {len(decoded_points)}" ) - + np.testing.assert_allclose( test_points, decoded_points, @@ -198,23 +206,23 @@ def test_ros_conversion(): err_msg="Points from ROS message don't match", ) print("✓ ROS to DIMOS conversion works correctly") - + # Test 3: Empty point cloud empty_pc = PointCloud2( pointcloud=o3d.geometry.PointCloud(), frame_id="empty_frame", ts=1234567890.0, ) - + empty_ros = empty_pc.to_ros_msg() assert empty_ros.width == 0, "Empty cloud should have width 0" assert empty_ros.height == 0, "Empty cloud should have height 0" assert len(empty_ros.data) == 0, "Empty cloud should have no data" - + empty_converted = PointCloud2.from_ros_msg(empty_ros) assert len(empty_converted) == 0, "Empty cloud conversion failed" print("✓ Empty point cloud handling works") - + print("\n✓ All ROS conversion tests passed!") diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index cb807be3a5..485a51d52f 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -292,7 +292,9 @@ def _deploy_ros_bridge(self): "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS ) - logger.info("ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)") + logger.info( + "ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)" + ) def _start_modules(self): """Start all deployed modules.""" From 0449463b1d42a7918f29aa3766af0c306058b6a0 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Tue, 16 Sep 2025 19:34:25 -0700 Subject: [PATCH 12/21] enable with ROBOT_IP set --- dimos/robot/unitree_webrtc/unitree_g1.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 485a51d52f..7804744f9e 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -378,7 +378,7 @@ def main(): replay_path=args.replay, enable_joystick=args.joystick, enable_camera=args.camera, - enable_connection=False, + enable_connection=os.getenv("ROBOT_IP") is not None, enable_ros_bridge=True, ) robot.start() From 34fa532f7df286327ef7300d1ef30f70d8440ea1 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Tue, 16 Sep 2025 23:14:51 -0400 Subject: [PATCH 13/21] added new message types to __init__.py --- dimos/msgs/geometry_msgs/__init__.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index 137b113a4d..de46a0a079 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -1,7 +1,11 @@ from dimos.msgs.geometry_msgs.Pose import Pose, PoseLike, to_pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance +from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import PoseWithCovarianceStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance +from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import TwistWithCovarianceStamped from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike From 1ce4166af7d5b1bb3ea56c6f052d3504795efa84 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Wed, 17 Sep 2025 14:31:36 -0400 Subject: [PATCH 14/21] vectorized pointcloud message conversion and remove require IP --- dimos/msgs/sensor_msgs/PointCloud2.py | 65 +++++++++++++++++++----- dimos/robot/unitree_webrtc/unitree_g1.py | 4 -- 2 files changed, 51 insertions(+), 18 deletions(-) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index c3a0c49bab..b5352ed6cb 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -257,27 +257,64 @@ def from_ros_msg(cls, ros_msg: "ROSPointCloud2") -> "PointCloud2": if any(offset is None for offset in [x_offset, y_offset, z_offset]): raise ValueError("PointCloud2 message missing X, Y, or Z fields") - # Extract points from binary data + # Extract points from binary data using numpy for bulk conversion num_points = ros_msg.width * ros_msg.height - points = np.zeros((num_points, 3), dtype=np.float32) - data = ros_msg.data point_step = ros_msg.point_step # Determine byte order byte_order = ">" if ros_msg.is_bigendian else "<" - for i in range(num_points): - base_offset = i * point_step - - # Extract X, Y, Z (assuming float32) - x_bytes = data[base_offset + x_offset : base_offset + x_offset + 4] - y_bytes = data[base_offset + y_offset : base_offset + y_offset + 4] - z_bytes = data[base_offset + z_offset : base_offset + z_offset + 4] - - points[i, 0] = struct.unpack(f"{byte_order}f", x_bytes)[0] - points[i, 1] = struct.unpack(f"{byte_order}f", y_bytes)[0] - points[i, 2] = struct.unpack(f"{byte_order}f", z_bytes)[0] + # Check if we can use fast numpy path (common case: sequential float32 x,y,z) + if ( + x_offset == 0 + and y_offset == 4 + and z_offset == 8 + and point_step >= 12 + and not ros_msg.is_bigendian + ): + # Fast path: direct numpy reshape for tightly packed float32 x,y,z + # This is the most common case for point clouds + if point_step == 12: + # Perfectly packed x,y,z with no padding + points = np.frombuffer(data, dtype=np.float32).reshape(-1, 3) + else: + # Has additional fields after x,y,z, need to extract with stride + dt = np.dtype( + [("x", " 0: + dt_fields.append(("_pad_x", f"V{x_offset}")) + dt_fields.append(("x", f"{byte_order}f4")) + + # Add padding between x and y if needed + gap_xy = y_offset - x_offset - 4 + if gap_xy > 0: + dt_fields.append(("_pad_xy", f"V{gap_xy}")) + dt_fields.append(("y", f"{byte_order}f4")) + + # Add padding between y and z if needed + gap_yz = z_offset - y_offset - 4 + if gap_yz > 0: + dt_fields.append(("_pad_yz", f"V{gap_yz}")) + dt_fields.append(("z", f"{byte_order}f4")) + + # Add padding at the end to match point_step + remaining = point_step - z_offset - 4 + if remaining > 0: + dt_fields.append(("_pad_end", f"V{remaining}")) + + dt = np.dtype(dt_fields) + structured = np.frombuffer(data, dtype=dt, count=num_points) + points = np.column_stack((structured["x"], structured["y"], structured["z"])) # Filter out NaN and Inf values if not dense if not ros_msg.is_dense: diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 7804744f9e..2963c2cfc6 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -365,10 +365,6 @@ def main(): args = parser.parse_args() - if not args.ip: - logger.error("Robot IP not set. Use --ip or set ROBOT_IP environment variable") - return - pubsub.lcm.autoconf() robot = UnitreeG1( From d81b3a2fa0cecbe34a5c0f922bfc6cf2a8736d71 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Wed, 17 Sep 2025 14:39:41 -0400 Subject: [PATCH 15/21] ultimate fix for python stupid GIL --- dimos/robot/ros_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/robot/ros_bridge.py b/dimos/robot/ros_bridge.py index 55dea29442..f26d81c92c 100644 --- a/dimos/robot/ros_bridge.py +++ b/dimos/robot/ros_bridge.py @@ -19,7 +19,7 @@ from enum import Enum import rclpy -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy @@ -52,7 +52,7 @@ def __init__(self, node_name: str = "dimos_ros_bridge"): self.lcm = LCM() self.lcm.start() - self._executor = MultiThreadedExecutor() + self._executor = SingleThreadedExecutor() self._executor.add_node(self.node) self._spin_thread = threading.Thread(target=self._ros_spin, daemon=True) From c880680316526b27a0c42ce9fa6eb9a69d655bd2 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Wed, 17 Sep 2025 16:56:31 -0400 Subject: [PATCH 16/21] re did tests for ros_bridge --- dimos/robot/test_ros_bridge.py | 1225 ++++++++++---------------------- 1 file changed, 368 insertions(+), 857 deletions(-) diff --git a/dimos/robot/test_ros_bridge.py b/dimos/robot/test_ros_bridge.py index a02967e62a..537a17b904 100644 --- a/dimos/robot/test_ros_bridge.py +++ b/dimos/robot/test_ros_bridge.py @@ -12,898 +12,409 @@ # See the License for the specific language governing permissions and # limitations under the License. -from unittest.mock import MagicMock, patch - -import pytest -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Vector3 as ROSVector3 -from geometry_msgs.msg import PoseStamped as ROSPoseStamped -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion -from nav_msgs.msg import Path as ROSPath -from sensor_msgs.msg import LaserScan as ROSLaserScan +import time +import threading +import unittest +import numpy as np + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped as ROSTwistStamped from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 -from std_msgs.msg import String as ROSString -from std_msgs.msg import Header as ROSHeader +from sensor_msgs.msg import PointField +from tf2_msgs.msg import TFMessage as ROSTFMessage +from geometry_msgs.msg import TransformStamped -from dimos.msgs.geometry_msgs import Twist, Vector3, PoseStamped, Pose, Quaternion -from dimos.msgs.nav_msgs import Path from dimos.protocol.pubsub.lcmpubsub import LCM, Topic +from dimos.msgs.geometry_msgs import TwistStamped +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.tf2_msgs import TFMessage from dimos.robot.ros_bridge import ROSBridge, BridgeDirection -@pytest.fixture -def bridge(): - """Create a ROSBridge instance with mocked internals.""" - with ( - patch("dimos.robot.ros_bridge.rclpy") as mock_rclpy, - patch("dimos.robot.ros_bridge.Node") as mock_node_class, - patch("dimos.robot.ros_bridge.LCM") as mock_lcm_class, - patch("dimos.robot.ros_bridge.MultiThreadedExecutor") as mock_executor_class, - ): - mock_rclpy.ok.return_value = False - mock_node = MagicMock() - mock_node.create_subscription = MagicMock(return_value=MagicMock()) - mock_node.create_publisher = MagicMock(return_value=MagicMock()) - mock_node_class.return_value = mock_node +class TestROSBridge(unittest.TestCase): + """Test suite for ROS-DIMOS bridge.""" - mock_lcm = MagicMock() - mock_lcm.subscribe = MagicMock(return_value=MagicMock()) - mock_lcm.publish = MagicMock() - mock_lcm_class.return_value = mock_lcm + def setUp(self): + """Set up test fixtures.""" + # Initialize ROS if not already done + if not rclpy.ok(): + rclpy.init() - mock_executor = MagicMock() - mock_executor_class.return_value = mock_executor + # Create test bridge + self.bridge = ROSBridge("test_ros_bridge") - bridge = ROSBridge("test_bridge") + # Create test node for publishing/subscribing + self.test_node = Node("test_node") - bridge._mock_rclpy = mock_rclpy - bridge._mock_node_class = mock_node_class - bridge._mock_lcm_class = mock_lcm_class + # Track received messages + self.ros_messages = [] + self.dimos_messages = [] + self.message_timestamps = {"ros": [], "dimos": []} - return bridge + def tearDown(self): + """Clean up test fixtures.""" + self.test_node.destroy_node() + self.bridge.shutdown() + if rclpy.ok(): + rclpy.try_shutdown() - -def test_bridge_initialization(): - """Test that the bridge initializes correctly with its own instances.""" - with ( - patch("dimos.robot.ros_bridge.rclpy") as mock_rclpy, - patch("dimos.robot.ros_bridge.Node") as mock_node_class, - patch("dimos.robot.ros_bridge.LCM") as mock_lcm_class, - patch("dimos.robot.ros_bridge.MultiThreadedExecutor"), - ): - mock_rclpy.ok.return_value = False - - bridge = ROSBridge("test_bridge") - - mock_rclpy.init.assert_called_once() - mock_node_class.assert_called_once_with("test_bridge") - mock_lcm_class.assert_called_once() - bridge.lcm.start.assert_called_once() - - assert bridge._bridges == {} - assert bridge._qos is not None - - -def test_add_topic_ros_to_dimos(bridge): - """Test that add_topic creates ROS subscription for ROS->DIMOS direction.""" - topic_name = "/cmd_vel" - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - - bridge.node.create_subscription.assert_called_once() - call_args = bridge.node.create_subscription.call_args - assert call_args[0][0] == ROSTwist - assert call_args[0][1] == topic_name - - bridge.node.create_publisher.assert_not_called() - bridge.lcm.subscribe.assert_not_called() - - assert topic_name in bridge._bridges - assert "dimos_topic" in bridge._bridges[topic_name] - assert "dimos_type" in bridge._bridges[topic_name] - assert "ros_type" in bridge._bridges[topic_name] - assert bridge._bridges[topic_name]["dimos_type"] == Twist - assert bridge._bridges[topic_name]["ros_type"] == ROSTwist - assert bridge._bridges[topic_name]["direction"] == BridgeDirection.ROS_TO_DIMOS - - -def test_add_topic_dimos_to_ros(bridge): - """Test that add_topic creates ROS publisher for DIMOS->ROS direction.""" - topic_name = "/cmd_vel" - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) - - bridge.node.create_subscription.assert_not_called() - bridge.node.create_publisher.assert_called_once_with(ROSTwist, topic_name, bridge._qos) - bridge.lcm.subscribe.assert_called_once() - - assert topic_name in bridge._bridges - assert "dimos_topic" in bridge._bridges[topic_name] - assert "dimos_type" in bridge._bridges[topic_name] - assert "ros_type" in bridge._bridges[topic_name] - assert bridge._bridges[topic_name]["dimos_type"] == Twist - assert bridge._bridges[topic_name]["ros_type"] == ROSTwist - assert bridge._bridges[topic_name]["direction"] == BridgeDirection.DIMOS_TO_ROS - - -def test_ros_to_dimos_conversion(bridge): - """Test ROS to DIMOS message conversion and publishing.""" - # Create a ROS Twist message - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=1.0, y=2.0, z=3.0) - ros_msg.angular = ROSVector3(x=0.1, y=0.2, z=0.3) - - # Create DIMOS topic - dimos_topic = Topic("/test", Twist) - - # Call the conversion method with type - bridge._ros_to_dimos(ros_msg, dimos_topic, Twist, "/test") - - # Verify DIMOS publish was called - bridge.lcm.publish.assert_called_once() - - # Get the published message - published_topic, published_msg = bridge.lcm.publish.call_args[0] - - assert published_topic == dimos_topic - assert isinstance(published_msg, Twist) - assert published_msg.linear.x == 1.0 - assert published_msg.linear.y == 2.0 - assert published_msg.linear.z == 3.0 - assert published_msg.angular.x == 0.1 - assert published_msg.angular.y == 0.2 - assert published_msg.angular.z == 0.3 - - -def test_dimos_to_ros_conversion(bridge): - """Test DIMOS to ROS message conversion and publishing.""" - # Create a DIMOS Twist message - dimos_msg = Twist(linear=Vector3(4.0, 5.0, 6.0), angular=Vector3(0.4, 0.5, 0.6)) - - # Create mock ROS publisher - ros_pub = MagicMock() - - # Call the conversion method - bridge._dimos_to_ros(dimos_msg, ros_pub, "/test") - - # Verify ROS publish was called - ros_pub.publish.assert_called_once() - - # Get the published message - published_msg = ros_pub.publish.call_args[0][0] - - assert isinstance(published_msg, ROSTwist) - assert published_msg.linear.x == 4.0 - assert published_msg.linear.y == 5.0 - assert published_msg.linear.z == 6.0 - assert published_msg.angular.x == 0.4 - assert published_msg.angular.y == 0.5 - assert published_msg.angular.z == 0.6 - - -def test_unidirectional_flow_ros_to_dimos(bridge): - """Test that messages flow from ROS to DIMOS when configured.""" - topic_name = "/cmd_vel" - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - - ros_callback = bridge.node.create_subscription.call_args[0][2] - - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=1.5, y=2.5, z=3.5) - ros_msg.angular = ROSVector3(x=0.15, y=0.25, z=0.35) - - ros_callback(ros_msg) - - bridge.lcm.publish.assert_called_once() - _, published_msg = bridge.lcm.publish.call_args[0] - assert isinstance(published_msg, Twist) - assert published_msg.linear.x == 1.5 - - -def test_unidirectional_flow_dimos_to_ros(bridge): - """Test that messages flow from DIMOS to ROS when configured.""" - topic_name = "/cmd_vel" - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) - - dimos_callback = bridge.lcm.subscribe.call_args[0][1] - - dimos_msg = Twist(linear=Vector3(7.0, 8.0, 9.0), angular=Vector3(0.7, 0.8, 0.9)) - - ros_publisher = bridge.node.create_publisher.return_value - - dimos_callback(dimos_msg, None) - - ros_publisher.publish.assert_called_once() - published_ros_msg = ros_publisher.publish.call_args[0][0] - assert isinstance(published_ros_msg, ROSTwist) - assert published_ros_msg.linear.x == 7.0 - - -def test_multiple_topics(bridge): - """Test that multiple topics can be bridged simultaneously.""" - topics = [ - ("/cmd_vel", BridgeDirection.ROS_TO_DIMOS), - ("/teleop", BridgeDirection.DIMOS_TO_ROS), - ("/nav_cmd", BridgeDirection.ROS_TO_DIMOS), - ] - - for topic, direction in topics: - bridge.add_topic(topic, Twist, ROSTwist, direction=direction) - - assert len(bridge._bridges) == 3 - for topic, _ in topics: - assert topic in bridge._bridges - - assert bridge.node.create_subscription.call_count == 2 - assert bridge.node.create_publisher.call_count == 1 - assert bridge.lcm.subscribe.call_count == 1 - - -def test_stress_ros_to_dimos_100_messages(bridge): - """Test publishing 100 ROS messages and verify DIMOS receives them all.""" - topic_name = "/stress_test" - num_messages = 100 - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - - ros_callback = bridge.node.create_subscription.call_args[0][2] - - for i in range(num_messages): - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=float(i), y=float(i * 2), z=float(i * 3)) - ros_msg.angular = ROSVector3(x=float(i * 0.1), y=float(i * 0.2), z=float(i * 0.3)) - - ros_callback(ros_msg) - - assert bridge.lcm.publish.call_count == num_messages - - last_call = bridge.lcm.publish.call_args_list[-1] - _, last_msg = last_call[0] - assert isinstance(last_msg, Twist) - assert last_msg.linear.x == 99.0 - assert last_msg.linear.y == 198.0 - assert last_msg.linear.z == 297.0 - assert abs(last_msg.angular.x - 9.9) < 0.01 - assert abs(last_msg.angular.y - 19.8) < 0.01 - assert abs(last_msg.angular.z - 29.7) < 0.01 - - -def test_stress_dimos_to_ros_100_messages(bridge): - """Test publishing 100 DIMOS messages and verify ROS receives them all.""" - topic_name = "/stress_test_reverse" - num_messages = 100 - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) - - dimos_callback = bridge.lcm.subscribe.call_args[0][1] - ros_publisher = bridge.node.create_publisher.return_value - - for i in range(num_messages): - dimos_msg = Twist( - linear=Vector3(float(i * 10), float(i * 20), float(i * 30)), - angular=Vector3(float(i * 0.01), float(i * 0.02), float(i * 0.03)), + def test_ros_to_dimos_twist(self): + """Test ROS TwistStamped to DIMOS conversion and transmission.""" + # Set up bridge + self.bridge.add_topic( + "/test_twist", TwistStamped, ROSTwistStamped, BridgeDirection.ROS_TO_DIMOS ) - dimos_callback(dimos_msg, None) - - assert ros_publisher.publish.call_count == num_messages - - last_call = ros_publisher.publish.call_args_list[-1] - last_ros_msg = last_call[0][0] - assert isinstance(last_ros_msg, ROSTwist) - assert last_ros_msg.linear.x == 990.0 - assert last_ros_msg.linear.y == 1980.0 - assert last_ros_msg.linear.z == 2970.0 - assert abs(last_ros_msg.angular.x - 0.99) < 0.001 - assert abs(last_ros_msg.angular.y - 1.98) < 0.001 - assert abs(last_ros_msg.angular.z - 2.97) < 0.001 - - -def test_two_topics_different_directions(bridge): - """Test two topics with different directions handling messages.""" - topic_r2d = "/ros_to_dimos" - topic_d2r = "/dimos_to_ros" - - bridge.add_topic(topic_r2d, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - bridge.add_topic(topic_d2r, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) - - ros_callback = bridge.node.create_subscription.call_args[0][2] - dimos_callback = bridge.lcm.subscribe.call_args[0][1] - ros_publisher = bridge.node.create_publisher.return_value - - for i in range(50): - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=float(i), y=0.0, z=0.0) - ros_msg.angular = ROSVector3(x=0.0, y=0.0, z=float(i * 0.1)) - ros_callback(ros_msg) - - dimos_msg = Twist( - linear=Vector3(0.0, float(i), 0.0), angular=Vector3(0.0, 0.0, float(i * 0.2)) + # Subscribe to DIMOS side + lcm = LCM() + lcm.start() + topic = Topic("/test_twist", TwistStamped) + + def dimos_callback(msg, _topic): + self.dimos_messages.append(msg) + self.message_timestamps["dimos"].append(time.time()) + + lcm.subscribe(topic, dimos_callback) + + # Publish from ROS side + ros_pub = self.test_node.create_publisher(ROSTwistStamped, "/test_twist", 10) + + # Send test messages + for i in range(10): + msg = ROSTwistStamped() + msg.header.stamp = self.test_node.get_clock().now().to_msg() + msg.header.frame_id = f"frame_{i}" + msg.twist.linear.x = float(i) + msg.twist.linear.y = float(i * 2) + msg.twist.angular.z = float(i * 0.1) + + ros_pub.publish(msg) + self.message_timestamps["ros"].append(time.time()) + time.sleep(0.01) # 100Hz + + # Allow time for processing + time.sleep(0.5) + + # Verify messages received + self.assertEqual(len(self.dimos_messages), 10, "Should receive all 10 messages") + + # Verify message content + for i, msg in enumerate(self.dimos_messages): + self.assertEqual(msg.frame_id, f"frame_{i}") + self.assertAlmostEqual(msg.linear.x, float(i), places=5) + self.assertAlmostEqual(msg.linear.y, float(i * 2), places=5) + self.assertAlmostEqual(msg.angular.z, float(i * 0.1), places=5) + + def test_dimos_to_ros_twist(self): + """Test DIMOS TwistStamped to ROS conversion and transmission.""" + # Set up bridge + self.bridge.add_topic( + "/test_twist_reverse", TwistStamped, ROSTwistStamped, BridgeDirection.DIMOS_TO_ROS ) - dimos_callback(dimos_msg, None) - - assert bridge.lcm.publish.call_count == 50 - assert ros_publisher.publish.call_count == 50 - - last_dimos_call = bridge.lcm.publish.call_args_list[-1] - _, last_dimos_msg = last_dimos_call[0] - assert last_dimos_msg.linear.x == 49.0 - - last_ros_call = ros_publisher.publish.call_args_list[-1] - last_ros_msg = last_ros_call[0][0] - assert last_ros_msg.linear.y == 49.0 + # Subscribe to ROS side + def ros_callback(msg): + self.ros_messages.append(msg) + self.message_timestamps["ros"].append(time.time()) + + self.test_node.create_subscription(ROSTwistStamped, "/test_twist_reverse", ros_callback, 10) + + # Use the bridge's LCM instance for publishing + topic = Topic("/test_twist_reverse", TwistStamped) + + # Send test messages + for i in range(10): + msg = TwistStamped(ts=time.time(), frame_id=f"dimos_frame_{i}") + msg.linear.x = float(i * 3) + msg.linear.y = float(i * 4) + msg.angular.z = float(i * 0.2) + + self.bridge.lcm.publish(topic, msg) + self.message_timestamps["dimos"].append(time.time()) + time.sleep(0.01) # 100Hz + + # Allow time for processing and spin the test node + for _ in range(50): # Spin for 0.5 seconds + rclpy.spin_once(self.test_node, timeout_sec=0.01) + + # Verify messages received + self.assertEqual(len(self.ros_messages), 10, "Should receive all 10 messages") + + # Verify message content + for i, msg in enumerate(self.ros_messages): + self.assertEqual(msg.header.frame_id, f"dimos_frame_{i}") + self.assertAlmostEqual(msg.twist.linear.x, float(i * 3), places=5) + self.assertAlmostEqual(msg.twist.linear.y, float(i * 4), places=5) + self.assertAlmostEqual(msg.twist.angular.z, float(i * 0.2), places=5) + + def test_frequency_preservation(self): + """Test that message frequencies are preserved through the bridge.""" + # Set up bridge + self.bridge.add_topic( + "/test_freq", TwistStamped, ROSTwistStamped, BridgeDirection.ROS_TO_DIMOS + ) -def test_high_frequency_burst(bridge): - """Test handling a burst of 1000 messages to ensure no drops.""" - topic_name = "/burst_test" - burst_size = 1000 - - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - - ros_callback = bridge.node.create_subscription.call_args[0][2] - - messages_sent = [] - for i in range(burst_size): - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=float(i), y=float(i), z=float(i)) - ros_msg.angular = ROSVector3(x=0.0, y=0.0, z=0.0) - messages_sent.append(i) - ros_callback(ros_msg) + # Subscribe to DIMOS side + lcm = LCM() + lcm.start() + topic = Topic("/test_freq", TwistStamped) + + receive_times = [] + + def dimos_callback(_msg, _topic): + receive_times.append(time.time()) + + lcm.subscribe(topic, dimos_callback) + + # Publish from ROS at specific frequencies + ros_pub = self.test_node.create_publisher(ROSTwistStamped, "/test_freq", 10) + + # Test different frequencies + test_frequencies = [10, 50, 100] # Hz + + for target_freq in test_frequencies: + receive_times.clear() + send_times = [] + period = 1.0 / target_freq + + # Send messages at target frequency + start_time = time.time() + while time.time() - start_time < 1.0: # Run for 1 second + msg = ROSTwistStamped() + msg.header.stamp = self.test_node.get_clock().now().to_msg() + msg.twist.linear.x = 1.0 + + ros_pub.publish(msg) + send_times.append(time.time()) + time.sleep(period) + + # Allow processing time + time.sleep(0.2) + + # Calculate actual frequencies + if len(send_times) > 1: + send_intervals = np.diff(send_times) + send_freq = 1.0 / np.mean(send_intervals) + else: + send_freq = 0 + + if len(receive_times) > 1: + receive_intervals = np.diff(receive_times) + receive_freq = 1.0 / np.mean(receive_intervals) + else: + receive_freq = 0 + + # Verify frequency preservation (within 10% tolerance) + self.assertAlmostEqual( + receive_freq, + send_freq, + delta=send_freq * 0.1, + msg=f"Frequency not preserved for {target_freq}Hz: sent={send_freq:.1f}Hz, received={receive_freq:.1f}Hz", + ) + + def test_pointcloud_conversion(self): + """Test PointCloud2 message conversion with numpy optimization.""" + # Set up bridge + self.bridge.add_topic( + "/test_cloud", PointCloud2, ROSPointCloud2, BridgeDirection.ROS_TO_DIMOS + ) - assert bridge.lcm.publish.call_count == burst_size + # Subscribe to DIMOS side + lcm = LCM() + lcm.start() + topic = Topic("/test_cloud", PointCloud2) + + received_cloud = [] + + def dimos_callback(msg, _topic): + received_cloud.append(msg) + + lcm.subscribe(topic, dimos_callback) + + # Create test point cloud + ros_pub = self.test_node.create_publisher(ROSPointCloud2, "/test_cloud", 10) + + # Generate test points + num_points = 1000 + points = np.random.randn(num_points, 3).astype(np.float32) + + # Create ROS PointCloud2 message + msg = ROSPointCloud2() + msg.header.stamp = self.test_node.get_clock().now().to_msg() + msg.header.frame_id = "test_frame" + msg.height = 1 + msg.width = num_points + msg.fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + ] + msg.is_bigendian = False + msg.point_step = 12 + msg.row_step = msg.point_step * msg.width + msg.data = points.tobytes() + msg.is_dense = True + + # Send point cloud + ros_pub.publish(msg) + + # Allow processing time + time.sleep(0.5) + + # Verify reception + self.assertEqual(len(received_cloud), 1, "Should receive point cloud") + + # Verify point data + received_points = received_cloud[0].as_numpy() + self.assertEqual(received_points.shape, points.shape) + np.testing.assert_array_almost_equal(received_points, points, decimal=5) + + def test_tf_high_frequency(self): + """Test TF message handling at high frequency.""" + # Set up bridge + self.bridge.add_topic("/test_tf", TFMessage, ROSTFMessage, BridgeDirection.ROS_TO_DIMOS) + + # Subscribe to DIMOS side + lcm = LCM() + lcm.start() + topic = Topic("/test_tf", TFMessage) + + received_tfs = [] + receive_times = [] + + def dimos_callback(msg, _topic): + received_tfs.append(msg) + receive_times.append(time.time()) + + lcm.subscribe(topic, dimos_callback) + + # Publish TF at high frequency (200Hz) + ros_pub = self.test_node.create_publisher(ROSTFMessage, "/test_tf", 100) + + target_freq = 200 # Hz + period = 1.0 / target_freq + num_messages = 200 # 1 second worth + + send_times = [] + for i in range(num_messages): + msg = ROSTFMessage() + transform = TransformStamped() + transform.header.stamp = self.test_node.get_clock().now().to_msg() + transform.header.frame_id = "world" + transform.child_frame_id = f"link_{i}" + transform.transform.translation.x = float(i) + transform.transform.rotation.w = 1.0 + msg.transforms = [transform] + + ros_pub.publish(msg) + send_times.append(time.time()) + time.sleep(period) + + # Allow processing time + time.sleep(0.5) + + # Check message count (allow 5% loss tolerance) + min_expected = int(num_messages * 0.95) + self.assertGreaterEqual( + len(received_tfs), + min_expected, + f"Should receive at least {min_expected} of {num_messages} TF messages", + ) - for idx, call in enumerate(bridge.lcm.publish.call_args_list): - _, msg = call[0] - assert msg.linear.x == float(idx) + # Check frequency preservation + if len(receive_times) > 1: + receive_intervals = np.diff(receive_times) + receive_freq = 1.0 / np.mean(receive_intervals) + + # For high frequency, allow 20% tolerance + self.assertAlmostEqual( + receive_freq, + target_freq, + delta=target_freq * 0.2, + msg=f"High frequency TF not preserved: expected={target_freq}Hz, got={receive_freq:.1f}Hz", + ) + + def test_bidirectional_bridge(self): + """Test simultaneous bidirectional message flow.""" + # Set up bidirectional bridges for same topic type + self.bridge.add_topic( + "/ros_to_dimos", TwistStamped, ROSTwistStamped, BridgeDirection.ROS_TO_DIMOS + ) + self.bridge.add_topic( + "/dimos_to_ros", TwistStamped, ROSTwistStamped, BridgeDirection.DIMOS_TO_ROS + ) -def test_multiple_topics_with_different_rates(bridge): - """Test multiple topics receiving messages at different rates.""" - topics = { - "/fast_topic": 100, # 100 messages - "/medium_topic": 50, # 50 messages - "/slow_topic": 10, # 10 messages - } + dimos_received = [] + ros_received = [] - for topic_name in topics: - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) + # DIMOS subscriber - use bridge's LCM + topic_r2d = Topic("/ros_to_dimos", TwistStamped) + self.bridge.lcm.subscribe(topic_r2d, lambda msg, _: dimos_received.append(msg)) - callbacks = [] - for i in range(3): - callbacks.append(bridge.node.create_subscription.call_args_list[i][0][2]) + # ROS subscriber + self.test_node.create_subscription( + ROSTwistStamped, "/dimos_to_ros", lambda msg: ros_received.append(msg), 10 + ) - bridge.lcm.publish.reset_mock() + # Set up publishers + ros_pub = self.test_node.create_publisher(ROSTwistStamped, "/ros_to_dimos", 10) + topic_d2r = Topic("/dimos_to_ros", TwistStamped) - for topic_idx, (topic_name, msg_count) in enumerate(topics.items()): - for i in range(msg_count): - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=float(topic_idx), y=float(i), z=0.0) - callbacks[topic_idx](ros_msg) + # Keep track of whether threads should continue + stop_spinning = threading.Event() - total_expected = sum(topics.values()) - assert bridge.lcm.publish.call_count == total_expected + # Spin the test node in background to receive messages + def spin_test_node(): + while not stop_spinning.is_set(): + rclpy.spin_once(self.test_node, timeout_sec=0.01) + spin_thread = threading.Thread(target=spin_test_node, daemon=True) + spin_thread.start() -def test_pose_stamped_bridging(bridge): - """Test bridging PoseStamped messages.""" - topic_name = "/robot_pose" + # Send messages in both directions simultaneously + def send_ros_messages(): + for i in range(50): + msg = ROSTwistStamped() + msg.header.stamp = self.test_node.get_clock().now().to_msg() + msg.twist.linear.x = float(i) + ros_pub.publish(msg) + time.sleep(0.02) # 50Hz - # Test ROS to DIMOS - bridge.add_topic( - topic_name, PoseStamped, ROSPoseStamped, direction=BridgeDirection.ROS_TO_DIMOS - ) + def send_dimos_messages(): + for i in range(50): + msg = TwistStamped(ts=time.time()) + msg.linear.y = float(i * 2) + self.bridge.lcm.publish(topic_d2r, msg) + time.sleep(0.02) # 50Hz - ros_callback = bridge.node.create_subscription.call_args[0][2] + # Run both senders in parallel + ros_thread = threading.Thread(target=send_ros_messages) + dimos_thread = threading.Thread(target=send_dimos_messages) - ros_msg = ROSPoseStamped() - ros_msg.header.frame_id = "map" - ros_msg.header.stamp.sec = 100 - ros_msg.header.stamp.nanosec = 500000000 - ros_msg.pose.position.x = 10.0 - ros_msg.pose.position.y = 20.0 - ros_msg.pose.position.z = 30.0 - ros_msg.pose.orientation.x = 0.0 - ros_msg.pose.orientation.y = 0.0 - ros_msg.pose.orientation.z = 0.707 - ros_msg.pose.orientation.w = 0.707 + ros_thread.start() + dimos_thread.start() - ros_callback(ros_msg) + ros_thread.join() + dimos_thread.join() - bridge.lcm.publish.assert_called_once() - _, published_msg = bridge.lcm.publish.call_args[0] - assert hasattr(published_msg, "frame_id") - assert hasattr(published_msg, "position") - assert hasattr(published_msg, "orientation") + # Allow processing time + time.sleep(0.5) + stop_spinning.set() + spin_thread.join(timeout=1.0) + # Verify both directions worked + self.assertGreaterEqual(len(dimos_received), 45, "Should receive most ROS->DIMOS messages") + self.assertGreaterEqual(len(ros_received), 45, "Should receive most DIMOS->ROS messages") -def test_path_bridging(bridge): - """Test bridging Path messages.""" - topic_name = "/planned_path" + # Verify message integrity + for i, msg in enumerate(dimos_received[:45]): + self.assertAlmostEqual(msg.linear.x, float(i), places=5) - # Test DIMOS to ROS - bridge.add_topic(topic_name, Path, ROSPath, direction=BridgeDirection.DIMOS_TO_ROS) + for i, msg in enumerate(ros_received[:45]): + self.assertAlmostEqual(msg.twist.linear.y, float(i * 2), places=5) - dimos_callback = bridge.lcm.subscribe.call_args[0][1] - ros_publisher = bridge.node.create_publisher.return_value - # Create a DIMOS Path with multiple poses - poses = [] - for i in range(5): - pose = PoseStamped( - ts=100.0 + i, - frame_id="map", - position=Vector3(float(i), float(i * 2), 0.0), - orientation=Quaternion(0, 0, 0, 1), - ) - poses.append(pose) - - dimos_path = Path(frame_id="map", poses=poses) - - dimos_callback(dimos_path, None) - - ros_publisher.publish.assert_called_once() - published_ros_msg = ros_publisher.publish.call_args[0][0] - assert isinstance(published_ros_msg, ROSPath) - - -def test_multiple_message_types(bridge): - """Test bridging multiple different message types simultaneously.""" - topics = [ - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS), - ("/robot_pose", PoseStamped, ROSPoseStamped, BridgeDirection.DIMOS_TO_ROS), - ("/global_path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/local_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), - ("/teleop_twist", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS), - ] - - for topic_name, dimos_type, ros_type, direction in topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == 5 - - # Count subscriptions and publishers - ros_to_dimos_count = sum(1 for _, _, _, d in topics if d == BridgeDirection.ROS_TO_DIMOS) - dimos_to_ros_count = sum(1 for _, _, _, d in topics if d == BridgeDirection.DIMOS_TO_ROS) - - assert bridge.node.create_subscription.call_count == ros_to_dimos_count - assert bridge.node.create_publisher.call_count == dimos_to_ros_count - assert bridge.lcm.subscribe.call_count == dimos_to_ros_count - - -def test_topic_remapping_ros_to_dimos(bridge): - """Test remapping topic names for ROS to DIMOS direction.""" - ros_topic = "/cmd_vel" - dimos_remapped = "/robot/velocity_command" - - bridge.add_topic( - ros_topic, - Twist, - ROSTwist, - direction=BridgeDirection.ROS_TO_DIMOS, - remap_topic=dimos_remapped, - ) - - # Verify ROS subscribes to original topic - bridge.node.create_subscription.assert_called_once() - call_args = bridge.node.create_subscription.call_args - assert call_args[0][1] == ros_topic # ROS side uses original topic - - # Verify bridge metadata contains both names - assert ros_topic in bridge._bridges - bridge_info = bridge._bridges[ros_topic] - assert bridge_info["ros_topic_name"] == ros_topic - assert bridge_info["dimos_topic_name"] == dimos_remapped - assert bridge_info["dimos_topic"].topic == dimos_remapped - - -def test_topic_remapping_dimos_to_ros(bridge): - """Test remapping topic names for DIMOS to ROS direction.""" - dimos_topic = "/velocity_command" - ros_remapped = "/mobile_base/cmd_vel" - - bridge.add_topic( - dimos_topic, - Twist, - ROSTwist, - direction=BridgeDirection.DIMOS_TO_ROS, - remap_topic=ros_remapped, - ) - - # Verify ROS publishes to remapped topic - bridge.node.create_publisher.assert_called_once_with(ROSTwist, ros_remapped, bridge._qos) - - # Verify DIMOS subscribes to original topic - bridge.lcm.subscribe.assert_called_once() - dimos_topic_arg = bridge.lcm.subscribe.call_args[0][0] - assert dimos_topic_arg.topic == dimos_topic - - # Verify bridge metadata - assert dimos_topic in bridge._bridges - bridge_info = bridge._bridges[dimos_topic] - assert bridge_info["ros_topic_name"] == ros_remapped - assert bridge_info["dimos_topic_name"] == dimos_topic - - -def test_remapped_message_flow_ros_to_dimos(bridge): - """Test message flow with remapped topics from ROS to DIMOS.""" - ros_topic = "/ros/cmd_vel" - dimos_remapped = "/dimos/velocity" - - bridge.add_topic( - ros_topic, - Twist, - ROSTwist, - direction=BridgeDirection.ROS_TO_DIMOS, - remap_topic=dimos_remapped, - ) - - # Get the ROS callback - ros_callback = bridge.node.create_subscription.call_args[0][2] - - # Send a ROS message - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=2.0, y=3.0, z=4.0) - ros_msg.angular = ROSVector3(x=0.2, y=0.3, z=0.4) - - ros_callback(ros_msg) - - # Verify DIMOS publishes to remapped topic - bridge.lcm.publish.assert_called_once() - published_topic, published_msg = bridge.lcm.publish.call_args[0] - assert published_topic.topic == dimos_remapped - assert isinstance(published_msg, Twist) - assert published_msg.linear.x == 2.0 - - -def test_remapped_message_flow_dimos_to_ros(bridge): - """Test message flow with remapped topics from DIMOS to ROS.""" - dimos_topic = "/dimos/velocity" - ros_remapped = "/ros/cmd_vel" - - bridge.add_topic( - dimos_topic, - Twist, - ROSTwist, - direction=BridgeDirection.DIMOS_TO_ROS, - remap_topic=ros_remapped, - ) - - # Get the DIMOS callback - dimos_callback = bridge.lcm.subscribe.call_args[0][1] - - # Send a DIMOS message - dimos_msg = Twist(linear=Vector3(5.0, 6.0, 7.0), angular=Vector3(0.5, 0.6, 0.7)) - - ros_publisher = bridge.node.create_publisher.return_value - dimos_callback(dimos_msg, None) - - # Verify ROS publishes to remapped topic - ros_publisher.publish.assert_called_once() - published_msg = ros_publisher.publish.call_args[0][0] - assert isinstance(published_msg, ROSTwist) - assert published_msg.linear.x == 5.0 - - -def test_multiple_remapped_topics(bridge): - """Test multiple topics with remapping.""" - topic_configs = [ - # (original_name, dimos_type, ros_type, direction, remap_name) - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.ROS_TO_DIMOS, "/robot/velocity"), - ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/odometry"), - ("/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/navigation/global_path"), - ( - "/goal", - PoseStamped, - ROSPoseStamped, - BridgeDirection.DIMOS_TO_ROS, - "/navigation/goal_pose", - ), - ] - - for topic, dimos_type, ros_type, direction, remap in topic_configs: - bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) - - assert len(bridge._bridges) == 4 - - # Verify ROS to DIMOS remapping - assert bridge._bridges["/cmd_vel"]["dimos_topic_name"] == "/robot/velocity" - assert bridge._bridges["/odom"]["dimos_topic_name"] == "/robot/odometry" - - # Verify DIMOS to ROS remapping - assert bridge._bridges["/path"]["ros_topic_name"] == "/navigation/global_path" - assert bridge._bridges["/goal"]["ros_topic_name"] == "/navigation/goal_pose" - - -def test_no_remapping_when_none(bridge): - """Test that topics work normally when remap_topic is None.""" - topic = "/cmd_vel" - - bridge.add_topic( - topic, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS, remap_topic=None - ) - - bridge_info = bridge._bridges[topic] - assert bridge_info["ros_topic_name"] == topic - assert bridge_info["dimos_topic_name"] == topic - - -def test_stress_remapped_topics(bridge): - """Test stress scenario with remapped topics.""" - num_messages = 100 - ros_topic = "/ros/high_freq" - dimos_remapped = "/dimos/data_stream" - - bridge.add_topic( - ros_topic, - Twist, - ROSTwist, - direction=BridgeDirection.ROS_TO_DIMOS, - remap_topic=dimos_remapped, - ) - - ros_callback = bridge.node.create_subscription.call_args[0][2] - - for i in range(num_messages): - ros_msg = ROSTwist() - ros_msg.linear = ROSVector3(x=float(i), y=float(i * 2), z=float(i * 3)) - ros_callback(ros_msg) - - assert bridge.lcm.publish.call_count == num_messages - - # Verify all published to remapped topic - for call in bridge.lcm.publish.call_args_list: - topic, _ = call[0] - assert topic.topic == dimos_remapped - - -def test_navigation_stack_topics(bridge): - """Test common navigation stack topics.""" - nav_topics = [ - ("/move_base/goal", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/move_base/global_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/move_base/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/robot_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - for topic_name, dimos_type, ros_type, direction in nav_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == len(nav_topics) - - # Verify each topic is configured correctly - for topic_name, dimos_type, ros_type, direction in nav_topics: - assert topic_name in bridge._bridges - assert bridge._bridges[topic_name]["dimos_type"] == dimos_type - assert bridge._bridges[topic_name]["ros_type"] == ros_type - assert bridge._bridges[topic_name]["direction"] == direction - - -def test_control_topics(bridge): - """Test control system topics.""" - control_topics = [ - ("/joint_commands", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/feedback", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - for topic_name, dimos_type, ros_type, direction in control_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == len(control_topics) - - -def test_perception_topics(bridge): - """Test perception system topics.""" - perception_topics = [ - ("/detected_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/tracked_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), - ("/vision_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - for topic_name, dimos_type, ros_type, direction in perception_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - # All perception topics are ROS to DIMOS - assert bridge.node.create_subscription.call_count == len(perception_topics) - assert bridge.node.create_publisher.call_count == 0 - - -def test_mixed_frequency_topics(bridge): - """Test topics with different expected frequencies.""" - # High frequency (100Hz+) - high_freq_topics = [ - ("/imu/data", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - # Medium frequency (10-50Hz) - medium_freq_topics = [ - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - # Low frequency (1-5Hz) - low_freq_topics = [ - ("/global_path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/goal", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ] - - all_topics = high_freq_topics + medium_freq_topics + low_freq_topics - - for topic_name, dimos_type, ros_type, direction in all_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == len(all_topics) - - # Test high frequency message handling - for topic_name, _, _, direction in high_freq_topics: - if direction == BridgeDirection.ROS_TO_DIMOS: - # Find the callback for this topic - for i, call in enumerate(bridge.node.create_subscription.call_args_list): - if call[0][1] == topic_name: - callback = call[0][2] - # Send 100 messages rapidly - for j in range(100): - ros_msg = ROSPoseStamped() - ros_msg.header.stamp.sec = j - callback(ros_msg) - break - - -def test_bidirectional_prevention(bridge): - """Test that the same topic cannot be added in both directions.""" - topic_name = "/cmd_vel" - - # Add topic in one direction - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.ROS_TO_DIMOS) - - # Try to add the same topic in opposite direction should not create duplicate - # The bridge should handle this gracefully - initial_bridges = len(bridge._bridges) - bridge.add_topic(topic_name, Twist, ROSTwist, direction=BridgeDirection.DIMOS_TO_ROS) - - # Should still have the same number of bridges (topic gets reconfigured, not duplicated) - assert len(bridge._bridges) == initial_bridges - - -def test_robot_arm_topics(bridge): - """Test robot arm control topics.""" - arm_topics = [ - ("/arm/joint_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/arm/joint_states", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/arm/end_effector_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/arm/gripper_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/arm/cartesian_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ] - - for topic_name, dimos_type, ros_type, direction in arm_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == len(arm_topics) - - # Check that arm control commands go from DIMOS to ROS - dimos_to_ros = [t for t in arm_topics if t[3] == BridgeDirection.DIMOS_TO_ROS] - ros_to_dimos = [t for t in arm_topics if t[3] == BridgeDirection.ROS_TO_DIMOS] - - assert bridge.node.create_publisher.call_count == len(dimos_to_ros) - assert bridge.node.create_subscription.call_count == len(ros_to_dimos) - - -def test_mobile_base_topics(bridge): - """Test mobile robot base topics.""" - base_topics = [ - ("/base/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/base/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/base/global_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/base/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/base/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ] - - for topic_name, dimos_type, ros_type, direction in base_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - # Verify topics are properly categorized - for topic_name, dimos_type, ros_type, direction in base_topics: - bridge_info = bridge._bridges[topic_name] - assert bridge_info["direction"] == direction - assert bridge_info["dimos_type"] == dimos_type - assert bridge_info["ros_type"] == ros_type - - -def test_autonomous_vehicle_topics(bridge): - """Test autonomous vehicle topics.""" - av_topics = [ - ("/vehicle/steering_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/vehicle/throttle_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/vehicle/brake_cmd", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS), - ("/vehicle/pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS), - ("/vehicle/planned_trajectory", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS), - ("/vehicle/current_path", Path, ROSPath, BridgeDirection.ROS_TO_DIMOS), - ] - - for topic_name, dimos_type, ros_type, direction in av_topics: - bridge.add_topic(topic_name, dimos_type, ros_type, direction=direction) - - assert len(bridge._bridges) == len(av_topics) - - # Count control vs feedback topics - control_topics = [t for t in av_topics if "cmd" in t[0] or "planned" in t[0]] - feedback_topics = [t for t in av_topics if "pose" in t[0] or "current" in t[0]] - - assert len(control_topics) == 4 # steering, throttle, brake, planned_trajectory - assert len(feedback_topics) == 2 # pose, current_path - - -def test_remapping_with_navigation_stack(bridge): - """Test remapping with common navigation stack patterns.""" - # Map ROS2 Nav2 topics to custom DIMOS topics - nav_remapping = [ - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS, "/nav2/cmd_vel"), - ("/odom", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/odometry"), - ("/global_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/nav2/plan"), - ("/local_plan", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, "/nav2/local_plan"), - ("/goal_pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, "/robot/goal"), - ] - - for topic, dimos_type, ros_type, direction, remap in nav_remapping: - bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) - - # Verify DIMOS to ROS remappings - assert bridge._bridges["/cmd_vel"]["ros_topic_name"] == "/nav2/cmd_vel" - assert bridge._bridges["/global_plan"]["ros_topic_name"] == "/nav2/plan" - assert bridge._bridges["/local_plan"]["ros_topic_name"] == "/nav2/local_plan" - - # Verify ROS to DIMOS remappings - assert bridge._bridges["/odom"]["dimos_topic_name"] == "/robot/odometry" - assert bridge._bridges["/goal_pose"]["dimos_topic_name"] == "/robot/goal" - - -def test_remapping_with_robot_namespace(bridge): - """Test remapping for multi-robot systems with namespaces.""" - robot_id = "robot1" - - # Remap topics to include robot namespace - topics_with_namespace = [ - ("/cmd_vel", Twist, ROSTwist, BridgeDirection.DIMOS_TO_ROS, f"/{robot_id}/cmd_vel"), - ("/pose", PoseStamped, ROSPoseStamped, BridgeDirection.ROS_TO_DIMOS, f"/{robot_id}/pose"), - ("/path", Path, ROSPath, BridgeDirection.DIMOS_TO_ROS, f"/{robot_id}/path"), - ] - - for topic, dimos_type, ros_type, direction, remap in topics_with_namespace: - bridge.add_topic(topic, dimos_type, ros_type, direction=direction, remap_topic=remap) - - # Verify all topics are properly namespaced - assert bridge._bridges["/cmd_vel"]["ros_topic_name"] == "/robot1/cmd_vel" - assert bridge._bridges["/pose"]["dimos_topic_name"] == "/robot1/pose" - assert bridge._bridges["/path"]["ros_topic_name"] == "/robot1/path" - - -def test_remapping_preserves_original_key(bridge): - """Test that remapping preserves the original topic name as the key.""" - original_topic = "/original_topic" - remapped_name = "/remapped_topic" - - bridge.add_topic( - original_topic, - Twist, - ROSTwist, - direction=BridgeDirection.ROS_TO_DIMOS, - remap_topic=remapped_name, - ) - - # Original topic name should be the key - assert original_topic in bridge._bridges - assert remapped_name not in bridge._bridges - - # Bridge info should contain both names - bridge_info = bridge._bridges[original_topic] - assert bridge_info["ros_topic_name"] == original_topic - assert bridge_info["dimos_topic_name"] == remapped_name +if __name__ == "__main__": + unittest.main() From 3622095bb6daa6cd1cd976565058bf921c37632d Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 17 Sep 2025 19:22:38 -0700 Subject: [PATCH 17/21] Fix ROS tests for CI --- dimos/msgs/geometry_msgs/test_Pose.py | 58 ++++++++++++++- dimos/msgs/geometry_msgs/test_PoseStamped.py | 18 ++++- .../geometry_msgs/test_PoseWithCovariance.py | 42 +++++++++-- .../test_PoseWithCovarianceStamped.py | 53 ++++++++++++-- dimos/msgs/geometry_msgs/test_Transform.py | 25 ++++++- dimos/msgs/geometry_msgs/test_Twist.py | 33 ++++++++- dimos/msgs/geometry_msgs/test_TwistStamped.py | 14 +++- .../geometry_msgs/test_TwistWithCovariance.py | 40 ++++++++++- .../test_TwistWithCovarianceStamped.py | 50 +++++++++++-- dimos/msgs/nav_msgs/test_Odometry.py | 72 ++++++++++++++++--- dimos/msgs/nav_msgs/test_Path.py | 35 ++++++++- dimos/msgs/sensor_msgs/test_CameraInfo.py | 21 +++++- dimos/msgs/sensor_msgs/test_PointCloud2.py | 23 ++++-- dimos/msgs/tf2_msgs/test_TFMessage.py | 21 +++++- dimos/robot/test_ros_bridge.py | 30 ++++++-- 15 files changed, 479 insertions(+), 56 deletions(-) diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index 31f05934c9..51ebf88587 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -17,15 +17,22 @@ import numpy as np import pytest from dimos_lcm.geometry_msgs import Pose as LCMPose -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion + +try: + from geometry_msgs.msg import Pose as ROSPose + from geometry_msgs.msg import Point as ROSPoint + from geometry_msgs.msg import Quaternion as ROSQuaternion +except ImportError: + ROSPose = None + ROSPoint = None + ROSQuaternion = None from dimos.msgs.geometry_msgs.Pose import Pose, to_pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 +@pytest.mark.ros def test_pose_default_init(): """Test that default initialization creates a pose at origin with identity orientation.""" pose = Pose() @@ -47,6 +54,7 @@ def test_pose_default_init(): assert pose.z == 0.0 +@pytest.mark.ros def test_pose_pose_init(): """Test initialization with position coordinates only (identity orientation).""" pose_data = Pose(1.0, 2.0, 3.0) @@ -70,6 +78,7 @@ def test_pose_pose_init(): assert pose.z == 3.0 +@pytest.mark.ros def test_pose_position_init(): """Test initialization with position coordinates only (identity orientation).""" pose = Pose(1.0, 2.0, 3.0) @@ -91,6 +100,7 @@ def test_pose_position_init(): assert pose.z == 3.0 +@pytest.mark.ros def test_pose_full_init(): """Test initialization with position and orientation coordinates.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -112,6 +122,7 @@ def test_pose_full_init(): assert pose.z == 3.0 +@pytest.mark.ros def test_pose_vector_position_init(): """Test initialization with Vector3 position (identity orientation).""" position = Vector3(4.0, 5.0, 6.0) @@ -129,6 +140,7 @@ def test_pose_vector_position_init(): assert pose.orientation.w == 1.0 +@pytest.mark.ros def test_pose_vector_quaternion_init(): """Test initialization with Vector3 position and Quaternion orientation.""" position = Vector3(1.0, 2.0, 3.0) @@ -147,6 +159,7 @@ def test_pose_vector_quaternion_init(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_list_init(): """Test initialization with lists for position and orientation.""" position_list = [1.0, 2.0, 3.0] @@ -165,6 +178,7 @@ def test_pose_list_init(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_tuple_init(): """Test initialization from a tuple of (position, orientation).""" position = [1.0, 2.0, 3.0] @@ -184,6 +198,7 @@ def test_pose_tuple_init(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_dict_init(): """Test initialization from a dictionary with 'position' and 'orientation' keys.""" pose_dict = {"position": [1.0, 2.0, 3.0], "orientation": [0.1, 0.2, 0.3, 0.9]} @@ -201,6 +216,7 @@ def test_pose_dict_init(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_copy_init(): """Test initialization from another Pose (copy constructor).""" original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -222,6 +238,7 @@ def test_pose_copy_init(): assert copy == original +@pytest.mark.ros def test_pose_lcm_init(): """Test initialization from an LCM Pose.""" # Create LCM pose @@ -248,6 +265,7 @@ def test_pose_lcm_init(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_properties(): """Test pose property access.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -264,6 +282,7 @@ def test_pose_properties(): assert pose.yaw == euler.z +@pytest.mark.ros def test_pose_euler_properties_identity(): """Test pose Euler angle properties with identity orientation.""" pose = Pose(1.0, 2.0, 3.0) # Identity orientation @@ -279,6 +298,7 @@ def test_pose_euler_properties_identity(): assert np.isclose(pose.orientation.euler.z, 0.0, atol=1e-10) +@pytest.mark.ros def test_pose_repr(): """Test pose string representation.""" pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) @@ -295,6 +315,7 @@ def test_pose_repr(): assert "2.567" in repr_str or "2.57" in repr_str +@pytest.mark.ros def test_pose_str(): """Test pose string formatting.""" pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) @@ -313,6 +334,7 @@ def test_pose_str(): assert str_repr.count("Pose") == 1 +@pytest.mark.ros def test_pose_equality(): """Test pose equality comparison.""" pose1 = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -335,6 +357,7 @@ def test_pose_equality(): assert pose1 != None +@pytest.mark.ros def test_pose_with_numpy_arrays(): """Test pose initialization with numpy arrays.""" position_array = np.array([1.0, 2.0, 3.0]) @@ -354,6 +377,7 @@ def test_pose_with_numpy_arrays(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_with_mixed_types(): """Test pose initialization with mixed input types.""" # Position as tuple, orientation as list @@ -374,6 +398,7 @@ def test_pose_with_mixed_types(): assert pose1.orientation.w == pose2.orientation.w +@pytest.mark.ros def test_to_pose_passthrough(): """Test to_pose function with Pose input (passthrough).""" original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -383,6 +408,7 @@ def test_to_pose_passthrough(): assert result is original +@pytest.mark.ros def test_to_pose_conversion(): """Test to_pose function with convertible inputs.""" # Note: The to_pose conversion function has type checking issues in the current implementation @@ -415,6 +441,7 @@ def test_to_pose_conversion(): assert result2.orientation.w == 0.9 +@pytest.mark.ros def test_pose_euler_roundtrip(): """Test conversion from Euler angles to quaternion and back.""" # Start with known Euler angles (small angles to avoid gimbal lock) @@ -438,6 +465,7 @@ def test_pose_euler_roundtrip(): assert np.isclose(result_euler.z, yaw, atol=1e-6) +@pytest.mark.ros def test_pose_zero_position(): """Test pose with zero position vector.""" # Use manual construction since Vector3.zeros has signature issues @@ -451,6 +479,7 @@ def test_pose_zero_position(): assert np.isclose(pose.yaw, 0.0, atol=1e-10) +@pytest.mark.ros def test_pose_unit_vectors(): """Test pose with unit vector positions.""" # Test unit x vector position @@ -472,6 +501,7 @@ def test_pose_unit_vectors(): assert pose_z.z == 1.0 +@pytest.mark.ros def test_pose_negative_coordinates(): """Test pose with negative coordinates.""" pose = Pose(-1.0, -2.0, -3.0, -0.1, -0.2, -0.3, 0.9) @@ -488,6 +518,7 @@ def test_pose_negative_coordinates(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_large_coordinates(): """Test pose with large coordinate values.""" large_value = 1000.0 @@ -508,6 +539,7 @@ def test_pose_large_coordinates(): "x,y,z", [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (0.5, -0.5, 1.5), (100.0, -100.0, 0.0)], ) +@pytest.mark.ros def test_pose_parametrized_positions(x, y, z): """Parametrized test for various position values.""" pose = Pose(x, y, z) @@ -533,6 +565,7 @@ def test_pose_parametrized_positions(x, y, z): (0.5, 0.5, 0.5, 0.5), # Equal components ], ) +@pytest.mark.ros def test_pose_parametrized_orientations(qx, qy, qz, qw): """Parametrized test for various orientation values.""" pose = Pose(0.0, 0.0, 0.0, qx, qy, qz, qw) @@ -549,6 +582,7 @@ def test_pose_parametrized_orientations(qx, qy, qz, qw): assert pose.orientation.w == qw +@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -568,6 +602,7 @@ def encodepass(): print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") +@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -584,6 +619,7 @@ def encodepass(): print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") +@pytest.mark.ros def test_pose_addition_translation_only(): """Test pose addition with translation only (identity rotations).""" # Two poses with only translations @@ -604,6 +640,7 @@ def test_pose_addition_translation_only(): assert result.orientation.w == 1.0 +@pytest.mark.ros def test_pose_addition_with_rotation(): """Test pose addition with rotation applied to translation.""" # First pose: at origin, rotated 90 degrees around Z (yaw) @@ -629,6 +666,7 @@ def test_pose_addition_with_rotation(): assert np.isclose(result.orientation.w, np.cos(angle / 2), atol=1e-10) +@pytest.mark.ros def test_pose_addition_rotation_composition(): """Test that rotations are properly composed.""" # First pose: 45 degrees around Z @@ -651,6 +689,7 @@ def test_pose_addition_rotation_composition(): assert np.isclose(result.orientation.w, expected_qw, atol=1e-10) +@pytest.mark.ros def test_pose_addition_full_transform(): """Test full pose composition with translation and rotation.""" # Robot pose: at (2, 1, 0), facing 90 degrees left (positive yaw) @@ -676,6 +715,7 @@ def test_pose_addition_full_transform(): assert np.isclose(object_in_world.yaw, robot_yaw, atol=1e-10) +@pytest.mark.ros def test_pose_addition_chain(): """Test chaining multiple pose additions.""" # Create a chain of transformations @@ -692,6 +732,7 @@ def test_pose_addition_chain(): assert result.position.z == 1.0 +@pytest.mark.ros def test_pose_addition_with_convertible(): """Test pose addition with convertible types.""" pose1 = Pose(1.0, 2.0, 3.0) @@ -711,6 +752,7 @@ def test_pose_addition_with_convertible(): assert result2.position.z == 3.0 +@pytest.mark.ros def test_pose_identity_addition(): """Test that adding identity pose leaves pose unchanged.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -728,6 +770,7 @@ def test_pose_identity_addition(): assert result.orientation.w == pose.orientation.w +@pytest.mark.ros def test_pose_addition_3d_rotation(): """Test pose addition with 3D rotations.""" # First pose: rotated around X axis (roll) @@ -752,8 +795,11 @@ def test_pose_addition_3d_rotation(): assert np.isclose(result.position.z, sin45 + cos45, atol=1e-10) +@pytest.mark.ros def test_pose_from_ros_msg(): """Test creating a Pose from a ROS Pose message.""" + if ROSPose is None: + pytest.skip("ROS not available") ros_msg = ROSPose() ros_msg.position = ROSPoint(x=1.0, y=2.0, z=3.0) ros_msg.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) @@ -769,8 +815,11 @@ def test_pose_from_ros_msg(): assert pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_to_ros_msg(): """Test converting a Pose to a ROS Pose message.""" + if ROSPose is None: + pytest.skip("ROS not available") pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) ros_msg = pose.to_ros_msg() @@ -785,8 +834,11 @@ def test_pose_to_ros_msg(): assert ros_msg.orientation.w == 0.9 +@pytest.mark.ros def test_pose_ros_roundtrip(): """Test round-trip conversion between Pose and ROS Pose.""" + if ROSPose is None: + pytest.skip("ROS not available") original = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) ros_msg = original.to_ros_msg() diff --git a/dimos/msgs/geometry_msgs/test_PoseStamped.py b/dimos/msgs/geometry_msgs/test_PoseStamped.py index 33ddee1fc3..e689fc6851 100644 --- a/dimos/msgs/geometry_msgs/test_PoseStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -15,11 +15,17 @@ import pickle import time -from geometry_msgs.msg import PoseStamped as ROSPoseStamped +import pytest + +try: + from geometry_msgs.msg import PoseStamped as ROSPoseStamped +except ImportError: + ROSPoseStamped = None from dimos.msgs.geometry_msgs import PoseStamped +@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -42,6 +48,7 @@ def test_lcm_encode_decode(): assert pose_dest == pose_source +@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of PoseStamped to/from binary LCM format.""" @@ -57,8 +64,11 @@ def test_pickle_encode_decode(): assert pose_dest == pose_source +@pytest.mark.ros def test_pose_stamped_from_ros_msg(): """Test creating a PoseStamped from a ROS PoseStamped message.""" + if ROSPoseStamped is None: + pytest.skip("ROS not available") ros_msg = ROSPoseStamped() ros_msg.header.frame_id = "world" ros_msg.header.stamp.sec = 123 @@ -84,8 +94,11 @@ def test_pose_stamped_from_ros_msg(): assert pose_stamped.orientation.w == 0.9 +@pytest.mark.ros def test_pose_stamped_to_ros_msg(): """Test converting a PoseStamped to a ROS PoseStamped message.""" + if ROSPoseStamped is None: + pytest.skip("ROS not available") pose_stamped = PoseStamped( ts=123.456, frame_id="base_link", @@ -108,8 +121,11 @@ def test_pose_stamped_to_ros_msg(): assert ros_msg.pose.orientation.w == 0.9 +@pytest.mark.ros def test_pose_stamped_ros_roundtrip(): """Test round-trip conversion between PoseStamped and ROS PoseStamped.""" + if ROSPoseStamped is None: + pytest.skip("ROS not available") original = PoseStamped( ts=123.789, frame_id="odom", diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py index d35946cc4a..32f19d27ff 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py @@ -15,10 +15,17 @@ import numpy as np import pytest from dimos_lcm.geometry_msgs import PoseWithCovariance as LCMPoseWithCovariance -from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion + +try: + from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance + from geometry_msgs.msg import Pose as ROSPose + from geometry_msgs.msg import Point as ROSPoint + from geometry_msgs.msg import Quaternion as ROSQuaternion +except ImportError: + ROSPoseWithCovariance = None + ROSPose = None + ROSPoint = None + ROSQuaternion = None from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -26,6 +33,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 +@pytest.mark.ros def test_pose_with_covariance_default_init(): """Test that default initialization creates a pose at origin with zero covariance.""" pose_cov = PoseWithCovariance() @@ -44,6 +52,7 @@ def test_pose_with_covariance_default_init(): assert pose_cov.covariance.shape == (36,) +@pytest.mark.ros def test_pose_with_covariance_pose_init(): """Test initialization with a Pose object.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -62,6 +71,7 @@ def test_pose_with_covariance_pose_init(): assert np.all(pose_cov.covariance == 0.0) +@pytest.mark.ros def test_pose_with_covariance_pose_and_covariance_init(): """Test initialization with pose and covariance.""" pose = Pose(1.0, 2.0, 3.0) @@ -77,6 +87,7 @@ def test_pose_with_covariance_pose_and_covariance_init(): assert np.array_equal(pose_cov.covariance, covariance) +@pytest.mark.ros def test_pose_with_covariance_list_covariance(): """Test initialization with covariance as a list.""" pose = Pose(1.0, 2.0, 3.0) @@ -88,6 +99,7 @@ def test_pose_with_covariance_list_covariance(): assert np.array_equal(pose_cov.covariance, np.array(covariance_list)) +@pytest.mark.ros def test_pose_with_covariance_copy_init(): """Test copy constructor.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -106,6 +118,7 @@ def test_pose_with_covariance_copy_init(): assert copy.covariance[0] != 999.0 +@pytest.mark.ros def test_pose_with_covariance_lcm_init(): """Test initialization from LCM message.""" lcm_msg = LCMPoseWithCovariance() @@ -133,6 +146,7 @@ def test_pose_with_covariance_lcm_init(): assert np.array_equal(pose_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_pose_with_covariance_dict_init(): """Test initialization from dictionary.""" pose_dict = {"pose": Pose(1.0, 2.0, 3.0), "covariance": list(range(36))} @@ -144,6 +158,7 @@ def test_pose_with_covariance_dict_init(): assert np.array_equal(pose_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_pose_with_covariance_dict_init_no_covariance(): """Test initialization from dictionary without covariance.""" pose_dict = {"pose": Pose(1.0, 2.0, 3.0)} @@ -153,6 +168,7 @@ def test_pose_with_covariance_dict_init_no_covariance(): assert np.all(pose_cov.covariance == 0.0) +@pytest.mark.ros def test_pose_with_covariance_tuple_init(): """Test initialization from tuple.""" pose = Pose(1.0, 2.0, 3.0) @@ -166,6 +182,7 @@ def test_pose_with_covariance_tuple_init(): assert np.array_equal(pose_cov.covariance, covariance) +@pytest.mark.ros def test_pose_with_covariance_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -191,6 +208,7 @@ def test_pose_with_covariance_properties(): assert pose_cov.yaw == pose.yaw +@pytest.mark.ros def test_pose_with_covariance_matrix_property(): """Test covariance matrix property.""" pose = Pose() @@ -209,6 +227,7 @@ def test_pose_with_covariance_matrix_property(): assert np.array_equal(pose_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) +@pytest.mark.ros def test_pose_with_covariance_repr(): """Test string representation.""" pose = Pose(1.234, 2.567, 3.891) @@ -221,6 +240,7 @@ def test_pose_with_covariance_repr(): assert "36 elements" in repr_str +@pytest.mark.ros def test_pose_with_covariance_str(): """Test string formatting.""" pose = Pose(1.234, 2.567, 3.891) @@ -236,6 +256,7 @@ def test_pose_with_covariance_str(): assert "6.000" in str_repr # Trace of identity matrix is 6 +@pytest.mark.ros def test_pose_with_covariance_equality(): """Test equality comparison.""" pose1 = Pose(1.0, 2.0, 3.0) @@ -264,6 +285,7 @@ def test_pose_with_covariance_equality(): assert pose_cov1 != None +@pytest.mark.ros def test_pose_with_covariance_lcm_encode_decode(): """Test LCM encoding and decoding.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -281,8 +303,11 @@ def test_pose_with_covariance_lcm_encode_decode(): assert isinstance(decoded.covariance, np.ndarray) +@pytest.mark.ros def test_pose_with_covariance_from_ros_msg(): """Test creating from ROS message.""" + if ROSPoseWithCovariance is None: + pytest.skip("ROS not available") ros_msg = ROSPoseWithCovariance() ros_msg.pose.position = ROSPoint(x=1.0, y=2.0, z=3.0) ros_msg.pose.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) @@ -300,8 +325,11 @@ def test_pose_with_covariance_from_ros_msg(): assert np.array_equal(pose_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_pose_with_covariance_to_ros_msg(): """Test converting to ROS message.""" + if ROSPoseWithCovariance is None: + pytest.skip("ROS not available") pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) covariance = np.arange(36, dtype=float) pose_cov = PoseWithCovariance(pose, covariance) @@ -319,8 +347,11 @@ def test_pose_with_covariance_to_ros_msg(): assert list(ros_msg.covariance) == list(range(36)) +@pytest.mark.ros def test_pose_with_covariance_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" + if ROSPoseWithCovariance is None: + pytest.skip("ROS not available") pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) covariance = np.random.rand(36) original = PoseWithCovariance(pose, covariance) @@ -331,6 +362,7 @@ def test_pose_with_covariance_ros_roundtrip(): assert restored == original +@pytest.mark.ros def test_pose_with_covariance_zero_covariance(): """Test with zero covariance matrix.""" pose = Pose(1.0, 2.0, 3.0) @@ -340,6 +372,7 @@ def test_pose_with_covariance_zero_covariance(): assert np.trace(pose_cov.covariance_matrix) == 0.0 +@pytest.mark.ros def test_pose_with_covariance_diagonal_covariance(): """Test with diagonal covariance matrix.""" pose = Pose() @@ -368,6 +401,7 @@ def test_pose_with_covariance_diagonal_covariance(): "x,y,z", [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (100.0, -100.0, 0.0)], ) +@pytest.mark.ros def test_pose_with_covariance_parametrized_positions(x, y, z): """Parametrized test for various position values.""" pose = Pose(x, y, z) diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py index f6b7560e16..af921ef67e 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py @@ -16,16 +16,27 @@ import numpy as np import pytest + +try: + from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped + from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance + from geometry_msgs.msg import Pose as ROSPose + from geometry_msgs.msg import Point as ROSPoint + from geometry_msgs.msg import Quaternion as ROSQuaternion + from std_msgs.msg import Header as ROSHeader + from builtin_interfaces.msg import Time as ROSTime +except ImportError: + ROSHeader = None + ROSPoseWithCovarianceStamped = None + ROSPose = None + ROSQuaternion = None + ROSPoint = None + ROSTime = None + ROSPoseWithCovariance = None + from dimos_lcm.geometry_msgs import PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime -from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped -from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion -from std_msgs.msg import Header as ROSHeader -from builtin_interfaces.msg import Time as ROSTime from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -34,8 +45,23 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 +@pytest.mark.ros def test_pose_with_covariance_stamped_default_init(): """Test default initialization.""" + if ROSPoseWithCovariance is None: + pytest.skip("ROS not available") + if ROSTime is None: + pytest.skip("ROS not available") + if ROSPoint is None: + pytest.skip("ROS not available") + if ROSQuaternion is None: + pytest.skip("ROS not available") + if ROSPose is None: + pytest.skip("ROS not available") + if ROSPoseWithCovarianceStamped is None: + pytest.skip("ROS not available") + if ROSHeader is None: + pytest.skip("ROS not available") pose_cov_stamped = PoseWithCovarianceStamped() # Should have current timestamp @@ -52,6 +78,7 @@ def test_pose_with_covariance_stamped_default_init(): assert np.all(pose_cov_stamped.covariance == 0.0) +@pytest.mark.ros def test_pose_with_covariance_stamped_with_timestamp(): """Test initialization with specific timestamp.""" ts = 1234567890.123456 @@ -62,6 +89,7 @@ def test_pose_with_covariance_stamped_with_timestamp(): assert pose_cov_stamped.frame_id == frame_id +@pytest.mark.ros def test_pose_with_covariance_stamped_with_pose(): """Test initialization with pose.""" ts = 1234567890.123456 @@ -81,6 +109,7 @@ def test_pose_with_covariance_stamped_with_pose(): assert np.array_equal(pose_cov_stamped.covariance, covariance) +@pytest.mark.ros def test_pose_with_covariance_stamped_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -111,6 +140,7 @@ def test_pose_with_covariance_stamped_properties(): assert np.trace(cov_matrix) == 6.0 +@pytest.mark.ros def test_pose_with_covariance_stamped_str(): """Test string representation.""" pose = Pose(1.234, 2.567, 3.891) @@ -128,6 +158,7 @@ def test_pose_with_covariance_stamped_str(): assert "12.000" in str_repr # Trace of 2*identity is 12 +@pytest.mark.ros def test_pose_with_covariance_stamped_lcm_encode_decode(): """Test LCM encoding and decoding.""" ts = 1234567890.123456 @@ -158,6 +189,7 @@ def test_pose_with_covariance_stamped_lcm_encode_decode(): assert np.array_equal(decoded.covariance, covariance) +@pytest.mark.ros def test_pose_with_covariance_stamped_from_ros_msg(): """Test creating from ROS message.""" ros_msg = ROSPoseWithCovarianceStamped() @@ -190,6 +222,7 @@ def test_pose_with_covariance_stamped_from_ros_msg(): assert np.array_equal(pose_cov_stamped.covariance, np.arange(36)) +@pytest.mark.ros def test_pose_with_covariance_stamped_to_ros_msg(): """Test converting to ROS message.""" ts = 1234567890.567890 @@ -218,6 +251,7 @@ def test_pose_with_covariance_stamped_to_ros_msg(): assert list(ros_msg.pose.covariance) == list(range(36)) +@pytest.mark.ros def test_pose_with_covariance_stamped_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" ts = 2147483647.987654 # Max int32 value for ROS Time.sec @@ -247,6 +281,7 @@ def test_pose_with_covariance_stamped_ros_roundtrip(): assert np.allclose(restored.covariance, original.covariance) +@pytest.mark.ros def test_pose_with_covariance_stamped_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" pose_cov_stamped = PoseWithCovarianceStamped(ts=0.0) @@ -256,6 +291,7 @@ def test_pose_with_covariance_stamped_zero_timestamp(): assert pose_cov_stamped.ts <= time.time() +@pytest.mark.ros def test_pose_with_covariance_stamped_inheritance(): """Test that it properly inherits from PoseWithCovariance and Timestamped.""" pose = Pose(1.0, 2.0, 3.0) @@ -276,6 +312,7 @@ def test_pose_with_covariance_stamped_inheritance(): assert hasattr(pose_cov_stamped, "covariance") +@pytest.mark.ros def test_pose_with_covariance_stamped_sec_nsec(): """Test the sec_nsec helper function.""" from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import sec_nsec @@ -309,6 +346,7 @@ def test_pose_with_covariance_stamped_sec_nsec(): "frame_id", ["", "map", "odom", "base_link", "camera_optical_frame", "sensor/lidar/front"], ) +@pytest.mark.ros def test_pose_with_covariance_stamped_frame_ids(frame_id): """Test various frame ID values.""" pose_cov_stamped = PoseWithCovarianceStamped(frame_id=frame_id) @@ -322,6 +360,7 @@ def test_pose_with_covariance_stamped_frame_ids(frame_id): assert restored.frame_id == frame_id +@pytest.mark.ros def test_pose_with_covariance_stamped_different_covariances(): """Test with different covariance patterns.""" pose = Pose(1.0, 2.0, 3.0) diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index d6c695c858..ca30d6cf46 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -17,13 +17,19 @@ import numpy as np import pytest + +try: + from geometry_msgs.msg import TransformStamped as ROSTransformStamped +except ImportError: + ROSTransformStamped = None + from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped -from geometry_msgs.msg import TransformStamped as ROSTransformStamped from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 +@pytest.mark.ros def test_transform_initialization(): # Test default initialization (identity transform) tf = Transform() @@ -70,6 +76,7 @@ def test_transform_initialization(): assert tf9.rotation == Quaternion(0, 0, 1, 0) +@pytest.mark.ros def test_transform_identity(): # Test identity class method tf = Transform.identity() @@ -83,6 +90,7 @@ def test_transform_identity(): assert tf == Transform() +@pytest.mark.ros def test_transform_equality(): 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)) @@ -97,6 +105,7 @@ def test_transform_equality(): assert tf1 != "not a transform" +@pytest.mark.ros def test_transform_string_representations(): tf = Transform( translation=Vector3(1.5, -2.0, 3.14), rotation=Quaternion(0, 0, 0.707107, 0.707107) @@ -116,6 +125,7 @@ def test_transform_string_representations(): assert "Rotation:" in str_str +@pytest.mark.ros def test_pose_add_transform(): initial_pose = Pose(1.0, 0.0, 0.0) @@ -163,6 +173,7 @@ def test_pose_add_transform(): print(found_tf.rotation, found_tf.translation) +@pytest.mark.ros def test_pose_add_transform_with_rotation(): # Create a pose at (0, 0, 0) rotated 90 degrees around Z angle = np.pi / 2 @@ -225,6 +236,7 @@ def test_pose_add_transform_with_rotation(): assert np.isclose(transformed_pose2.orientation.w, np.cos(total_angle2 / 2), atol=1e-10) +@pytest.mark.ros def test_lcm_encode_decode(): angle = np.pi / 2 transform = Transform( @@ -239,6 +251,7 @@ def test_lcm_encode_decode(): assert decoded_transform == transform +@pytest.mark.ros def test_transform_addition(): # Test 1: Simple translation addition (no rotation) t1 = Transform( @@ -315,8 +328,11 @@ def test_transform_addition(): t1 + "not a transform" +@pytest.mark.ros def test_transform_from_pose(): """Test converting Pose to Transform""" + if ROSTransformStamped is None: + pytest.skip("ROS not available") # Create a Pose with position and orientation pose = Pose( position=Vector3(1.0, 2.0, 3.0), @@ -335,6 +351,7 @@ def test_transform_from_pose(): # validating results from example @ # https://foxglove.dev/blog/understanding-ros-transforms +@pytest.mark.ros def test_transform_from_ros(): """Test converting PoseStamped to Transform""" test_time = time.time() @@ -365,6 +382,7 @@ def test_transform_from_ros(): assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) +@pytest.mark.ros def test_transform_from_pose_stamped(): """Test converting PoseStamped to Transform""" # Create a PoseStamped with position, orientation, timestamp and frame @@ -387,6 +405,7 @@ def test_transform_from_pose_stamped(): assert transform.child_frame_id == "robot_base" # passed as first argument +@pytest.mark.ros def test_transform_from_pose_variants(): """Test from_pose with different Pose initialization methods""" # Test with Pose created from x,y,z @@ -412,6 +431,7 @@ def test_transform_from_pose_variants(): assert transform3.translation.z == 12.0 +@pytest.mark.ros def test_transform_from_pose_invalid_type(): """Test that from_pose raises TypeError for invalid types""" with pytest.raises(TypeError): @@ -424,6 +444,7 @@ def test_transform_from_pose_invalid_type(): Transform.from_pose(None) +@pytest.mark.ros def test_transform_from_ros_transform_stamped(): """Test creating a Transform from a ROS TransformStamped message.""" ros_msg = ROSTransformStamped() @@ -453,6 +474,7 @@ def test_transform_from_ros_transform_stamped(): assert transform.rotation.w == 0.9 +@pytest.mark.ros def test_transform_to_ros_transform_stamped(): """Test converting a Transform to a ROS TransformStamped message.""" transform = Transform( @@ -479,6 +501,7 @@ def test_transform_to_ros_transform_stamped(): assert ros_msg.transform.rotation.w == 0.85 +@pytest.mark.ros def test_transform_ros_roundtrip(): """Test round-trip conversion between Transform and ROS TransformStamped.""" original = Transform( diff --git a/dimos/msgs/geometry_msgs/test_Twist.py b/dimos/msgs/geometry_msgs/test_Twist.py index 6cf3fe0f03..3636a20b17 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -14,13 +14,20 @@ import numpy as np import pytest + +try: + from geometry_msgs.msg import Twist as ROSTwist + from geometry_msgs.msg import Vector3 as ROSVector3 +except ImportError: + ROSTwist = None + ROSVector3 = None + from dimos_lcm.geometry_msgs import Twist as LCMTwist -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Vector3 as ROSVector3 from dimos.msgs.geometry_msgs import Quaternion, Twist, Vector3 +@pytest.mark.ros def test_twist_initialization(): # Test default initialization (zero twist) tw = Twist() @@ -98,6 +105,7 @@ def test_twist_initialization(): assert tw11.angular.is_zero() # Identity quaternion -> zero euler angles +@pytest.mark.ros def test_twist_zero(): # Test zero class method tw = Twist.zero() @@ -109,6 +117,7 @@ def test_twist_zero(): assert tw == Twist() +@pytest.mark.ros def test_twist_equality(): tw1 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) tw2 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) @@ -121,6 +130,7 @@ def test_twist_equality(): assert tw1 != "not a twist" +@pytest.mark.ros def test_twist_string_representations(): tw = Twist(Vector3(1.5, -2.0, 3.14), Vector3(0.1, -0.2, 0.3)) @@ -139,6 +149,7 @@ def test_twist_string_representations(): assert "Angular:" in str_str +@pytest.mark.ros def test_twist_is_zero(): # Test zero twist tw1 = Twist() @@ -157,6 +168,7 @@ def test_twist_is_zero(): assert not tw4.is_zero() +@pytest.mark.ros def test_twist_bool(): # Test zero twist is False tw1 = Twist() @@ -173,6 +185,7 @@ def test_twist_bool(): assert tw4 +@pytest.mark.ros def test_twist_lcm_encoding(): # Test encoding and decoding tw = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.1, 0.2, 0.3)) @@ -190,6 +203,7 @@ def test_twist_lcm_encoding(): assert decoded == tw +@pytest.mark.ros def test_twist_with_lists(): # Test initialization with lists instead of Vector3 tw1 = Twist(linear=[1, 2, 3], angular=[0.1, 0.2, 0.3]) @@ -202,8 +216,13 @@ def test_twist_with_lists(): assert tw2.angular == Vector3(0.4, 0.5, 0.6) +@pytest.mark.ros def test_twist_from_ros_msg(): """Test Twist.from_ros_msg conversion.""" + if ROSVector3 is None: + pytest.skip("ROS not available") + if ROSTwist is None: + pytest.skip("ROS not available") # Create ROS message ros_msg = ROSTwist() ros_msg.linear = ROSVector3(x=10.0, y=20.0, z=30.0) @@ -221,8 +240,13 @@ def test_twist_from_ros_msg(): assert lcm_msg.angular.z == 3.0 +@pytest.mark.ros def test_twist_to_ros_msg(): """Test Twist.to_ros_msg conversion.""" + if ROSVector3 is None: + pytest.skip("ROS not available") + if ROSTwist is None: + pytest.skip("ROS not available") # Create LCM message lcm_msg = Twist(linear=Vector3(40.0, 50.0, 60.0), angular=Vector3(4.0, 5.0, 6.0)) @@ -238,6 +262,7 @@ def test_twist_to_ros_msg(): assert ros_msg.angular.z == 6.0 +@pytest.mark.ros def test_ros_zero_twist_conversion(): """Test conversion of zero twist messages between ROS and LCM.""" # Test ROS to LCM with zero twist @@ -256,8 +281,11 @@ def test_ros_zero_twist_conversion(): assert ros_zero2.angular.z == 0.0 +@pytest.mark.ros def test_ros_negative_values_conversion(): """Test ROS conversion with negative values.""" + if ROSTwist is None: + pytest.skip("ROS not available") # Create ROS message with negative values ros_msg = ROSTwist() ros_msg.linear = ROSVector3(x=-1.5, y=-2.5, z=-3.5) @@ -275,6 +303,7 @@ def test_ros_negative_values_conversion(): assert ros_msg2.angular.z == -0.3 +@pytest.mark.ros def test_ros_roundtrip_conversion(): """Test round-trip conversion maintains data integrity.""" # LCM -> ROS -> LCM diff --git a/dimos/msgs/geometry_msgs/test_TwistStamped.py b/dimos/msgs/geometry_msgs/test_TwistStamped.py index c84cba8cf2..326b3f5468 100644 --- a/dimos/msgs/geometry_msgs/test_TwistStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistStamped.py @@ -12,16 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. +import pytest import pickle import time -from geometry_msgs.msg import TwistStamped as ROSTwistStamped + +try: + from geometry_msgs.msg import TwistStamped as ROSTwistStamped +except ImportError: + ROSTwistStamped = None from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of TwistStamped to/from binary LCM format.""" + if ROSTwistStamped is None: + pytest.skip("ROS not available") twist_source = TwistStamped( ts=time.time(), @@ -42,6 +50,7 @@ def test_lcm_encode_decode(): assert twist_dest == twist_source +@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of TwistStamped to/from binary pickle format.""" @@ -57,6 +66,7 @@ def test_pickle_encode_decode(): assert twist_dest == twist_source +@pytest.mark.ros def test_twist_stamped_from_ros_msg(): """Test creating a TwistStamped from a ROS TwistStamped message.""" ros_msg = ROSTwistStamped() @@ -82,6 +92,7 @@ def test_twist_stamped_from_ros_msg(): assert twist_stamped.angular.z == 0.3 +@pytest.mark.ros def test_twist_stamped_to_ros_msg(): """Test converting a TwistStamped to a ROS TwistStamped message.""" twist_stamped = TwistStamped( @@ -105,6 +116,7 @@ def test_twist_stamped_to_ros_msg(): assert ros_msg.twist.angular.z == 0.3 +@pytest.mark.ros def test_twist_stamped_ros_roundtrip(): """Test round-trip conversion between TwistStamped and ROS TwistStamped.""" original = TwistStamped( diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py index 0a2dbdff06..9bfda26689 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py @@ -14,18 +14,30 @@ import numpy as np import pytest + +try: + from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance + from geometry_msgs.msg import Twist as ROSTwist + from geometry_msgs.msg import Vector3 as ROSVector3 +except ImportError: + ROSTwist = None + ROSTwistWithCovariance = None + ROSVector3 = None + from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance -from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Vector3 as ROSVector3 from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance from dimos.msgs.geometry_msgs.Vector3 import Vector3 +@pytest.mark.ros def test_twist_with_covariance_default_init(): """Test that default initialization creates a zero twist with zero covariance.""" + if ROSVector3 is None: + pytest.skip("ROS not available") + if ROSTwistWithCovariance is None: + pytest.skip("ROS not available") twist_cov = TwistWithCovariance() # Twist should be zero @@ -41,6 +53,7 @@ def test_twist_with_covariance_default_init(): assert twist_cov.covariance.shape == (36,) +@pytest.mark.ros def test_twist_with_covariance_twist_init(): """Test initialization with a Twist object.""" linear = Vector3(1.0, 2.0, 3.0) @@ -60,6 +73,7 @@ def test_twist_with_covariance_twist_init(): assert np.all(twist_cov.covariance == 0.0) +@pytest.mark.ros def test_twist_with_covariance_twist_and_covariance_init(): """Test initialization with twist and covariance.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -75,6 +89,7 @@ def test_twist_with_covariance_twist_and_covariance_init(): assert np.array_equal(twist_cov.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_tuple_init(): """Test initialization with tuple of (linear, angular) velocities.""" linear = [1.0, 2.0, 3.0] @@ -94,6 +109,7 @@ def test_twist_with_covariance_tuple_init(): assert np.array_equal(twist_cov.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_list_covariance(): """Test initialization with covariance as a list.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -105,6 +121,7 @@ def test_twist_with_covariance_list_covariance(): assert np.array_equal(twist_cov.covariance, np.array(covariance_list)) +@pytest.mark.ros def test_twist_with_covariance_copy_init(): """Test copy constructor.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -123,6 +140,7 @@ def test_twist_with_covariance_copy_init(): assert copy.covariance[0] != 999.0 +@pytest.mark.ros def test_twist_with_covariance_lcm_init(): """Test initialization from LCM message.""" lcm_msg = LCMTwistWithCovariance() @@ -148,6 +166,7 @@ def test_twist_with_covariance_lcm_init(): assert np.array_equal(twist_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_twist_with_covariance_dict_init(): """Test initialization from dictionary.""" twist_dict = { @@ -162,6 +181,7 @@ def test_twist_with_covariance_dict_init(): assert np.array_equal(twist_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_twist_with_covariance_dict_init_no_covariance(): """Test initialization from dictionary without covariance.""" twist_dict = {"twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3))} @@ -171,6 +191,7 @@ def test_twist_with_covariance_dict_init_no_covariance(): assert np.all(twist_cov.covariance == 0.0) +@pytest.mark.ros def test_twist_with_covariance_tuple_of_tuple_init(): """Test initialization from tuple of (twist_tuple, covariance).""" twist_tuple = ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]) @@ -186,6 +207,7 @@ def test_twist_with_covariance_tuple_of_tuple_init(): assert np.array_equal(twist_cov.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_properties(): """Test convenience properties.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -200,6 +222,7 @@ def test_twist_with_covariance_properties(): assert twist_cov.angular.z == 0.3 +@pytest.mark.ros def test_twist_with_covariance_matrix_property(): """Test covariance matrix property.""" twist = Twist() @@ -218,6 +241,7 @@ def test_twist_with_covariance_matrix_property(): assert np.array_equal(twist_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) +@pytest.mark.ros def test_twist_with_covariance_repr(): """Test string representation.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) @@ -230,6 +254,7 @@ def test_twist_with_covariance_repr(): assert "36 elements" in repr_str +@pytest.mark.ros def test_twist_with_covariance_str(): """Test string formatting.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) @@ -245,6 +270,7 @@ def test_twist_with_covariance_str(): assert "6.000" in str_repr # Trace of identity matrix is 6 +@pytest.mark.ros def test_twist_with_covariance_equality(): """Test equality comparison.""" twist1 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -273,6 +299,7 @@ def test_twist_with_covariance_equality(): assert twist_cov1 != None +@pytest.mark.ros def test_twist_with_covariance_is_zero(): """Test is_zero method.""" # Zero twist @@ -287,6 +314,7 @@ def test_twist_with_covariance_is_zero(): assert twist_cov2 # Boolean conversion +@pytest.mark.ros def test_twist_with_covariance_lcm_encode_decode(): """Test LCM encoding and decoding.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -304,6 +332,7 @@ def test_twist_with_covariance_lcm_encode_decode(): assert isinstance(decoded.covariance, np.ndarray) +@pytest.mark.ros def test_twist_with_covariance_from_ros_msg(): """Test creating from ROS message.""" ros_msg = ROSTwistWithCovariance() @@ -322,6 +351,7 @@ def test_twist_with_covariance_from_ros_msg(): assert np.array_equal(twist_cov.covariance, np.arange(36)) +@pytest.mark.ros def test_twist_with_covariance_to_ros_msg(): """Test converting to ROS message.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -340,6 +370,7 @@ def test_twist_with_covariance_to_ros_msg(): assert list(ros_msg.covariance) == list(range(36)) +@pytest.mark.ros def test_twist_with_covariance_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" twist = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.15, 0.25, 0.35)) @@ -352,6 +383,7 @@ def test_twist_with_covariance_ros_roundtrip(): assert restored == original +@pytest.mark.ros def test_twist_with_covariance_zero_covariance(): """Test with zero covariance matrix.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -361,6 +393,7 @@ def test_twist_with_covariance_zero_covariance(): assert np.trace(twist_cov.covariance_matrix) == 0.0 +@pytest.mark.ros def test_twist_with_covariance_diagonal_covariance(): """Test with diagonal covariance matrix.""" twist = Twist() @@ -394,6 +427,7 @@ def test_twist_with_covariance_diagonal_covariance(): ([100.0, -100.0, 0.0], [3.14, -3.14, 0.0]), ], ) +@pytest.mark.ros def test_twist_with_covariance_parametrized_velocities(linear, angular): """Parametrized test for various velocity values.""" twist = Twist(linear, angular) diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py index 7abc3f689b..adeaa760df 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py @@ -16,15 +16,25 @@ import numpy as np import pytest + +try: + from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped + from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance + from geometry_msgs.msg import Twist as ROSTwist + from geometry_msgs.msg import Vector3 as ROSVector3 + from std_msgs.msg import Header as ROSHeader + from builtin_interfaces.msg import Time as ROSTime +except ImportError: + ROSTwistWithCovarianceStamped = None + ROSTwist = None + ROSHeader = None + ROSTime = None + ROSTwistWithCovariance = None + ROSVector3 = None + from dimos_lcm.geometry_msgs import TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime -from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped -from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Vector3 as ROSVector3 -from std_msgs.msg import Header as ROSHeader -from builtin_interfaces.msg import Time as ROSTime from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance @@ -32,8 +42,21 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 +@pytest.mark.ros def test_twist_with_covariance_stamped_default_init(): """Test default initialization.""" + if ROSVector3 is None: + pytest.skip("ROS not available") + if ROSTwistWithCovariance is None: + pytest.skip("ROS not available") + if ROSTime is None: + pytest.skip("ROS not available") + if ROSHeader is None: + pytest.skip("ROS not available") + if ROSTwist is None: + pytest.skip("ROS not available") + if ROSTwistWithCovarianceStamped is None: + pytest.skip("ROS not available") twist_cov_stamped = TwistWithCovarianceStamped() # Should have current timestamp @@ -52,6 +75,7 @@ def test_twist_with_covariance_stamped_default_init(): assert np.all(twist_cov_stamped.covariance == 0.0) +@pytest.mark.ros def test_twist_with_covariance_stamped_with_timestamp(): """Test initialization with specific timestamp.""" ts = 1234567890.123456 @@ -62,6 +86,7 @@ def test_twist_with_covariance_stamped_with_timestamp(): assert twist_cov_stamped.frame_id == frame_id +@pytest.mark.ros def test_twist_with_covariance_stamped_with_twist(): """Test initialization with twist.""" ts = 1234567890.123456 @@ -81,6 +106,7 @@ def test_twist_with_covariance_stamped_with_twist(): assert np.array_equal(twist_cov_stamped.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_stamped_with_tuple(): """Test initialization with tuple of velocities.""" ts = 1234567890.123456 @@ -100,6 +126,7 @@ def test_twist_with_covariance_stamped_with_tuple(): assert np.array_equal(twist_cov_stamped.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_stamped_properties(): """Test convenience properties.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -122,6 +149,7 @@ def test_twist_with_covariance_stamped_properties(): assert np.trace(cov_matrix) == 6.0 +@pytest.mark.ros def test_twist_with_covariance_stamped_str(): """Test string representation.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.111, 0.222, 0.333)) @@ -139,6 +167,7 @@ def test_twist_with_covariance_stamped_str(): assert "12.000" in str_repr # Trace of 2*identity is 12 +@pytest.mark.ros def test_twist_with_covariance_stamped_lcm_encode_decode(): """Test LCM encoding and decoding.""" ts = 1234567890.123456 @@ -170,6 +199,7 @@ def test_twist_with_covariance_stamped_lcm_encode_decode(): assert np.array_equal(decoded.covariance, covariance) +@pytest.mark.ros def test_twist_with_covariance_stamped_from_ros_msg(): """Test creating from ROS message.""" ros_msg = ROSTwistWithCovarianceStamped() @@ -201,6 +231,7 @@ def test_twist_with_covariance_stamped_from_ros_msg(): assert np.array_equal(twist_cov_stamped.covariance, np.arange(36)) +@pytest.mark.ros def test_twist_with_covariance_stamped_to_ros_msg(): """Test converting to ROS message.""" ts = 1234567890.567890 @@ -228,6 +259,7 @@ def test_twist_with_covariance_stamped_to_ros_msg(): assert list(ros_msg.twist.covariance) == list(range(36)) +@pytest.mark.ros def test_twist_with_covariance_stamped_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" ts = 2147483647.987654 # Max int32 value for ROS Time.sec @@ -258,6 +290,7 @@ def test_twist_with_covariance_stamped_ros_roundtrip(): assert np.allclose(restored.covariance, original.covariance) +@pytest.mark.ros def test_twist_with_covariance_stamped_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" twist_cov_stamped = TwistWithCovarianceStamped(ts=0.0) @@ -267,6 +300,7 @@ def test_twist_with_covariance_stamped_zero_timestamp(): assert twist_cov_stamped.ts <= time.time() +@pytest.mark.ros def test_twist_with_covariance_stamped_inheritance(): """Test that it properly inherits from TwistWithCovariance and Timestamped.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -287,6 +321,7 @@ def test_twist_with_covariance_stamped_inheritance(): assert hasattr(twist_cov_stamped, "covariance") +@pytest.mark.ros def test_twist_with_covariance_stamped_is_zero(): """Test is_zero method inheritance.""" # Zero twist @@ -301,6 +336,7 @@ def test_twist_with_covariance_stamped_is_zero(): assert twist_cov_stamped2 # Boolean conversion +@pytest.mark.ros def test_twist_with_covariance_stamped_sec_nsec(): """Test the sec_nsec helper function.""" from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import sec_nsec @@ -334,6 +370,7 @@ def test_twist_with_covariance_stamped_sec_nsec(): "frame_id", ["", "map", "odom", "base_link", "cmd_vel", "sensor/velocity/front"], ) +@pytest.mark.ros def test_twist_with_covariance_stamped_frame_ids(frame_id): """Test various frame ID values.""" twist_cov_stamped = TwistWithCovarianceStamped(frame_id=frame_id) @@ -347,6 +384,7 @@ def test_twist_with_covariance_stamped_frame_ids(frame_id): assert restored.frame_id == frame_id +@pytest.mark.ros def test_twist_with_covariance_stamped_different_covariances(): """Test with different covariance patterns.""" twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.5)) diff --git a/dimos/msgs/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py index 6961cc2bed..e97ac480ee 100644 --- a/dimos/msgs/nav_msgs/test_Odometry.py +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -16,17 +16,31 @@ import numpy as np import pytest + +try: + from nav_msgs.msg import Odometry as ROSOdometry + from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance + from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance + from geometry_msgs.msg import Pose as ROSPose + from geometry_msgs.msg import Twist as ROSTwist + from geometry_msgs.msg import Point as ROSPoint + from geometry_msgs.msg import Quaternion as ROSQuaternion + from geometry_msgs.msg import Vector3 as ROSVector3 + from std_msgs.msg import Header as ROSHeader + from builtin_interfaces.msg import Time as ROSTime +except ImportError: + ROSTwist = None + ROSHeader = None + ROSPose = None + ROSPoseWithCovariance = None + ROSQuaternion = None + ROSOdometry = None + ROSPoint = None + ROSTime = None + ROSTwistWithCovariance = None + ROSVector3 = None + from dimos_lcm.nav_msgs import Odometry as LCMOdometry -from nav_msgs.msg import Odometry as ROSOdometry -from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance -from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion -from geometry_msgs.msg import Vector3 as ROSVector3 -from std_msgs.msg import Header as ROSHeader -from builtin_interfaces.msg import Time as ROSTime from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.geometry_msgs.Pose import Pose @@ -37,8 +51,29 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion +@pytest.mark.ros def test_odometry_default_init(): """Test default initialization.""" + if ROSVector3 is None: + pytest.skip("ROS not available") + if ROSTwistWithCovariance is None: + pytest.skip("ROS not available") + if ROSTime is None: + pytest.skip("ROS not available") + if ROSPoint is None: + pytest.skip("ROS not available") + if ROSOdometry is None: + pytest.skip("ROS not available") + if ROSQuaternion is None: + pytest.skip("ROS not available") + if ROSPoseWithCovariance is None: + pytest.skip("ROS not available") + if ROSPose is None: + pytest.skip("ROS not available") + if ROSHeader is None: + pytest.skip("ROS not available") + if ROSTwist is None: + pytest.skip("ROS not available") odom = Odometry() # Should have current timestamp @@ -65,6 +100,7 @@ def test_odometry_default_init(): assert np.all(odom.twist.covariance == 0.0) +@pytest.mark.ros def test_odometry_with_frames(): """Test initialization with frame IDs.""" ts = 1234567890.123456 @@ -78,6 +114,7 @@ def test_odometry_with_frames(): assert odom.child_frame_id == child_frame_id +@pytest.mark.ros def test_odometry_with_pose_and_twist(): """Test initialization with pose and twist.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -92,6 +129,7 @@ def test_odometry_with_pose_and_twist(): assert odom.twist.twist.angular.z == 0.1 +@pytest.mark.ros def test_odometry_with_covariances(): """Test initialization with pose and twist with covariances.""" pose = Pose(1.0, 2.0, 3.0) @@ -116,6 +154,7 @@ def test_odometry_with_covariances(): assert np.array_equal(odom.twist.covariance, twist_cov) +@pytest.mark.ros def test_odometry_copy_constructor(): """Test copy constructor.""" original = Odometry( @@ -134,6 +173,7 @@ def test_odometry_copy_constructor(): assert copy.twist is not original.twist +@pytest.mark.ros def test_odometry_dict_init(): """Test initialization from dictionary.""" odom_dict = { @@ -153,6 +193,7 @@ def test_odometry_dict_init(): assert odom.twist.linear.x == 0.5 +@pytest.mark.ros def test_odometry_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -196,6 +237,7 @@ def test_odometry_properties(): assert odom.yaw == pose.yaw +@pytest.mark.ros def test_odometry_str_repr(): """Test string representations.""" odom = Odometry( @@ -219,6 +261,7 @@ def test_odometry_str_repr(): assert "0.500" in str_repr +@pytest.mark.ros def test_odometry_equality(): """Test equality comparison.""" odom1 = Odometry( @@ -250,6 +293,7 @@ def test_odometry_equality(): assert odom1 != "not an odometry" +@pytest.mark.ros def test_odometry_lcm_encode_decode(): """Test LCM encoding and decoding.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -277,6 +321,7 @@ def test_odometry_lcm_encode_decode(): assert decoded.twist == source.twist +@pytest.mark.ros def test_odometry_from_ros_msg(): """Test creating from ROS message.""" ros_msg = ROSOdometry() @@ -314,6 +359,7 @@ def test_odometry_from_ros_msg(): assert np.array_equal(odom.twist.covariance, np.arange(36, 72)) +@pytest.mark.ros def test_odometry_to_ros_msg(): """Test converting to ROS message.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -357,6 +403,7 @@ def test_odometry_to_ros_msg(): assert list(ros_msg.twist.covariance) == list(range(36, 72)) +@pytest.mark.ros def test_odometry_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) @@ -383,6 +430,7 @@ def test_odometry_ros_roundtrip(): assert restored.twist == original.twist +@pytest.mark.ros def test_odometry_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" odom = Odometry(ts=0.0) @@ -392,6 +440,7 @@ def test_odometry_zero_timestamp(): assert odom.ts <= time.time() +@pytest.mark.ros def test_odometry_with_just_pose(): """Test initialization with just a Pose (no covariance).""" pose = Pose(1.0, 2.0, 3.0) @@ -405,6 +454,7 @@ def test_odometry_with_just_pose(): assert np.all(odom.twist.covariance == 0.0) # Twist should also be zero +@pytest.mark.ros def test_odometry_with_just_twist(): """Test initialization with just a Twist (no covariance).""" twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) @@ -427,6 +477,7 @@ def test_odometry_with_just_twist(): ("", ""), # Empty frames ], ) +@pytest.mark.ros def test_odometry_frame_combinations(frame_id, child_frame_id): """Test various frame ID combinations.""" odom = Odometry(frame_id=frame_id, child_frame_id=child_frame_id) @@ -444,6 +495,7 @@ def test_odometry_frame_combinations(frame_id, child_frame_id): assert restored.child_frame_id == child_frame_id +@pytest.mark.ros def test_odometry_typical_robot_scenario(): """Test a typical robot odometry scenario.""" # Robot moving forward at 0.5 m/s with slight rotation diff --git a/dimos/msgs/nav_msgs/test_Path.py b/dimos/msgs/nav_msgs/test_Path.py index e7156d48c0..9294581f7f 100644 --- a/dimos/msgs/nav_msgs/test_Path.py +++ b/dimos/msgs/nav_msgs/test_Path.py @@ -15,7 +15,14 @@ import time import pytest -from nav_msgs.msg import Path as ROSPath + + +try: + from nav_msgs.msg import Path as ROSPath + from geometry_msgs.msg import PoseStamped as ROSPoseStamped +except ImportError: + ROSPoseStamped = None + ROSPath = None from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -31,8 +38,13 @@ def create_test_pose(x: float, y: float, z: float, frame_id: str = "map") -> Pos ) +@pytest.mark.ros def test_init_empty(): """Test creating an empty path.""" + if ROSPath is None: + pytest.skip("ROS not available") + if ROSPoseStamped is None: + pytest.skip("ROS not available") path = Path(frame_id="map") assert path.frame_id == "map" assert len(path) == 0 @@ -40,6 +52,7 @@ def test_init_empty(): assert path.poses == [] +@pytest.mark.ros def test_init_with_poses(): """Test creating a path with initial poses.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -49,6 +62,7 @@ def test_init_with_poses(): assert path.poses == poses +@pytest.mark.ros def test_head(): """Test getting the first pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -60,6 +74,7 @@ def test_head(): assert empty_path.head() is None +@pytest.mark.ros def test_last(): """Test getting the last pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -71,6 +86,7 @@ def test_last(): assert empty_path.last() is None +@pytest.mark.ros def test_tail(): """Test getting all poses except the first.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -89,6 +105,7 @@ def test_tail(): assert len(empty_path.tail()) == 0 +@pytest.mark.ros def test_push_immutable(): """Test immutable push operation.""" path = Path(frame_id="map") @@ -108,6 +125,7 @@ def test_push_immutable(): assert path3.poses == [pose1, pose2] +@pytest.mark.ros def test_push_mutable(): """Test mutable push operation.""" path = Path(frame_id="map") @@ -124,6 +142,7 @@ def test_push_mutable(): assert path.poses == [pose1, pose2] +@pytest.mark.ros def test_indexing(): """Test indexing and slicing.""" poses = [create_test_pose(i, i, 0) for i in range(5)] @@ -139,6 +158,7 @@ def test_indexing(): assert path[3:] == poses[3:] +@pytest.mark.ros def test_iteration(): """Test iterating over poses.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -150,6 +170,7 @@ def test_iteration(): assert collected == poses +@pytest.mark.ros def test_slice_method(): """Test slice method.""" poses = [create_test_pose(i, i, 0) for i in range(5)] @@ -165,6 +186,7 @@ def test_slice_method(): assert sliced2.poses == poses[2:] +@pytest.mark.ros def test_extend_immutable(): """Test immutable extend operation.""" poses1 = [create_test_pose(i, i, 0) for i in range(2)] @@ -180,6 +202,7 @@ def test_extend_immutable(): assert extended.frame_id == "map" # Keeps first path's frame +@pytest.mark.ros def test_extend_mutable(): """Test mutable extend operation.""" poses1 = [create_test_pose(i, i, 0) for i in range(2)] @@ -197,6 +220,7 @@ def test_extend_mutable(): assert p1.z == p2.z +@pytest.mark.ros def test_reverse(): """Test reverse operation.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -207,6 +231,7 @@ def test_reverse(): assert reversed_path.poses == list(reversed(poses)) +@pytest.mark.ros def test_clear(): """Test clear operation.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -217,6 +242,7 @@ def test_clear(): assert path.poses == [] +@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Path to/from binary LCM format.""" # Create path with poses @@ -269,6 +295,7 @@ def test_lcm_encode_decode(): assert decoded.orientation.w == orig.orientation.w +@pytest.mark.ros def test_lcm_encode_decode_empty(): """Test encoding and decoding of empty Path.""" path_source = Path(frame_id="base_link") @@ -281,6 +308,7 @@ def test_lcm_encode_decode_empty(): assert len(path_dest.poses) == 0 +@pytest.mark.ros def test_str_representation(): """Test string representation.""" path = Path(frame_id="map") @@ -291,6 +319,7 @@ def test_str_representation(): assert str(path) == "Path(frame_id='map', poses=2)" +@pytest.mark.ros def test_path_from_ros_msg(): """Test creating a Path from a ROS Path message.""" ros_msg = ROSPath() @@ -300,8 +329,6 @@ def test_path_from_ros_msg(): # Add some poses for i in range(3): - from geometry_msgs.msg import PoseStamped as ROSPoseStamped - ros_pose = ROSPoseStamped() ros_pose.header.frame_id = "map" ros_pose.header.stamp.sec = 123 + i @@ -328,6 +355,7 @@ def test_path_from_ros_msg(): assert pose.orientation.w == 1.0 +@pytest.mark.ros def test_path_to_ros_msg(): """Test converting a Path to a ROS Path message.""" poses = [ @@ -354,6 +382,7 @@ def test_path_to_ros_msg(): assert ros_pose.pose.orientation.w == 1.0 +@pytest.mark.ros def test_path_ros_roundtrip(): """Test round-trip conversion between Path and ROS Path.""" poses = [ diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index e29cb2e95e..6d18e60160 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -13,23 +13,37 @@ # See the License for the specific language governing permissions and # limitations under the License. +import pytest import numpy as np -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -# Try to import ROS types for testing try: from sensor_msgs.msg import CameraInfo as ROSCameraInfo from sensor_msgs.msg import RegionOfInterest as ROSRegionOfInterest from std_msgs.msg import Header as ROSHeader +except ImportError: + ROSCameraInfo = None + ROSRegionOfInterest = None + ROSHeader = None +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + +# Try to import ROS types for testing +try: ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False +@pytest.mark.ros def test_lcm_encode_decode(): """Test LCM encode/decode preserves CameraInfo data.""" + if ROSHeader is None: + pytest.skip("ROS not available") + if ROSRegionOfInterest is None: + pytest.skip("ROS not available") + if ROSCameraInfo is None: + pytest.skip("ROS not available") print("Testing CameraInfo LCM encode/decode...") # Create test camera info with sample calibration data @@ -149,6 +163,7 @@ def test_lcm_encode_decode(): print("✓ LCM encode/decode test passed - all properties preserved!") +@pytest.mark.ros def test_numpy_matrix_operations(): """Test numpy matrix getter/setter operations.""" print("\nTesting numpy matrix operations...") @@ -186,6 +201,7 @@ def test_numpy_matrix_operations(): print("✓ All numpy matrix operations passed!") +@pytest.mark.ros def test_ros_conversion(): """Test ROS message conversion preserves CameraInfo data.""" if not ROS_AVAILABLE: @@ -358,6 +374,7 @@ def test_ros_conversion(): print("\n✓ All ROS conversion tests passed!") +@pytest.mark.ros def test_equality(): """Test CameraInfo equality comparison.""" print("\nTesting CameraInfo equality...") diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index 776100ec2e..d44b43a581 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -13,26 +13,40 @@ # See the License for the specific language governing permissions and # limitations under the License. +import pytest import numpy as np import struct -from dimos.msgs.sensor_msgs import PointCloud2 -from dimos.robot.unitree_webrtc.type.lidar import LidarMessage -from dimos.utils.testing import SensorReplay -# Try to import ROS types for testing try: from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 from sensor_msgs.msg import PointField as ROSPointField from std_msgs.msg import Header as ROSHeader +except ImportError: + ROSPointCloud2 = None + ROSPointField = None + ROSHeader = None +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.robot.unitree_webrtc.type.lidar import LidarMessage +from dimos.utils.testing import SensorReplay + +# Try to import ROS types for testing +try: ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False +@pytest.mark.ros def test_lcm_encode_decode(): """Test LCM encode/decode preserves pointcloud data.""" + if ROSHeader is None: + pytest.skip("ROS not available") + if ROSPointField is None: + pytest.skip("ROS not available") + if ROSPointCloud2 is None: + pytest.skip("ROS not available") replay = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) lidar_msg: LidarMessage = replay.load_one("lidar_data_021") @@ -92,6 +106,7 @@ def test_lcm_encode_decode(): print("✓ LCM encode/decode test passed - all properties preserved!") +@pytest.mark.ros def test_ros_conversion(): """Test ROS message conversion preserves pointcloud data.""" if not ROS_AVAILABLE: diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index 4bb5b2f3b0..ba18805468 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -13,15 +13,27 @@ # limitations under the License. import pytest + +try: + from tf2_msgs.msg import TFMessage as ROSTFMessage + from geometry_msgs.msg import TransformStamped as ROSTransformStamped +except ImportError: + ROSTransformStamped = None + ROSTFMessage = None + from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage -from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.tf2_msgs import TFMessage +@pytest.mark.ros def test_tfmessage_initialization(): """Test TFMessage initialization with Transform objects.""" + if ROSTFMessage is None: + pytest.skip("ROS not available") + if ROSTransformStamped is None: + pytest.skip("ROS not available") # Create some transforms tf1 = Transform( translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1), frame_id="world", ts=100.0 @@ -45,6 +57,7 @@ def test_tfmessage_initialization(): assert transforms == [tf1, tf2] +@pytest.mark.ros def test_tfmessage_empty(): """Test empty TFMessage.""" msg = TFMessage() @@ -52,6 +65,7 @@ def test_tfmessage_empty(): assert list(msg) == [] +@pytest.mark.ros def test_tfmessage_add_transform(): """Test adding transforms to TFMessage.""" msg = TFMessage() @@ -63,6 +77,7 @@ def test_tfmessage_add_transform(): assert msg[0] == tf +@pytest.mark.ros def test_tfmessage_lcm_encode_decode(): """Test encoding TFMessage to LCM bytes.""" # Create transforms @@ -110,9 +125,9 @@ def test_tfmessage_lcm_encode_decode(): assert ts2.transform.rotation.w == 0.707 +@pytest.mark.ros def test_tfmessage_from_ros_msg(): """Test creating a TFMessage from a ROS TFMessage message.""" - from geometry_msgs.msg import TransformStamped as ROSTransformStamped ros_msg = ROSTFMessage() @@ -171,6 +186,7 @@ def test_tfmessage_from_ros_msg(): assert tfmsg[1].rotation.w == 0.707 +@pytest.mark.ros def test_tfmessage_to_ros_msg(): """Test converting a TFMessage to a ROS TFMessage message.""" # Create transforms @@ -221,6 +237,7 @@ def test_tfmessage_to_ros_msg(): assert ros_msg.transforms[1].transform.rotation.w == 0.9 +@pytest.mark.ros def test_tfmessage_ros_roundtrip(): """Test round-trip conversion between TFMessage and ROS TFMessage.""" # Create transforms with various properties diff --git a/dimos/robot/test_ros_bridge.py b/dimos/robot/test_ros_bridge.py index 537a17b904..9f83913768 100644 --- a/dimos/robot/test_ros_bridge.py +++ b/dimos/robot/test_ros_bridge.py @@ -17,13 +17,24 @@ import unittest import numpy as np -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped as ROSTwistStamped -from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 -from sensor_msgs.msg import PointField -from tf2_msgs.msg import TFMessage as ROSTFMessage -from geometry_msgs.msg import TransformStamped +import pytest + +try: + import rclpy + from rclpy.node import Node + from geometry_msgs.msg import TwistStamped as ROSTwistStamped + from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 + from sensor_msgs.msg import PointField + from tf2_msgs.msg import TFMessage as ROSTFMessage + from geometry_msgs.msg import TransformStamped +except ImportError: + rclpy = None + Node = None + ROSTwistStamped = None + ROSPointCloud2 = None + PointField = None + ROSTFMessage = None + TransformStamped = None from dimos.protocol.pubsub.lcmpubsub import LCM, Topic from dimos.msgs.geometry_msgs import TwistStamped @@ -32,11 +43,16 @@ from dimos.robot.ros_bridge import ROSBridge, BridgeDirection +@pytest.mark.ros class TestROSBridge(unittest.TestCase): """Test suite for ROS-DIMOS bridge.""" def setUp(self): """Set up test fixtures.""" + # Skip if ROS is not available + if rclpy is None: + self.skipTest("ROS not available") + # Initialize ROS if not already done if not rclpy.ok(): rclpy.init() From 7e63a6b3aff22d6ef8ee4c65b49b8ac7272fd0e8 Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 17 Sep 2025 20:11:25 -0700 Subject: [PATCH 18/21] Wrap ROS types in try except - TODO: remove ros types --- dimos/msgs/geometry_msgs/Pose.py | 13 ++++++++++--- dimos/msgs/geometry_msgs/PoseStamped.py | 7 ++++++- dimos/msgs/geometry_msgs/PoseWithCovariance.py | 8 +++++++- .../geometry_msgs/PoseWithCovarianceStamped.py | 8 +++++++- dimos/msgs/geometry_msgs/Transform.py | 17 +++++++++++++---- dimos/msgs/geometry_msgs/Twist.py | 11 +++++++++-- dimos/msgs/geometry_msgs/TwistStamped.py | 8 +++++++- dimos/msgs/geometry_msgs/TwistWithCovariance.py | 8 +++++++- .../geometry_msgs/TwistWithCovarianceStamped.py | 8 +++++++- dimos/msgs/nav_msgs/Odometry.py | 8 +++++++- dimos/msgs/nav_msgs/Path.py | 8 +++++++- dimos/msgs/tf2_msgs/TFMessage.py | 9 +++++++-- 12 files changed, 94 insertions(+), 19 deletions(-) diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index 55ecaeca9e..1cf6c95442 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -18,9 +18,16 @@ from dimos_lcm.geometry_msgs import Pose as LCMPose from dimos_lcm.geometry_msgs import Transform as LCMTransform -from geometry_msgs.msg import Pose as ROSPose -from geometry_msgs.msg import Point as ROSPoint -from geometry_msgs.msg import Quaternion as ROSQuaternion + +try: + from geometry_msgs.msg import Pose as ROSPose + from geometry_msgs.msg import Point as ROSPoint + from geometry_msgs.msg import Quaternion as ROSQuaternion +except ImportError: + ROSPose = None + ROSPoint = None + ROSQuaternion = None + from plum import dispatch from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py index 2927247d89..c44c9cd4ff 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -22,7 +22,12 @@ from dimos_lcm.geometry_msgs import PoseStamped as LCMPoseStamped from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime -from geometry_msgs.msg import PoseStamped as ROSPoseStamped + +try: + from geometry_msgs.msg import PoseStamped as ROSPoseStamped +except ImportError: + ROSPoseStamped = None + from plum import dispatch from dimos.msgs.geometry_msgs.Pose import Pose diff --git a/dimos/msgs/geometry_msgs/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py index e4c93cede9..3a49522653 100644 --- a/dimos/msgs/geometry_msgs/PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/PoseWithCovariance.py @@ -18,9 +18,13 @@ import numpy as np from dimos_lcm.geometry_msgs import PoseWithCovariance as LCMPoseWithCovariance -from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance from plum import dispatch +try: + from geometry_msgs.msg import PoseWithCovariance as ROSPoseWithCovariance +except ImportError: + ROSPoseWithCovariance = None + from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -200,6 +204,7 @@ def from_ros_msg(cls, ros_msg: ROSPoseWithCovariance) -> "PoseWithCovariance": Returns: PoseWithCovariance instance """ + pose = Pose.from_ros_msg(ros_msg.pose) return cls(pose, list(ros_msg.covariance)) @@ -209,6 +214,7 @@ def to_ros_msg(self) -> ROSPoseWithCovariance: Returns: ROS PoseWithCovariance message """ + ros_msg = ROSPoseWithCovariance() ros_msg.pose = self.pose.to_ros_msg() # ROS expects list, not numpy array diff --git a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py index 9f48d8e2dc..05e1847734 100644 --- a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py @@ -19,9 +19,13 @@ import numpy as np from dimos_lcm.geometry_msgs import PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped -from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped from plum import dispatch +try: + from geometry_msgs.msg import PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped +except ImportError: + ROSPoseWithCovarianceStamped = None + from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance from dimos.types.timestamped import Timestamped @@ -118,6 +122,7 @@ def from_ros_msg(cls, ros_msg: ROSPoseWithCovarianceStamped) -> "PoseWithCovaria Returns: PoseWithCovarianceStamped instance """ + # Convert timestamp from ROS header ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -137,6 +142,7 @@ def to_ros_msg(self) -> ROSPoseWithCovarianceStamped: Returns: ROS PoseWithCovarianceStamped message """ + ros_msg = ROSPoseWithCovarianceStamped() # Set header diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index 1f6121f6cf..61951d34b5 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -19,10 +19,17 @@ from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos_lcm.geometry_msgs import TransformStamped as LCMTransformStamped -from geometry_msgs.msg import TransformStamped as ROSTransformStamped -from geometry_msgs.msg import Transform as ROSTransform -from geometry_msgs.msg import Vector3 as ROSVector3 -from geometry_msgs.msg import Quaternion as ROSQuaternion + +try: + from geometry_msgs.msg import TransformStamped as ROSTransformStamped + from geometry_msgs.msg import Transform as ROSTransform + from geometry_msgs.msg import Vector3 as ROSVector3 + from geometry_msgs.msg import Quaternion as ROSQuaternion +except ImportError: + ROSTransformStamped = None + ROSTransform = None + ROSVector3 = None + ROSQuaternion = None from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -151,6 +158,7 @@ def from_ros_transform_stamped(cls, ros_msg: ROSTransformStamped) -> "Transform" Returns: Transform instance """ + # Convert timestamp ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -183,6 +191,7 @@ def to_ros_transform_stamped(self) -> ROSTransformStamped: Returns: ROS TransformStamped message """ + ros_msg = ROSTransformStamped() # Set header diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py index fe951bff09..2b7b4206a3 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -19,10 +19,15 @@ from typing import BinaryIO from dimos_lcm.geometry_msgs import Twist as LCMTwist -from geometry_msgs.msg import Twist as ROSTwist -from geometry_msgs.msg import Vector3 as ROSVector3 from plum import dispatch +try: + from geometry_msgs.msg import Twist as ROSTwist + from geometry_msgs.msg import Vector3 as ROSVector3 +except ImportError: + ROSTwist = None + ROSVector3 = None + from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike @@ -113,6 +118,7 @@ def from_ros_msg(cls, ros_msg: ROSTwist) -> "Twist": Returns: Twist instance """ + linear = Vector3(ros_msg.linear.x, ros_msg.linear.y, ros_msg.linear.z) angular = Vector3(ros_msg.angular.x, ros_msg.angular.y, ros_msg.angular.z) return cls(linear, angular) @@ -123,6 +129,7 @@ def to_ros_msg(self) -> ROSTwist: Returns: ROS Twist message """ + ros_msg = ROSTwist() ros_msg.linear = ROSVector3(x=self.linear.x, y=self.linear.y, z=self.linear.z) ros_msg.angular = ROSVector3(x=self.angular.x, y=self.angular.y, z=self.angular.z) diff --git a/dimos/msgs/geometry_msgs/TwistStamped.py b/dimos/msgs/geometry_msgs/TwistStamped.py index 0afa7e662f..5c464dfa17 100644 --- a/dimos/msgs/geometry_msgs/TwistStamped.py +++ b/dimos/msgs/geometry_msgs/TwistStamped.py @@ -22,9 +22,13 @@ from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime -from geometry_msgs.msg import TwistStamped as ROSTwistStamped from plum import dispatch +try: + from geometry_msgs.msg import TwistStamped as ROSTwistStamped +except ImportError: + ROSTwistStamped = None + from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable from dimos.types.timestamped import Timestamped @@ -84,6 +88,7 @@ def from_ros_msg(cls, ros_msg: ROSTwistStamped) -> "TwistStamped": Returns: TwistStamped instance """ + # Convert timestamp from ROS header ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -103,6 +108,7 @@ def to_ros_msg(self) -> ROSTwistStamped: Returns: ROS TwistStamped message """ + ros_msg = ROSTwistStamped() # Set header diff --git a/dimos/msgs/geometry_msgs/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py index 81be1c3874..18237cf7b9 100644 --- a/dimos/msgs/geometry_msgs/TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/TwistWithCovariance.py @@ -18,9 +18,13 @@ import numpy as np from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance -from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance from plum import dispatch +try: + from geometry_msgs.msg import TwistWithCovariance as ROSTwistWithCovariance +except ImportError: + ROSTwistWithCovariance = None + from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable @@ -200,6 +204,7 @@ def from_ros_msg(cls, ros_msg: ROSTwistWithCovariance) -> "TwistWithCovariance": Returns: TwistWithCovariance instance """ + twist = Twist.from_ros_msg(ros_msg.twist) return cls(twist, list(ros_msg.covariance)) @@ -209,6 +214,7 @@ def to_ros_msg(self) -> ROSTwistWithCovariance: Returns: ROS TwistWithCovariance message """ + ros_msg = ROSTwistWithCovariance() ros_msg.twist = self.twist.to_ros_msg() # ROS expects list, not numpy array diff --git a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py index f199eb60c9..1cc4c010a5 100644 --- a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py @@ -19,9 +19,13 @@ import numpy as np from dimos_lcm.geometry_msgs import TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped -from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped from plum import dispatch +try: + from geometry_msgs.msg import TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped +except ImportError: + ROSTwistWithCovarianceStamped = None + from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable @@ -126,6 +130,7 @@ def from_ros_msg(cls, ros_msg: ROSTwistWithCovarianceStamped) -> "TwistWithCovar Returns: TwistWithCovarianceStamped instance """ + # Convert timestamp from ROS header ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -145,6 +150,7 @@ def to_ros_msg(self) -> ROSTwistWithCovarianceStamped: Returns: ROS TwistWithCovarianceStamped message """ + ros_msg = ROSTwistWithCovarianceStamped() # Set header diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index d5c875db20..6e8b6c27fc 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -19,9 +19,13 @@ import numpy as np from dimos_lcm.nav_msgs import Odometry as LCMOdometry -from nav_msgs.msg import Odometry as ROSOdometry from plum import dispatch +try: + from nav_msgs.msg import Odometry as ROSOdometry +except ImportError: + ROSOdometry = None + from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance from dimos.msgs.geometry_msgs.Twist import Twist @@ -333,6 +337,7 @@ def from_ros_msg(cls, ros_msg: ROSOdometry) -> "Odometry": Returns: Odometry instance """ + # Convert timestamp from ROS header ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -354,6 +359,7 @@ def to_ros_msg(self) -> ROSOdometry: Returns: ROS Odometry message """ + ros_msg = ROSOdometry() # Set header diff --git a/dimos/msgs/nav_msgs/Path.py b/dimos/msgs/nav_msgs/Path.py index bb0b509369..18a2fb07ee 100644 --- a/dimos/msgs/nav_msgs/Path.py +++ b/dimos/msgs/nav_msgs/Path.py @@ -26,7 +26,11 @@ from dimos_lcm.nav_msgs import Path as LCMPath from dimos_lcm.std_msgs import Header as LCMHeader from dimos_lcm.std_msgs import Time as LCMTime -from nav_msgs.msg import Path as ROSPath + +try: + from nav_msgs.msg import Path as ROSPath +except ImportError: + ROSPath = None from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -199,6 +203,7 @@ def from_ros_msg(cls, ros_msg: ROSPath) -> "Path": Returns: Path instance """ + # Convert timestamp from ROS header ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) @@ -215,6 +220,7 @@ def to_ros_msg(self) -> ROSPath: Returns: ROS Path message """ + ros_msg = ROSPath() # Set header diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index 3d61c37a16..d2bb018c34 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -34,8 +34,13 @@ 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 tf2_msgs.msg import TFMessage as ROSTFMessage -from geometry_msgs.msg import TransformStamped as ROSTransformStamped + +try: + from tf2_msgs.msg import TFMessage as ROSTFMessage + from geometry_msgs.msg import TransformStamped as ROSTransformStamped +except ImportError: + ROSTFMessage = None + ROSTransformStamped = None from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 From 5456ebc839d0431d85b6efc4d2660bad8d366de2 Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 17 Sep 2025 20:16:39 -0700 Subject: [PATCH 19/21] rclpy CI import error --- dimos/robot/ros_bridge.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/dimos/robot/ros_bridge.py b/dimos/robot/ros_bridge.py index f26d81c92c..7e845e08d0 100644 --- a/dimos/robot/ros_bridge.py +++ b/dimos/robot/ros_bridge.py @@ -18,10 +18,19 @@ from typing import Dict, Any, Type, Literal, Optional from enum import Enum -import rclpy -from rclpy.executors import SingleThreadedExecutor -from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy +try: + import rclpy + from rclpy.executors import SingleThreadedExecutor + from rclpy.node import Node + from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy +except ImportError: + rclpy = None + SingleThreadedExecutor = None + Node = None + QoSProfile = None + QoSReliabilityPolicy = None + QoSHistoryPolicy = None + QoSDurabilityPolicy = None from dimos.protocol.pubsub.lcmpubsub import LCM, Topic from dimos.utils.logging_config import setup_logger From e69362528645578d9352777089851227993fda22 Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 17 Sep 2025 21:03:17 -0700 Subject: [PATCH 20/21] Fixed broken camera info test --- dimos/msgs/sensor_msgs/test_CameraInfo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index 6d18e60160..8cc27c9bfe 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -235,7 +235,7 @@ def test_ros_conversion(): binning_x=1, binning_y=1, frame_id="left_camera_optical", - ts=9876543210.987654, + ts=1234567890.987654, ) # Set ROI From 5ece11fc193fbd5bd3c3064e8d5dadee91b2f23e Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 17 Sep 2025 22:51:28 -0700 Subject: [PATCH 21/21] Revert unneeded ros pytest marks and skips --- dimos/msgs/geometry_msgs/test_Pose.py | 43 ------------------- dimos/msgs/geometry_msgs/test_PoseStamped.py | 8 ---- .../geometry_msgs/test_PoseWithCovariance.py | 24 ----------- .../test_PoseWithCovarianceStamped.py | 12 +----- dimos/msgs/geometry_msgs/test_Transform.py | 15 ------- dimos/msgs/geometry_msgs/test_Twist.py | 18 -------- dimos/msgs/geometry_msgs/test_TwistStamped.py | 5 --- .../geometry_msgs/test_TwistWithCovariance.py | 20 --------- .../test_TwistWithCovarianceStamped.py | 14 +----- dimos/msgs/nav_msgs/test_Odometry.py | 16 +------ dimos/msgs/nav_msgs/test_Path.py | 21 --------- dimos/msgs/sensor_msgs/test_CameraInfo.py | 9 ---- dimos/msgs/sensor_msgs/test_PointCloud2.py | 7 --- dimos/msgs/tf2_msgs/test_TFMessage.py | 8 ---- 14 files changed, 3 insertions(+), 217 deletions(-) diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index 51ebf88587..6d9c10b1c2 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -32,7 +32,6 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 -@pytest.mark.ros def test_pose_default_init(): """Test that default initialization creates a pose at origin with identity orientation.""" pose = Pose() @@ -54,7 +53,6 @@ def test_pose_default_init(): assert pose.z == 0.0 -@pytest.mark.ros def test_pose_pose_init(): """Test initialization with position coordinates only (identity orientation).""" pose_data = Pose(1.0, 2.0, 3.0) @@ -78,7 +76,6 @@ def test_pose_pose_init(): assert pose.z == 3.0 -@pytest.mark.ros def test_pose_position_init(): """Test initialization with position coordinates only (identity orientation).""" pose = Pose(1.0, 2.0, 3.0) @@ -100,7 +97,6 @@ def test_pose_position_init(): assert pose.z == 3.0 -@pytest.mark.ros def test_pose_full_init(): """Test initialization with position and orientation coordinates.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -122,7 +118,6 @@ def test_pose_full_init(): assert pose.z == 3.0 -@pytest.mark.ros def test_pose_vector_position_init(): """Test initialization with Vector3 position (identity orientation).""" position = Vector3(4.0, 5.0, 6.0) @@ -140,7 +135,6 @@ def test_pose_vector_position_init(): assert pose.orientation.w == 1.0 -@pytest.mark.ros def test_pose_vector_quaternion_init(): """Test initialization with Vector3 position and Quaternion orientation.""" position = Vector3(1.0, 2.0, 3.0) @@ -159,7 +153,6 @@ def test_pose_vector_quaternion_init(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_list_init(): """Test initialization with lists for position and orientation.""" position_list = [1.0, 2.0, 3.0] @@ -178,7 +171,6 @@ def test_pose_list_init(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_tuple_init(): """Test initialization from a tuple of (position, orientation).""" position = [1.0, 2.0, 3.0] @@ -198,7 +190,6 @@ def test_pose_tuple_init(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_dict_init(): """Test initialization from a dictionary with 'position' and 'orientation' keys.""" pose_dict = {"position": [1.0, 2.0, 3.0], "orientation": [0.1, 0.2, 0.3, 0.9]} @@ -216,7 +207,6 @@ def test_pose_dict_init(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_copy_init(): """Test initialization from another Pose (copy constructor).""" original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -238,7 +228,6 @@ def test_pose_copy_init(): assert copy == original -@pytest.mark.ros def test_pose_lcm_init(): """Test initialization from an LCM Pose.""" # Create LCM pose @@ -265,7 +254,6 @@ def test_pose_lcm_init(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_properties(): """Test pose property access.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -282,7 +270,6 @@ def test_pose_properties(): assert pose.yaw == euler.z -@pytest.mark.ros def test_pose_euler_properties_identity(): """Test pose Euler angle properties with identity orientation.""" pose = Pose(1.0, 2.0, 3.0) # Identity orientation @@ -298,7 +285,6 @@ def test_pose_euler_properties_identity(): assert np.isclose(pose.orientation.euler.z, 0.0, atol=1e-10) -@pytest.mark.ros def test_pose_repr(): """Test pose string representation.""" pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) @@ -315,7 +301,6 @@ def test_pose_repr(): assert "2.567" in repr_str or "2.57" in repr_str -@pytest.mark.ros def test_pose_str(): """Test pose string formatting.""" pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) @@ -334,7 +319,6 @@ def test_pose_str(): assert str_repr.count("Pose") == 1 -@pytest.mark.ros def test_pose_equality(): """Test pose equality comparison.""" pose1 = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -357,7 +341,6 @@ def test_pose_equality(): assert pose1 != None -@pytest.mark.ros def test_pose_with_numpy_arrays(): """Test pose initialization with numpy arrays.""" position_array = np.array([1.0, 2.0, 3.0]) @@ -377,7 +360,6 @@ def test_pose_with_numpy_arrays(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_with_mixed_types(): """Test pose initialization with mixed input types.""" # Position as tuple, orientation as list @@ -398,7 +380,6 @@ def test_pose_with_mixed_types(): assert pose1.orientation.w == pose2.orientation.w -@pytest.mark.ros def test_to_pose_passthrough(): """Test to_pose function with Pose input (passthrough).""" original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -408,7 +389,6 @@ def test_to_pose_passthrough(): assert result is original -@pytest.mark.ros def test_to_pose_conversion(): """Test to_pose function with convertible inputs.""" # Note: The to_pose conversion function has type checking issues in the current implementation @@ -441,7 +421,6 @@ def test_to_pose_conversion(): assert result2.orientation.w == 0.9 -@pytest.mark.ros def test_pose_euler_roundtrip(): """Test conversion from Euler angles to quaternion and back.""" # Start with known Euler angles (small angles to avoid gimbal lock) @@ -465,7 +444,6 @@ def test_pose_euler_roundtrip(): assert np.isclose(result_euler.z, yaw, atol=1e-6) -@pytest.mark.ros def test_pose_zero_position(): """Test pose with zero position vector.""" # Use manual construction since Vector3.zeros has signature issues @@ -479,7 +457,6 @@ def test_pose_zero_position(): assert np.isclose(pose.yaw, 0.0, atol=1e-10) -@pytest.mark.ros def test_pose_unit_vectors(): """Test pose with unit vector positions.""" # Test unit x vector position @@ -501,7 +478,6 @@ def test_pose_unit_vectors(): assert pose_z.z == 1.0 -@pytest.mark.ros def test_pose_negative_coordinates(): """Test pose with negative coordinates.""" pose = Pose(-1.0, -2.0, -3.0, -0.1, -0.2, -0.3, 0.9) @@ -518,7 +494,6 @@ def test_pose_negative_coordinates(): assert pose.orientation.w == 0.9 -@pytest.mark.ros def test_pose_large_coordinates(): """Test pose with large coordinate values.""" large_value = 1000.0 @@ -539,7 +514,6 @@ def test_pose_large_coordinates(): "x,y,z", [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (0.5, -0.5, 1.5), (100.0, -100.0, 0.0)], ) -@pytest.mark.ros def test_pose_parametrized_positions(x, y, z): """Parametrized test for various position values.""" pose = Pose(x, y, z) @@ -565,7 +539,6 @@ def test_pose_parametrized_positions(x, y, z): (0.5, 0.5, 0.5, 0.5), # Equal components ], ) -@pytest.mark.ros def test_pose_parametrized_orientations(qx, qy, qz, qw): """Parametrized test for various orientation values.""" pose = Pose(0.0, 0.0, 0.0, qx, qy, qz, qw) @@ -582,7 +555,6 @@ def test_pose_parametrized_orientations(qx, qy, qz, qw): assert pose.orientation.w == qw -@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -602,7 +574,6 @@ def encodepass(): print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") -@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -619,7 +590,6 @@ def encodepass(): print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") -@pytest.mark.ros def test_pose_addition_translation_only(): """Test pose addition with translation only (identity rotations).""" # Two poses with only translations @@ -640,7 +610,6 @@ def test_pose_addition_translation_only(): assert result.orientation.w == 1.0 -@pytest.mark.ros def test_pose_addition_with_rotation(): """Test pose addition with rotation applied to translation.""" # First pose: at origin, rotated 90 degrees around Z (yaw) @@ -666,7 +635,6 @@ def test_pose_addition_with_rotation(): assert np.isclose(result.orientation.w, np.cos(angle / 2), atol=1e-10) -@pytest.mark.ros def test_pose_addition_rotation_composition(): """Test that rotations are properly composed.""" # First pose: 45 degrees around Z @@ -689,7 +657,6 @@ def test_pose_addition_rotation_composition(): assert np.isclose(result.orientation.w, expected_qw, atol=1e-10) -@pytest.mark.ros def test_pose_addition_full_transform(): """Test full pose composition with translation and rotation.""" # Robot pose: at (2, 1, 0), facing 90 degrees left (positive yaw) @@ -715,7 +682,6 @@ def test_pose_addition_full_transform(): assert np.isclose(object_in_world.yaw, robot_yaw, atol=1e-10) -@pytest.mark.ros def test_pose_addition_chain(): """Test chaining multiple pose additions.""" # Create a chain of transformations @@ -732,7 +698,6 @@ def test_pose_addition_chain(): assert result.position.z == 1.0 -@pytest.mark.ros def test_pose_addition_with_convertible(): """Test pose addition with convertible types.""" pose1 = Pose(1.0, 2.0, 3.0) @@ -752,7 +717,6 @@ def test_pose_addition_with_convertible(): assert result2.position.z == 3.0 -@pytest.mark.ros def test_pose_identity_addition(): """Test that adding identity pose leaves pose unchanged.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -770,7 +734,6 @@ def test_pose_identity_addition(): assert result.orientation.w == pose.orientation.w -@pytest.mark.ros def test_pose_addition_3d_rotation(): """Test pose addition with 3D rotations.""" # First pose: rotated around X axis (roll) @@ -798,8 +761,6 @@ def test_pose_addition_3d_rotation(): @pytest.mark.ros def test_pose_from_ros_msg(): """Test creating a Pose from a ROS Pose message.""" - if ROSPose is None: - pytest.skip("ROS not available") ros_msg = ROSPose() ros_msg.position = ROSPoint(x=1.0, y=2.0, z=3.0) ros_msg.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) @@ -818,8 +779,6 @@ def test_pose_from_ros_msg(): @pytest.mark.ros def test_pose_to_ros_msg(): """Test converting a Pose to a ROS Pose message.""" - if ROSPose is None: - pytest.skip("ROS not available") pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) ros_msg = pose.to_ros_msg() @@ -837,8 +796,6 @@ def test_pose_to_ros_msg(): @pytest.mark.ros def test_pose_ros_roundtrip(): """Test round-trip conversion between Pose and ROS Pose.""" - if ROSPose is None: - pytest.skip("ROS not available") original = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) ros_msg = original.to_ros_msg() diff --git a/dimos/msgs/geometry_msgs/test_PoseStamped.py b/dimos/msgs/geometry_msgs/test_PoseStamped.py index e689fc6851..cbc0c26876 100644 --- a/dimos/msgs/geometry_msgs/test_PoseStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -25,7 +25,6 @@ from dimos.msgs.geometry_msgs import PoseStamped -@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Pose to/from binary LCM format.""" @@ -48,7 +47,6 @@ def test_lcm_encode_decode(): assert pose_dest == pose_source -@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of PoseStamped to/from binary LCM format.""" @@ -67,8 +65,6 @@ def test_pickle_encode_decode(): @pytest.mark.ros def test_pose_stamped_from_ros_msg(): """Test creating a PoseStamped from a ROS PoseStamped message.""" - if ROSPoseStamped is None: - pytest.skip("ROS not available") ros_msg = ROSPoseStamped() ros_msg.header.frame_id = "world" ros_msg.header.stamp.sec = 123 @@ -97,8 +93,6 @@ def test_pose_stamped_from_ros_msg(): @pytest.mark.ros def test_pose_stamped_to_ros_msg(): """Test converting a PoseStamped to a ROS PoseStamped message.""" - if ROSPoseStamped is None: - pytest.skip("ROS not available") pose_stamped = PoseStamped( ts=123.456, frame_id="base_link", @@ -124,8 +118,6 @@ def test_pose_stamped_to_ros_msg(): @pytest.mark.ros def test_pose_stamped_ros_roundtrip(): """Test round-trip conversion between PoseStamped and ROS PoseStamped.""" - if ROSPoseStamped is None: - pytest.skip("ROS not available") original = PoseStamped( ts=123.789, frame_id="odom", diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py index 32f19d27ff..dd254104a5 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py @@ -33,7 +33,6 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 -@pytest.mark.ros def test_pose_with_covariance_default_init(): """Test that default initialization creates a pose at origin with zero covariance.""" pose_cov = PoseWithCovariance() @@ -52,7 +51,6 @@ def test_pose_with_covariance_default_init(): assert pose_cov.covariance.shape == (36,) -@pytest.mark.ros def test_pose_with_covariance_pose_init(): """Test initialization with a Pose object.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -71,7 +69,6 @@ def test_pose_with_covariance_pose_init(): assert np.all(pose_cov.covariance == 0.0) -@pytest.mark.ros def test_pose_with_covariance_pose_and_covariance_init(): """Test initialization with pose and covariance.""" pose = Pose(1.0, 2.0, 3.0) @@ -87,7 +84,6 @@ def test_pose_with_covariance_pose_and_covariance_init(): assert np.array_equal(pose_cov.covariance, covariance) -@pytest.mark.ros def test_pose_with_covariance_list_covariance(): """Test initialization with covariance as a list.""" pose = Pose(1.0, 2.0, 3.0) @@ -99,7 +95,6 @@ def test_pose_with_covariance_list_covariance(): assert np.array_equal(pose_cov.covariance, np.array(covariance_list)) -@pytest.mark.ros def test_pose_with_covariance_copy_init(): """Test copy constructor.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -118,7 +113,6 @@ def test_pose_with_covariance_copy_init(): assert copy.covariance[0] != 999.0 -@pytest.mark.ros def test_pose_with_covariance_lcm_init(): """Test initialization from LCM message.""" lcm_msg = LCMPoseWithCovariance() @@ -146,7 +140,6 @@ def test_pose_with_covariance_lcm_init(): assert np.array_equal(pose_cov.covariance, np.arange(36)) -@pytest.mark.ros def test_pose_with_covariance_dict_init(): """Test initialization from dictionary.""" pose_dict = {"pose": Pose(1.0, 2.0, 3.0), "covariance": list(range(36))} @@ -158,7 +151,6 @@ def test_pose_with_covariance_dict_init(): assert np.array_equal(pose_cov.covariance, np.arange(36)) -@pytest.mark.ros def test_pose_with_covariance_dict_init_no_covariance(): """Test initialization from dictionary without covariance.""" pose_dict = {"pose": Pose(1.0, 2.0, 3.0)} @@ -168,7 +160,6 @@ def test_pose_with_covariance_dict_init_no_covariance(): assert np.all(pose_cov.covariance == 0.0) -@pytest.mark.ros def test_pose_with_covariance_tuple_init(): """Test initialization from tuple.""" pose = Pose(1.0, 2.0, 3.0) @@ -182,7 +173,6 @@ def test_pose_with_covariance_tuple_init(): assert np.array_equal(pose_cov.covariance, covariance) -@pytest.mark.ros def test_pose_with_covariance_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -208,7 +198,6 @@ def test_pose_with_covariance_properties(): assert pose_cov.yaw == pose.yaw -@pytest.mark.ros def test_pose_with_covariance_matrix_property(): """Test covariance matrix property.""" pose = Pose() @@ -227,7 +216,6 @@ def test_pose_with_covariance_matrix_property(): assert np.array_equal(pose_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) -@pytest.mark.ros def test_pose_with_covariance_repr(): """Test string representation.""" pose = Pose(1.234, 2.567, 3.891) @@ -240,7 +228,6 @@ def test_pose_with_covariance_repr(): assert "36 elements" in repr_str -@pytest.mark.ros def test_pose_with_covariance_str(): """Test string formatting.""" pose = Pose(1.234, 2.567, 3.891) @@ -256,7 +243,6 @@ def test_pose_with_covariance_str(): assert "6.000" in str_repr # Trace of identity matrix is 6 -@pytest.mark.ros def test_pose_with_covariance_equality(): """Test equality comparison.""" pose1 = Pose(1.0, 2.0, 3.0) @@ -285,7 +271,6 @@ def test_pose_with_covariance_equality(): assert pose_cov1 != None -@pytest.mark.ros def test_pose_with_covariance_lcm_encode_decode(): """Test LCM encoding and decoding.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -306,8 +291,6 @@ def test_pose_with_covariance_lcm_encode_decode(): @pytest.mark.ros def test_pose_with_covariance_from_ros_msg(): """Test creating from ROS message.""" - if ROSPoseWithCovariance is None: - pytest.skip("ROS not available") ros_msg = ROSPoseWithCovariance() ros_msg.pose.position = ROSPoint(x=1.0, y=2.0, z=3.0) ros_msg.pose.orientation = ROSQuaternion(x=0.1, y=0.2, z=0.3, w=0.9) @@ -328,8 +311,6 @@ def test_pose_with_covariance_from_ros_msg(): @pytest.mark.ros def test_pose_with_covariance_to_ros_msg(): """Test converting to ROS message.""" - if ROSPoseWithCovariance is None: - pytest.skip("ROS not available") pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) covariance = np.arange(36, dtype=float) pose_cov = PoseWithCovariance(pose, covariance) @@ -350,8 +331,6 @@ def test_pose_with_covariance_to_ros_msg(): @pytest.mark.ros def test_pose_with_covariance_ros_roundtrip(): """Test round-trip conversion with ROS messages.""" - if ROSPoseWithCovariance is None: - pytest.skip("ROS not available") pose = Pose(1.5, 2.5, 3.5, 0.15, 0.25, 0.35, 0.85) covariance = np.random.rand(36) original = PoseWithCovariance(pose, covariance) @@ -362,7 +341,6 @@ def test_pose_with_covariance_ros_roundtrip(): assert restored == original -@pytest.mark.ros def test_pose_with_covariance_zero_covariance(): """Test with zero covariance matrix.""" pose = Pose(1.0, 2.0, 3.0) @@ -372,7 +350,6 @@ def test_pose_with_covariance_zero_covariance(): assert np.trace(pose_cov.covariance_matrix) == 0.0 -@pytest.mark.ros def test_pose_with_covariance_diagonal_covariance(): """Test with diagonal covariance matrix.""" pose = Pose() @@ -401,7 +378,6 @@ def test_pose_with_covariance_diagonal_covariance(): "x,y,z", [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (100.0, -100.0, 0.0)], ) -@pytest.mark.ros def test_pose_with_covariance_parametrized_positions(x, y, z): """Parametrized test for various position values.""" pose = Pose(x, y, z) diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py index af921ef67e..139279add3 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py @@ -45,7 +45,6 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 -@pytest.mark.ros def test_pose_with_covariance_stamped_default_init(): """Test default initialization.""" if ROSPoseWithCovariance is None: @@ -78,7 +77,6 @@ def test_pose_with_covariance_stamped_default_init(): assert np.all(pose_cov_stamped.covariance == 0.0) -@pytest.mark.ros def test_pose_with_covariance_stamped_with_timestamp(): """Test initialization with specific timestamp.""" ts = 1234567890.123456 @@ -89,7 +87,6 @@ def test_pose_with_covariance_stamped_with_timestamp(): assert pose_cov_stamped.frame_id == frame_id -@pytest.mark.ros def test_pose_with_covariance_stamped_with_pose(): """Test initialization with pose.""" ts = 1234567890.123456 @@ -109,7 +106,6 @@ def test_pose_with_covariance_stamped_with_pose(): assert np.array_equal(pose_cov_stamped.covariance, covariance) -@pytest.mark.ros def test_pose_with_covariance_stamped_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -140,7 +136,6 @@ def test_pose_with_covariance_stamped_properties(): assert np.trace(cov_matrix) == 6.0 -@pytest.mark.ros def test_pose_with_covariance_stamped_str(): """Test string representation.""" pose = Pose(1.234, 2.567, 3.891) @@ -158,7 +153,6 @@ def test_pose_with_covariance_stamped_str(): assert "12.000" in str_repr # Trace of 2*identity is 12 -@pytest.mark.ros def test_pose_with_covariance_stamped_lcm_encode_decode(): """Test LCM encoding and decoding.""" ts = 1234567890.123456 @@ -281,7 +275,6 @@ def test_pose_with_covariance_stamped_ros_roundtrip(): assert np.allclose(restored.covariance, original.covariance) -@pytest.mark.ros def test_pose_with_covariance_stamped_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" pose_cov_stamped = PoseWithCovarianceStamped(ts=0.0) @@ -291,7 +284,6 @@ def test_pose_with_covariance_stamped_zero_timestamp(): assert pose_cov_stamped.ts <= time.time() -@pytest.mark.ros def test_pose_with_covariance_stamped_inheritance(): """Test that it properly inherits from PoseWithCovariance and Timestamped.""" pose = Pose(1.0, 2.0, 3.0) @@ -312,7 +304,6 @@ def test_pose_with_covariance_stamped_inheritance(): assert hasattr(pose_cov_stamped, "covariance") -@pytest.mark.ros def test_pose_with_covariance_stamped_sec_nsec(): """Test the sec_nsec helper function.""" from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import sec_nsec @@ -342,11 +333,11 @@ def test_pose_with_covariance_stamped_sec_nsec(): assert ns == 0 +@pytest.mark.ros @pytest.mark.parametrize( "frame_id", ["", "map", "odom", "base_link", "camera_optical_frame", "sensor/lidar/front"], ) -@pytest.mark.ros def test_pose_with_covariance_stamped_frame_ids(frame_id): """Test various frame ID values.""" pose_cov_stamped = PoseWithCovarianceStamped(frame_id=frame_id) @@ -360,7 +351,6 @@ def test_pose_with_covariance_stamped_frame_ids(frame_id): assert restored.frame_id == frame_id -@pytest.mark.ros def test_pose_with_covariance_stamped_different_covariances(): """Test with different covariance patterns.""" pose = Pose(1.0, 2.0, 3.0) diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index ca30d6cf46..f09f0c2966 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -29,7 +29,6 @@ from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 -@pytest.mark.ros def test_transform_initialization(): # Test default initialization (identity transform) tf = Transform() @@ -76,7 +75,6 @@ def test_transform_initialization(): assert tf9.rotation == Quaternion(0, 0, 1, 0) -@pytest.mark.ros def test_transform_identity(): # Test identity class method tf = Transform.identity() @@ -90,7 +88,6 @@ def test_transform_identity(): assert tf == Transform() -@pytest.mark.ros def test_transform_equality(): 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)) @@ -105,7 +102,6 @@ def test_transform_equality(): assert tf1 != "not a transform" -@pytest.mark.ros def test_transform_string_representations(): tf = Transform( translation=Vector3(1.5, -2.0, 3.14), rotation=Quaternion(0, 0, 0.707107, 0.707107) @@ -125,7 +121,6 @@ def test_transform_string_representations(): assert "Rotation:" in str_str -@pytest.mark.ros def test_pose_add_transform(): initial_pose = Pose(1.0, 0.0, 0.0) @@ -173,7 +168,6 @@ def test_pose_add_transform(): print(found_tf.rotation, found_tf.translation) -@pytest.mark.ros def test_pose_add_transform_with_rotation(): # Create a pose at (0, 0, 0) rotated 90 degrees around Z angle = np.pi / 2 @@ -236,7 +230,6 @@ def test_pose_add_transform_with_rotation(): assert np.isclose(transformed_pose2.orientation.w, np.cos(total_angle2 / 2), atol=1e-10) -@pytest.mark.ros def test_lcm_encode_decode(): angle = np.pi / 2 transform = Transform( @@ -251,7 +244,6 @@ def test_lcm_encode_decode(): assert decoded_transform == transform -@pytest.mark.ros def test_transform_addition(): # Test 1: Simple translation addition (no rotation) t1 = Transform( @@ -328,11 +320,8 @@ def test_transform_addition(): t1 + "not a transform" -@pytest.mark.ros def test_transform_from_pose(): """Test converting Pose to Transform""" - if ROSTransformStamped is None: - pytest.skip("ROS not available") # Create a Pose with position and orientation pose = Pose( position=Vector3(1.0, 2.0, 3.0), @@ -351,7 +340,6 @@ def test_transform_from_pose(): # validating results from example @ # https://foxglove.dev/blog/understanding-ros-transforms -@pytest.mark.ros def test_transform_from_ros(): """Test converting PoseStamped to Transform""" test_time = time.time() @@ -382,7 +370,6 @@ def test_transform_from_ros(): assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) -@pytest.mark.ros def test_transform_from_pose_stamped(): """Test converting PoseStamped to Transform""" # Create a PoseStamped with position, orientation, timestamp and frame @@ -405,7 +392,6 @@ def test_transform_from_pose_stamped(): assert transform.child_frame_id == "robot_base" # passed as first argument -@pytest.mark.ros def test_transform_from_pose_variants(): """Test from_pose with different Pose initialization methods""" # Test with Pose created from x,y,z @@ -431,7 +417,6 @@ def test_transform_from_pose_variants(): assert transform3.translation.z == 12.0 -@pytest.mark.ros def test_transform_from_pose_invalid_type(): """Test that from_pose raises TypeError for invalid types""" with pytest.raises(TypeError): diff --git a/dimos/msgs/geometry_msgs/test_Twist.py b/dimos/msgs/geometry_msgs/test_Twist.py index 3636a20b17..5f463d0bac 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -27,7 +27,6 @@ from dimos.msgs.geometry_msgs import Quaternion, Twist, Vector3 -@pytest.mark.ros def test_twist_initialization(): # Test default initialization (zero twist) tw = Twist() @@ -105,7 +104,6 @@ def test_twist_initialization(): assert tw11.angular.is_zero() # Identity quaternion -> zero euler angles -@pytest.mark.ros def test_twist_zero(): # Test zero class method tw = Twist.zero() @@ -117,7 +115,6 @@ def test_twist_zero(): assert tw == Twist() -@pytest.mark.ros def test_twist_equality(): tw1 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) tw2 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) @@ -130,7 +127,6 @@ def test_twist_equality(): assert tw1 != "not a twist" -@pytest.mark.ros def test_twist_string_representations(): tw = Twist(Vector3(1.5, -2.0, 3.14), Vector3(0.1, -0.2, 0.3)) @@ -149,7 +145,6 @@ def test_twist_string_representations(): assert "Angular:" in str_str -@pytest.mark.ros def test_twist_is_zero(): # Test zero twist tw1 = Twist() @@ -168,7 +163,6 @@ def test_twist_is_zero(): assert not tw4.is_zero() -@pytest.mark.ros def test_twist_bool(): # Test zero twist is False tw1 = Twist() @@ -185,7 +179,6 @@ def test_twist_bool(): assert tw4 -@pytest.mark.ros def test_twist_lcm_encoding(): # Test encoding and decoding tw = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.1, 0.2, 0.3)) @@ -203,7 +196,6 @@ def test_twist_lcm_encoding(): assert decoded == tw -@pytest.mark.ros def test_twist_with_lists(): # Test initialization with lists instead of Vector3 tw1 = Twist(linear=[1, 2, 3], angular=[0.1, 0.2, 0.3]) @@ -219,10 +211,6 @@ def test_twist_with_lists(): @pytest.mark.ros def test_twist_from_ros_msg(): """Test Twist.from_ros_msg conversion.""" - if ROSVector3 is None: - pytest.skip("ROS not available") - if ROSTwist is None: - pytest.skip("ROS not available") # Create ROS message ros_msg = ROSTwist() ros_msg.linear = ROSVector3(x=10.0, y=20.0, z=30.0) @@ -243,10 +231,6 @@ def test_twist_from_ros_msg(): @pytest.mark.ros def test_twist_to_ros_msg(): """Test Twist.to_ros_msg conversion.""" - if ROSVector3 is None: - pytest.skip("ROS not available") - if ROSTwist is None: - pytest.skip("ROS not available") # Create LCM message lcm_msg = Twist(linear=Vector3(40.0, 50.0, 60.0), angular=Vector3(4.0, 5.0, 6.0)) @@ -284,8 +268,6 @@ def test_ros_zero_twist_conversion(): @pytest.mark.ros def test_ros_negative_values_conversion(): """Test ROS conversion with negative values.""" - if ROSTwist is None: - pytest.skip("ROS not available") # Create ROS message with negative values ros_msg = ROSTwist() ros_msg.linear = ROSVector3(x=-1.5, y=-2.5, z=-3.5) diff --git a/dimos/msgs/geometry_msgs/test_TwistStamped.py b/dimos/msgs/geometry_msgs/test_TwistStamped.py index 326b3f5468..8414d4480a 100644 --- a/dimos/msgs/geometry_msgs/test_TwistStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistStamped.py @@ -25,12 +25,8 @@ from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of TwistStamped to/from binary LCM format.""" - if ROSTwistStamped is None: - pytest.skip("ROS not available") - twist_source = TwistStamped( ts=time.time(), linear=(1.0, 2.0, 3.0), @@ -50,7 +46,6 @@ def test_lcm_encode_decode(): assert twist_dest == twist_source -@pytest.mark.ros def test_pickle_encode_decode(): """Test encoding and decoding of TwistStamped to/from binary pickle format.""" diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py index 9bfda26689..d001482062 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py @@ -31,7 +31,6 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 -@pytest.mark.ros def test_twist_with_covariance_default_init(): """Test that default initialization creates a zero twist with zero covariance.""" if ROSVector3 is None: @@ -53,7 +52,6 @@ def test_twist_with_covariance_default_init(): assert twist_cov.covariance.shape == (36,) -@pytest.mark.ros def test_twist_with_covariance_twist_init(): """Test initialization with a Twist object.""" linear = Vector3(1.0, 2.0, 3.0) @@ -73,7 +71,6 @@ def test_twist_with_covariance_twist_init(): assert np.all(twist_cov.covariance == 0.0) -@pytest.mark.ros def test_twist_with_covariance_twist_and_covariance_init(): """Test initialization with twist and covariance.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -89,7 +86,6 @@ def test_twist_with_covariance_twist_and_covariance_init(): assert np.array_equal(twist_cov.covariance, covariance) -@pytest.mark.ros def test_twist_with_covariance_tuple_init(): """Test initialization with tuple of (linear, angular) velocities.""" linear = [1.0, 2.0, 3.0] @@ -109,7 +105,6 @@ def test_twist_with_covariance_tuple_init(): assert np.array_equal(twist_cov.covariance, covariance) -@pytest.mark.ros def test_twist_with_covariance_list_covariance(): """Test initialization with covariance as a list.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -121,7 +116,6 @@ def test_twist_with_covariance_list_covariance(): assert np.array_equal(twist_cov.covariance, np.array(covariance_list)) -@pytest.mark.ros def test_twist_with_covariance_copy_init(): """Test copy constructor.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -140,7 +134,6 @@ def test_twist_with_covariance_copy_init(): assert copy.covariance[0] != 999.0 -@pytest.mark.ros def test_twist_with_covariance_lcm_init(): """Test initialization from LCM message.""" lcm_msg = LCMTwistWithCovariance() @@ -166,7 +159,6 @@ def test_twist_with_covariance_lcm_init(): assert np.array_equal(twist_cov.covariance, np.arange(36)) -@pytest.mark.ros def test_twist_with_covariance_dict_init(): """Test initialization from dictionary.""" twist_dict = { @@ -181,7 +173,6 @@ def test_twist_with_covariance_dict_init(): assert np.array_equal(twist_cov.covariance, np.arange(36)) -@pytest.mark.ros def test_twist_with_covariance_dict_init_no_covariance(): """Test initialization from dictionary without covariance.""" twist_dict = {"twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3))} @@ -191,7 +182,6 @@ def test_twist_with_covariance_dict_init_no_covariance(): assert np.all(twist_cov.covariance == 0.0) -@pytest.mark.ros def test_twist_with_covariance_tuple_of_tuple_init(): """Test initialization from tuple of (twist_tuple, covariance).""" twist_tuple = ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]) @@ -207,7 +197,6 @@ def test_twist_with_covariance_tuple_of_tuple_init(): assert np.array_equal(twist_cov.covariance, covariance) -@pytest.mark.ros def test_twist_with_covariance_properties(): """Test convenience properties.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -222,7 +211,6 @@ def test_twist_with_covariance_properties(): assert twist_cov.angular.z == 0.3 -@pytest.mark.ros def test_twist_with_covariance_matrix_property(): """Test covariance matrix property.""" twist = Twist() @@ -241,7 +229,6 @@ def test_twist_with_covariance_matrix_property(): assert np.array_equal(twist_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) -@pytest.mark.ros def test_twist_with_covariance_repr(): """Test string representation.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) @@ -254,7 +241,6 @@ def test_twist_with_covariance_repr(): assert "36 elements" in repr_str -@pytest.mark.ros def test_twist_with_covariance_str(): """Test string formatting.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) @@ -270,7 +256,6 @@ def test_twist_with_covariance_str(): assert "6.000" in str_repr # Trace of identity matrix is 6 -@pytest.mark.ros def test_twist_with_covariance_equality(): """Test equality comparison.""" twist1 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -299,7 +284,6 @@ def test_twist_with_covariance_equality(): assert twist_cov1 != None -@pytest.mark.ros def test_twist_with_covariance_is_zero(): """Test is_zero method.""" # Zero twist @@ -314,7 +298,6 @@ def test_twist_with_covariance_is_zero(): assert twist_cov2 # Boolean conversion -@pytest.mark.ros def test_twist_with_covariance_lcm_encode_decode(): """Test LCM encoding and decoding.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -383,7 +366,6 @@ def test_twist_with_covariance_ros_roundtrip(): assert restored == original -@pytest.mark.ros def test_twist_with_covariance_zero_covariance(): """Test with zero covariance matrix.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -393,7 +375,6 @@ def test_twist_with_covariance_zero_covariance(): assert np.trace(twist_cov.covariance_matrix) == 0.0 -@pytest.mark.ros def test_twist_with_covariance_diagonal_covariance(): """Test with diagonal covariance matrix.""" twist = Twist() @@ -427,7 +408,6 @@ def test_twist_with_covariance_diagonal_covariance(): ([100.0, -100.0, 0.0], [3.14, -3.14, 0.0]), ], ) -@pytest.mark.ros def test_twist_with_covariance_parametrized_velocities(linear, angular): """Parametrized test for various velocity values.""" twist = Twist(linear, angular) diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py index adeaa760df..4174814c78 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py @@ -42,7 +42,6 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 -@pytest.mark.ros def test_twist_with_covariance_stamped_default_init(): """Test default initialization.""" if ROSVector3 is None: @@ -75,7 +74,6 @@ def test_twist_with_covariance_stamped_default_init(): assert np.all(twist_cov_stamped.covariance == 0.0) -@pytest.mark.ros def test_twist_with_covariance_stamped_with_timestamp(): """Test initialization with specific timestamp.""" ts = 1234567890.123456 @@ -86,7 +84,6 @@ def test_twist_with_covariance_stamped_with_timestamp(): assert twist_cov_stamped.frame_id == frame_id -@pytest.mark.ros def test_twist_with_covariance_stamped_with_twist(): """Test initialization with twist.""" ts = 1234567890.123456 @@ -106,7 +103,6 @@ def test_twist_with_covariance_stamped_with_twist(): assert np.array_equal(twist_cov_stamped.covariance, covariance) -@pytest.mark.ros def test_twist_with_covariance_stamped_with_tuple(): """Test initialization with tuple of velocities.""" ts = 1234567890.123456 @@ -126,7 +122,6 @@ def test_twist_with_covariance_stamped_with_tuple(): assert np.array_equal(twist_cov_stamped.covariance, covariance) -@pytest.mark.ros def test_twist_with_covariance_stamped_properties(): """Test convenience properties.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -149,7 +144,6 @@ def test_twist_with_covariance_stamped_properties(): assert np.trace(cov_matrix) == 6.0 -@pytest.mark.ros def test_twist_with_covariance_stamped_str(): """Test string representation.""" twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.111, 0.222, 0.333)) @@ -167,7 +161,6 @@ def test_twist_with_covariance_stamped_str(): assert "12.000" in str_repr # Trace of 2*identity is 12 -@pytest.mark.ros def test_twist_with_covariance_stamped_lcm_encode_decode(): """Test LCM encoding and decoding.""" ts = 1234567890.123456 @@ -290,7 +283,6 @@ def test_twist_with_covariance_stamped_ros_roundtrip(): assert np.allclose(restored.covariance, original.covariance) -@pytest.mark.ros def test_twist_with_covariance_stamped_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" twist_cov_stamped = TwistWithCovarianceStamped(ts=0.0) @@ -300,7 +292,6 @@ def test_twist_with_covariance_stamped_zero_timestamp(): assert twist_cov_stamped.ts <= time.time() -@pytest.mark.ros def test_twist_with_covariance_stamped_inheritance(): """Test that it properly inherits from TwistWithCovariance and Timestamped.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) @@ -321,7 +312,6 @@ def test_twist_with_covariance_stamped_inheritance(): assert hasattr(twist_cov_stamped, "covariance") -@pytest.mark.ros def test_twist_with_covariance_stamped_is_zero(): """Test is_zero method inheritance.""" # Zero twist @@ -336,7 +326,6 @@ def test_twist_with_covariance_stamped_is_zero(): assert twist_cov_stamped2 # Boolean conversion -@pytest.mark.ros def test_twist_with_covariance_stamped_sec_nsec(): """Test the sec_nsec helper function.""" from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import sec_nsec @@ -366,11 +355,11 @@ def test_twist_with_covariance_stamped_sec_nsec(): assert ns == 0 +@pytest.mark.ros @pytest.mark.parametrize( "frame_id", ["", "map", "odom", "base_link", "cmd_vel", "sensor/velocity/front"], ) -@pytest.mark.ros def test_twist_with_covariance_stamped_frame_ids(frame_id): """Test various frame ID values.""" twist_cov_stamped = TwistWithCovarianceStamped(frame_id=frame_id) @@ -384,7 +373,6 @@ def test_twist_with_covariance_stamped_frame_ids(frame_id): assert restored.frame_id == frame_id -@pytest.mark.ros def test_twist_with_covariance_stamped_different_covariances(): """Test with different covariance patterns.""" twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.5)) diff --git a/dimos/msgs/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py index e97ac480ee..2fee199b1b 100644 --- a/dimos/msgs/nav_msgs/test_Odometry.py +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -51,7 +51,6 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion -@pytest.mark.ros def test_odometry_default_init(): """Test default initialization.""" if ROSVector3 is None: @@ -100,7 +99,6 @@ def test_odometry_default_init(): assert np.all(odom.twist.covariance == 0.0) -@pytest.mark.ros def test_odometry_with_frames(): """Test initialization with frame IDs.""" ts = 1234567890.123456 @@ -114,7 +112,6 @@ def test_odometry_with_frames(): assert odom.child_frame_id == child_frame_id -@pytest.mark.ros def test_odometry_with_pose_and_twist(): """Test initialization with pose and twist.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -129,7 +126,6 @@ def test_odometry_with_pose_and_twist(): assert odom.twist.twist.angular.z == 0.1 -@pytest.mark.ros def test_odometry_with_covariances(): """Test initialization with pose and twist with covariances.""" pose = Pose(1.0, 2.0, 3.0) @@ -154,7 +150,6 @@ def test_odometry_with_covariances(): assert np.array_equal(odom.twist.covariance, twist_cov) -@pytest.mark.ros def test_odometry_copy_constructor(): """Test copy constructor.""" original = Odometry( @@ -173,7 +168,6 @@ def test_odometry_copy_constructor(): assert copy.twist is not original.twist -@pytest.mark.ros def test_odometry_dict_init(): """Test initialization from dictionary.""" odom_dict = { @@ -193,7 +187,6 @@ def test_odometry_dict_init(): assert odom.twist.linear.x == 0.5 -@pytest.mark.ros def test_odometry_properties(): """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -237,7 +230,6 @@ def test_odometry_properties(): assert odom.yaw == pose.yaw -@pytest.mark.ros def test_odometry_str_repr(): """Test string representations.""" odom = Odometry( @@ -261,7 +253,6 @@ def test_odometry_str_repr(): assert "0.500" in str_repr -@pytest.mark.ros def test_odometry_equality(): """Test equality comparison.""" odom1 = Odometry( @@ -293,7 +284,6 @@ def test_odometry_equality(): assert odom1 != "not an odometry" -@pytest.mark.ros def test_odometry_lcm_encode_decode(): """Test LCM encoding and decoding.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) @@ -430,7 +420,6 @@ def test_odometry_ros_roundtrip(): assert restored.twist == original.twist -@pytest.mark.ros def test_odometry_zero_timestamp(): """Test that zero timestamp gets replaced with current time.""" odom = Odometry(ts=0.0) @@ -440,7 +429,6 @@ def test_odometry_zero_timestamp(): assert odom.ts <= time.time() -@pytest.mark.ros def test_odometry_with_just_pose(): """Test initialization with just a Pose (no covariance).""" pose = Pose(1.0, 2.0, 3.0) @@ -454,7 +442,6 @@ def test_odometry_with_just_pose(): assert np.all(odom.twist.covariance == 0.0) # Twist should also be zero -@pytest.mark.ros def test_odometry_with_just_twist(): """Test initialization with just a Twist (no covariance).""" twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) @@ -467,6 +454,7 @@ def test_odometry_with_just_twist(): assert np.all(odom.pose.covariance == 0.0) # Pose should also be zero +@pytest.mark.ros @pytest.mark.parametrize( "frame_id,child_frame_id", [ @@ -477,7 +465,6 @@ def test_odometry_with_just_twist(): ("", ""), # Empty frames ], ) -@pytest.mark.ros def test_odometry_frame_combinations(frame_id, child_frame_id): """Test various frame ID combinations.""" odom = Odometry(frame_id=frame_id, child_frame_id=child_frame_id) @@ -495,7 +482,6 @@ def test_odometry_frame_combinations(frame_id, child_frame_id): assert restored.child_frame_id == child_frame_id -@pytest.mark.ros def test_odometry_typical_robot_scenario(): """Test a typical robot odometry scenario.""" # Robot moving forward at 0.5 m/s with slight rotation diff --git a/dimos/msgs/nav_msgs/test_Path.py b/dimos/msgs/nav_msgs/test_Path.py index 9294581f7f..94028d7959 100644 --- a/dimos/msgs/nav_msgs/test_Path.py +++ b/dimos/msgs/nav_msgs/test_Path.py @@ -38,13 +38,8 @@ def create_test_pose(x: float, y: float, z: float, frame_id: str = "map") -> Pos ) -@pytest.mark.ros def test_init_empty(): """Test creating an empty path.""" - if ROSPath is None: - pytest.skip("ROS not available") - if ROSPoseStamped is None: - pytest.skip("ROS not available") path = Path(frame_id="map") assert path.frame_id == "map" assert len(path) == 0 @@ -52,7 +47,6 @@ def test_init_empty(): assert path.poses == [] -@pytest.mark.ros def test_init_with_poses(): """Test creating a path with initial poses.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -62,7 +56,6 @@ def test_init_with_poses(): assert path.poses == poses -@pytest.mark.ros def test_head(): """Test getting the first pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -74,7 +67,6 @@ def test_head(): assert empty_path.head() is None -@pytest.mark.ros def test_last(): """Test getting the last pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -86,7 +78,6 @@ def test_last(): assert empty_path.last() is None -@pytest.mark.ros def test_tail(): """Test getting all poses except the first.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -105,7 +96,6 @@ def test_tail(): assert len(empty_path.tail()) == 0 -@pytest.mark.ros def test_push_immutable(): """Test immutable push operation.""" path = Path(frame_id="map") @@ -125,7 +115,6 @@ def test_push_immutable(): assert path3.poses == [pose1, pose2] -@pytest.mark.ros def test_push_mutable(): """Test mutable push operation.""" path = Path(frame_id="map") @@ -142,7 +131,6 @@ def test_push_mutable(): assert path.poses == [pose1, pose2] -@pytest.mark.ros def test_indexing(): """Test indexing and slicing.""" poses = [create_test_pose(i, i, 0) for i in range(5)] @@ -158,7 +146,6 @@ def test_indexing(): assert path[3:] == poses[3:] -@pytest.mark.ros def test_iteration(): """Test iterating over poses.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -170,7 +157,6 @@ def test_iteration(): assert collected == poses -@pytest.mark.ros def test_slice_method(): """Test slice method.""" poses = [create_test_pose(i, i, 0) for i in range(5)] @@ -186,7 +172,6 @@ def test_slice_method(): assert sliced2.poses == poses[2:] -@pytest.mark.ros def test_extend_immutable(): """Test immutable extend operation.""" poses1 = [create_test_pose(i, i, 0) for i in range(2)] @@ -202,7 +187,6 @@ def test_extend_immutable(): assert extended.frame_id == "map" # Keeps first path's frame -@pytest.mark.ros def test_extend_mutable(): """Test mutable extend operation.""" poses1 = [create_test_pose(i, i, 0) for i in range(2)] @@ -220,7 +204,6 @@ def test_extend_mutable(): assert p1.z == p2.z -@pytest.mark.ros def test_reverse(): """Test reverse operation.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -231,7 +214,6 @@ def test_reverse(): assert reversed_path.poses == list(reversed(poses)) -@pytest.mark.ros def test_clear(): """Test clear operation.""" poses = [create_test_pose(i, i, 0) for i in range(3)] @@ -242,7 +224,6 @@ def test_clear(): assert path.poses == [] -@pytest.mark.ros def test_lcm_encode_decode(): """Test encoding and decoding of Path to/from binary LCM format.""" # Create path with poses @@ -295,7 +276,6 @@ def test_lcm_encode_decode(): assert decoded.orientation.w == orig.orientation.w -@pytest.mark.ros def test_lcm_encode_decode_empty(): """Test encoding and decoding of empty Path.""" path_source = Path(frame_id="base_link") @@ -308,7 +288,6 @@ def test_lcm_encode_decode_empty(): assert len(path_dest.poses) == 0 -@pytest.mark.ros def test_str_representation(): """Test string representation.""" path = Path(frame_id="map") diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index 8cc27c9bfe..0c755f74f5 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -35,15 +35,8 @@ ROS_AVAILABLE = False -@pytest.mark.ros def test_lcm_encode_decode(): """Test LCM encode/decode preserves CameraInfo data.""" - if ROSHeader is None: - pytest.skip("ROS not available") - if ROSRegionOfInterest is None: - pytest.skip("ROS not available") - if ROSCameraInfo is None: - pytest.skip("ROS not available") print("Testing CameraInfo LCM encode/decode...") # Create test camera info with sample calibration data @@ -163,7 +156,6 @@ def test_lcm_encode_decode(): print("✓ LCM encode/decode test passed - all properties preserved!") -@pytest.mark.ros def test_numpy_matrix_operations(): """Test numpy matrix getter/setter operations.""" print("\nTesting numpy matrix operations...") @@ -374,7 +366,6 @@ def test_ros_conversion(): print("\n✓ All ROS conversion tests passed!") -@pytest.mark.ros def test_equality(): """Test CameraInfo equality comparison.""" print("\nTesting CameraInfo equality...") diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index d44b43a581..ac115462be 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -38,15 +38,8 @@ ROS_AVAILABLE = False -@pytest.mark.ros def test_lcm_encode_decode(): """Test LCM encode/decode preserves pointcloud data.""" - if ROSHeader is None: - pytest.skip("ROS not available") - if ROSPointField is None: - pytest.skip("ROS not available") - if ROSPointCloud2 is None: - pytest.skip("ROS not available") replay = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) lidar_msg: LidarMessage = replay.load_one("lidar_data_021") diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index ba18805468..dfe3400e1c 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -27,13 +27,8 @@ from dimos.msgs.tf2_msgs import TFMessage -@pytest.mark.ros def test_tfmessage_initialization(): """Test TFMessage initialization with Transform objects.""" - if ROSTFMessage is None: - pytest.skip("ROS not available") - if ROSTransformStamped is None: - pytest.skip("ROS not available") # Create some transforms tf1 = Transform( translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1), frame_id="world", ts=100.0 @@ -57,7 +52,6 @@ def test_tfmessage_initialization(): assert transforms == [tf1, tf2] -@pytest.mark.ros def test_tfmessage_empty(): """Test empty TFMessage.""" msg = TFMessage() @@ -65,7 +59,6 @@ def test_tfmessage_empty(): assert list(msg) == [] -@pytest.mark.ros def test_tfmessage_add_transform(): """Test adding transforms to TFMessage.""" msg = TFMessage() @@ -77,7 +70,6 @@ def test_tfmessage_add_transform(): assert msg[0] == tf -@pytest.mark.ros def test_tfmessage_lcm_encode_decode(): """Test encoding TFMessage to LCM bytes.""" # Create transforms