diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index cb2d5143d9..261aae6452 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -20,18 +20,6 @@ Pose as LCMPose, Transform as LCMTransform, ) - -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - Point as ROSPoint, - Pose as ROSPose, - Quaternion as ROSQuaternion, - ) -except ImportError: - ROSPose = None # type: ignore[assignment, misc] - ROSPoint = None # type: ignore[assignment, misc] - ROSQuaternion = None # type: ignore[assignment, misc] - from plum import dispatch from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable @@ -235,43 +223,6 @@ def __sub__(self, other: Pose) -> Pose: delta_orientation = self.orientation * other.orientation.inverse() return Pose(delta_position, delta_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() # type: ignore[no-untyped-call] - ros_msg.position = ROSPoint( # type: ignore[no-untyped-call] - x=float(self.position.x), y=float(self.position.y), z=float(self.position.z) - ) - ros_msg.orientation = ROSQuaternion( # type: ignore[no-untyped-call] - 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 abdbdc7c9d..acf0af8b32 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -22,13 +22,6 @@ from rerun._baseclasses import Archetype from dimos_lcm.geometry_msgs import PoseStamped as LCMPoseStamped - -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - PoseStamped as ROSPoseStamped, - ) -except ImportError: - ROSPoseStamped = None # type: ignore[assignment, misc] from plum import dispatch from dimos.msgs.geometry_msgs.Pose import Pose @@ -145,44 +138,3 @@ def find_transform(self, other: PoseStamped) -> Transform: translation=local_translation, rotation=relative_rotation, ) - - @classmethod - def from_ros_msg(cls, ros_msg: ROSPoseStamped) -> PoseStamped: # type: ignore[override] - """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: # type: ignore[override] - """Convert to a ROS geometry_msgs/PoseStamped message. - - Returns: - ROS PoseStamped message - """ - ros_msg = ROSPoseStamped() # type: ignore[no-untyped-call] - - # 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/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py index b619679a78..7e856da6dc 100644 --- a/dimos/msgs/geometry_msgs/PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/PoseWithCovariance.py @@ -22,13 +22,6 @@ import numpy as np from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - PoseWithCovariance as ROSPoseWithCovariance, - ) -except ImportError: - ROSPoseWithCovariance = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable if TYPE_CHECKING: @@ -201,33 +194,3 @@ def lcm_decode(cls, data: bytes) -> PoseWithCovariance: ], ) 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() # type: ignore[no-untyped-call] - ros_msg.pose = self.pose.to_ros_msg() - # ROS expects list, not numpy array - if isinstance(self.covariance, np.ndarray): # type: ignore[has-type] - ros_msg.covariance = self.covariance.tolist() # type: ignore[has-type] - else: - ros_msg.covariance = list(self.covariance) # type: ignore[has-type] - return ros_msg diff --git a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py index c6138fd064..9e92aa06f0 100644 --- a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py @@ -23,13 +23,6 @@ import numpy as np from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped, - ) -except ImportError: - ROSPoseWithCovarianceStamped = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance from dimos.types.timestamped import Timestamped @@ -115,51 +108,3 @@ def __str__(self) -> str: 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: # type: ignore[override] - """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, # type: ignore[has-type] - ) - - def to_ros_msg(self) -> ROSPoseWithCovarianceStamped: # type: ignore[override] - """Convert to a ROS geometry_msgs/PoseWithCovarianceStamped message. - - Returns: - ROS PoseWithCovarianceStamped message - """ - - ros_msg = ROSPoseWithCovarianceStamped() # type: ignore[no-untyped-call] - - # 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): # type: ignore[has-type] - ros_msg.pose.covariance = self.covariance.tolist() # type: ignore[has-type] - else: - ros_msg.pose.covariance = list(self.covariance) # type: ignore[has-type] - - return ros_msg diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index d1a0bc74b5..58bab1ec4a 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -25,19 +25,6 @@ TransformStamped as LCMTransformStamped, ) -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - Quaternion as ROSQuaternion, - Transform as ROSTransform, - TransformStamped as ROSTransformStamped, - Vector3 as ROSVector3, - ) -except ImportError: - ROSTransformStamped = None # type: ignore[assignment, misc] - ROSTransform = None # type: ignore[assignment, misc] - ROSVector3 = None # type: ignore[assignment, misc] - ROSQuaternion = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.std_msgs import Header @@ -168,70 +155,6 @@ 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() # type: ignore[no-untyped-call] - - # 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( # type: ignore[no-untyped-call] - x=self.translation.x, y=self.translation.y, z=self.translation.z - ) - ros_msg.transform.rotation = ROSQuaternion( # type: ignore[no-untyped-call] - 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 5184afc5f7..b1320bf986 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -17,15 +17,6 @@ from dimos_lcm.geometry_msgs import Twist as LCMTwist from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - Twist as ROSTwist, - Vector3 as ROSVector3, - ) -except ImportError: - ROSTwist = None # type: ignore[assignment, misc] - ROSVector3 = None # type: ignore[assignment, misc] - # Import Quaternion at runtime for beartype compatibility # (beartype needs to resolve forward references at runtime) from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -108,32 +99,5 @@ def __bool__(self) -> bool: """ 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() # type: ignore[no-untyped-call] - ros_msg.linear = ROSVector3(x=self.linear.x, y=self.linear.y, z=self.linear.z) # type: ignore[no-untyped-call] - ros_msg.angular = ROSVector3(x=self.angular.x, y=self.angular.y, z=self.angular.z) # type: ignore[no-untyped-call] - return ros_msg - __all__ = ["Quaternion", "Twist"] diff --git a/dimos/msgs/geometry_msgs/TwistStamped.py b/dimos/msgs/geometry_msgs/TwistStamped.py index f5305509e5..ab3dc507b9 100644 --- a/dimos/msgs/geometry_msgs/TwistStamped.py +++ b/dimos/msgs/geometry_msgs/TwistStamped.py @@ -20,13 +20,6 @@ from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - TwistStamped as ROSTwistStamped, - ) -except ImportError: - ROSTwistStamped = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable from dimos.types.timestamped import Timestamped @@ -75,46 +68,3 @@ def __str__(self) -> str: 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: # type: ignore[override] - """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: # type: ignore[override] - """Convert to a ROS geometry_msgs/TwistStamped message. - - Returns: - ROS TwistStamped message - """ - - ros_msg = ROSTwistStamped() # type: ignore[no-untyped-call] - - # 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/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py index 1abbe54468..09f234ea84 100644 --- a/dimos/msgs/geometry_msgs/TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/TwistWithCovariance.py @@ -22,13 +22,6 @@ import numpy as np from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - TwistWithCovariance as ROSTwistWithCovariance, - ) -except ImportError: - ROSTwistWithCovariance = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable @@ -197,33 +190,3 @@ def lcm_decode(cls, data: bytes) -> TwistWithCovariance: 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() # type: ignore[no-untyped-call] - ros_msg.twist = self.twist.to_ros_msg() - # ROS expects list, not numpy array - if isinstance(self.covariance, np.ndarray): # type: ignore[has-type] - ros_msg.covariance = self.covariance.tolist() # type: ignore[has-type] - else: - ros_msg.covariance = list(self.covariance) # type: ignore[has-type] - return ros_msg diff --git a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py index 3b1df6819b..82d0ba7eb2 100644 --- a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py @@ -23,13 +23,6 @@ import numpy as np from plum import dispatch -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped, - ) -except ImportError: - ROSTwistWithCovarianceStamped = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable @@ -123,51 +116,3 @@ def __str__(self) -> str: 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: # type: ignore[override] - """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, # type: ignore[has-type] - ) - - def to_ros_msg(self) -> ROSTwistWithCovarianceStamped: # type: ignore[override] - """Convert to a ROS geometry_msgs/TwistWithCovarianceStamped message. - - Returns: - ROS TwistWithCovarianceStamped message - """ - - ros_msg = ROSTwistWithCovarianceStamped() # type: ignore[no-untyped-call] - - # 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): # type: ignore[has-type] - ros_msg.twist.covariance = self.covariance.tolist() # type: ignore[has-type] - else: - ros_msg.twist.covariance = list(self.covariance) # type: ignore[has-type] - - return ros_msg diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index 50bfaf1388..dac5ed6207 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -18,13 +18,6 @@ import numpy as np import pytest -try: - from geometry_msgs.msg import Point as ROSPoint, Pose as ROSPose, 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 @@ -754,55 +747,3 @@ def test_pose_addition_3d_rotation() -> None: 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) - - -@pytest.mark.ros -def test_pose_from_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_pose_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_pose_ros_roundtrip() -> None: - """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 603723b610..82250a9113 100644 --- a/dimos/msgs/geometry_msgs/test_PoseStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -15,13 +15,6 @@ import pickle import time -import pytest - -try: - from geometry_msgs.msg import PoseStamped as ROSPoseStamped -except ImportError: - ROSPoseStamped = None - from dimos.msgs.geometry_msgs import PoseStamped @@ -60,80 +53,3 @@ def test_pickle_encode_decode() -> None: assert isinstance(pose_dest, PoseStamped) assert pose_dest is not pose_source assert pose_dest == pose_source - - -@pytest.mark.ros -def test_pose_stamped_from_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_pose_stamped_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_pose_stamped_ros_roundtrip() -> None: - """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_PoseWithCovariance.py b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py index d62ca6e806..f6936db9f7 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py @@ -16,19 +16,6 @@ import numpy as np import pytest -try: - from geometry_msgs.msg import ( - Point as ROSPoint, - Pose as ROSPose, - PoseWithCovariance as ROSPoseWithCovariance, - 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 @@ -288,59 +275,6 @@ def test_pose_with_covariance_lcm_encode_decode() -> None: assert isinstance(decoded.covariance, np.ndarray) -@pytest.mark.ros -def test_pose_with_covariance_from_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_pose_with_covariance_to_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_pose_with_covariance_ros_roundtrip() -> None: - """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() -> None: """Test with zero covariance matrix.""" pose = Pose(1.0, 2.0, 3.0) diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py index 1d04bd8e87..1e910b53e7 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py @@ -15,27 +15,6 @@ import time import numpy as np -import pytest - -try: - from builtin_interfaces.msg import Time as ROSTime - from geometry_msgs.msg import ( - Point as ROSPoint, - Pose as ROSPose, - PoseWithCovariance as ROSPoseWithCovariance, - PoseWithCovarianceStamped as ROSPoseWithCovarianceStamped, - Quaternion as ROSQuaternion, - ) - from std_msgs.msg import Header as ROSHeader -except ImportError: - ROSHeader = None - ROSPoseWithCovarianceStamped = None - ROSPose = None - ROSQuaternion = None - ROSPoint = None - ROSTime = None - ROSPoseWithCovariance = None - from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -44,20 +23,6 @@ def test_pose_with_covariance_stamped_default_init() -> None: """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 @@ -180,98 +145,6 @@ def test_pose_with_covariance_stamped_lcm_encode_decode() -> None: assert np.array_equal(decoded.covariance, covariance) -@pytest.mark.ros -def test_pose_with_covariance_stamped_from_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_pose_with_covariance_stamped_to_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_pose_with_covariance_stamped_ros_roundtrip() -> None: - """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() -> None: """Test that zero timestamp gets replaced with current time.""" pose_cov_stamped = PoseWithCovarianceStamped(ts=0.0) @@ -330,24 +203,6 @@ def test_pose_with_covariance_stamped_sec_nsec() -> None: assert ns == 0 -@pytest.mark.ros -@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) -> None: - """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() -> None: """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 2a1daff684..0c15610b05 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -18,12 +18,6 @@ import numpy as np import pytest -try: - from geometry_msgs.msg import TransformStamped as ROSTransformStamped -except ImportError: - ROSTransformStamped = None - - from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Transform, Vector3 @@ -424,86 +418,3 @@ def test_transform_from_pose_invalid_type() -> None: with pytest.raises(TypeError): Transform.from_pose(None) - - -@pytest.mark.ros -def test_transform_from_ros_transform_stamped() -> None: - """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 - - -@pytest.mark.ros -def test_transform_to_ros_transform_stamped() -> None: - """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 - - -@pytest.mark.ros -def test_transform_ros_roundtrip() -> None: - """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 f83ffa3fdd..df4bd8b6a2 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -12,16 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np -import pytest - -try: - from geometry_msgs.msg import Twist as ROSTwist, Vector3 as ROSVector3 -except ImportError: - ROSTwist = None - ROSVector3 = None - from dimos_lcm.geometry_msgs import Twist as LCMTwist +import numpy as np from dimos.msgs.geometry_msgs import Quaternion, Twist, Vector3 @@ -205,97 +197,3 @@ def test_twist_with_lists() -> None: 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) - - -@pytest.mark.ros -def test_twist_from_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_twist_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_ros_zero_twist_conversion() -> None: - """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 - - -@pytest.mark.ros -def test_ros_negative_values_conversion() -> None: - """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 - - -@pytest.mark.ros -def test_ros_roundtrip_conversion() -> None: - """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/geometry_msgs/test_TwistStamped.py b/dimos/msgs/geometry_msgs/test_TwistStamped.py index 7ba2f59e7d..afb8489032 100644 --- a/dimos/msgs/geometry_msgs/test_TwistStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistStamped.py @@ -15,13 +15,6 @@ import pickle import time -import pytest - -try: - from geometry_msgs.msg import TwistStamped as ROSTwistStamped -except ImportError: - ROSTwistStamped = None - from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped @@ -61,98 +54,13 @@ def test_pickle_encode_decode() -> None: assert twist_dest == twist_source -@pytest.mark.ros -def test_twist_stamped_from_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_twist_stamped_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_twist_stamped_ros_roundtrip() -> None: - """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("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("test_pickle_encode_decode passed") print("\nAll tests passed!") diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py index 746b0c3646..1d8ae820ad 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py @@ -12,22 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance import numpy as np import pytest -try: - from geometry_msgs.msg import ( - Twist as ROSTwist, - TwistWithCovariance as ROSTwistWithCovariance, - Vector3 as ROSVector3, - ) -except ImportError: - ROSTwist = None - ROSTwistWithCovariance = None - ROSVector3 = None - -from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance - from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -35,10 +23,6 @@ def test_twist_with_covariance_default_init() -> None: """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 @@ -317,57 +301,6 @@ def test_twist_with_covariance_lcm_encode_decode() -> None: assert isinstance(decoded.covariance, np.ndarray) -@pytest.mark.ros -def test_twist_with_covariance_from_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_twist_with_covariance_to_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_twist_with_covariance_ros_roundtrip() -> None: - """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() -> None: """Test with zero covariance matrix.""" twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py index f0d7e5b4ab..2be647bff1 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py @@ -15,25 +15,6 @@ import time import numpy as np -import pytest - -try: - from builtin_interfaces.msg import Time as ROSTime - from geometry_msgs.msg import ( - Twist as ROSTwist, - TwistWithCovariance as ROSTwistWithCovariance, - TwistWithCovarianceStamped as ROSTwistWithCovarianceStamped, - Vector3 as ROSVector3, - ) - from std_msgs.msg import Header as ROSHeader -except ImportError: - ROSTwistWithCovarianceStamped = None - ROSTwist = None - ROSHeader = None - ROSTime = None - ROSTwistWithCovariance = None - ROSVector3 = None - from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance @@ -43,18 +24,6 @@ def test_twist_with_covariance_stamped_default_init() -> None: """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 @@ -191,97 +160,6 @@ def test_twist_with_covariance_stamped_lcm_encode_decode() -> None: assert np.array_equal(decoded.covariance, covariance) -@pytest.mark.ros -def test_twist_with_covariance_stamped_from_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_twist_with_covariance_stamped_to_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_twist_with_covariance_stamped_ros_roundtrip() -> None: - """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() -> None: """Test that zero timestamp gets replaced with current time.""" twist_cov_stamped = TwistWithCovarianceStamped(ts=0.0) @@ -354,24 +232,6 @@ def test_twist_with_covariance_stamped_sec_nsec() -> None: assert ns == 0 -@pytest.mark.ros -@pytest.mark.parametrize( - "frame_id", - ["", "map", "odom", "base_link", "cmd_vel", "sensor/velocity/front"], -) -def test_twist_with_covariance_stamped_frame_ids(frame_id) -> None: - """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() -> None: """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/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index 3cdd631aa7..e6d0844f46 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -21,11 +21,6 @@ import numpy as np from plum import dispatch -try: - from nav_msgs.msg import Odometry as ROSOdometry # type: ignore[attr-defined] -except ImportError: - ROSOdometry = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance from dimos.msgs.geometry_msgs.Twist import Twist @@ -328,54 +323,3 @@ def lcm_decode(cls, data: bytes) -> Odometry: 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() # type: ignore[no-untyped-call] - - # 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/Path.py b/dimos/msgs/nav_msgs/Path.py index b25c778cd8..1582c4b775 100644 --- a/dimos/msgs/nav_msgs/Path.py +++ b/dimos/msgs/nav_msgs/Path.py @@ -26,11 +26,6 @@ from dimos_lcm.nav_msgs import Path as LCMPath from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime -try: - from nav_msgs.msg import Path as ROSPath # type: ignore[attr-defined] -except ImportError: - ROSPath = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.types.timestamped import Timestamped @@ -193,47 +188,6 @@ 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() # type: ignore[no-untyped-call] - - # 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 - def to_rerun( self, color: tuple[int, int, int] = (0, 255, 128), diff --git a/dimos/msgs/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py index ecdc83c6b4..e6eadc51f8 100644 --- a/dimos/msgs/nav_msgs/test_Odometry.py +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -15,33 +15,6 @@ import time import numpy as np -import pytest - -try: - from builtin_interfaces.msg import Time as ROSTime - from geometry_msgs.msg import ( - Point as ROSPoint, - Pose as ROSPose, - PoseWithCovariance as ROSPoseWithCovariance, - Quaternion as ROSQuaternion, - Twist as ROSTwist, - TwistWithCovariance as ROSTwistWithCovariance, - Vector3 as ROSVector3, - ) - from nav_msgs.msg import Odometry as ROSOdometry - from std_msgs.msg import Header as ROSHeader -except ImportError: - ROSTwist = None - ROSHeader = None - ROSPose = None - ROSPoseWithCovariance = None - ROSQuaternion = None - ROSOdometry = None - ROSPoint = None - ROSTime = None - ROSTwistWithCovariance = None - ROSVector3 = None - from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -53,26 +26,6 @@ def test_odometry_default_init() -> None: """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 @@ -311,115 +264,6 @@ def test_odometry_lcm_encode_decode() -> None: assert decoded.twist == source.twist -@pytest.mark.ros -def test_odometry_from_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_odometry_to_ros_msg() -> None: - """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)) - - -@pytest.mark.ros -def test_odometry_ros_roundtrip() -> None: - """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() -> None: """Test that zero timestamp gets replaced with current time.""" odom = Odometry(ts=0.0) @@ -454,34 +298,6 @@ def test_odometry_with_just_twist() -> None: assert np.all(odom.pose.covariance == 0.0) # Pose should also be zero -@pytest.mark.ros -@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) -> None: - """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() -> None: """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 d933123b2b..9bd0cc92b6 100644 --- a/dimos/msgs/nav_msgs/test_Path.py +++ b/dimos/msgs/nav_msgs/test_Path.py @@ -13,15 +13,6 @@ # limitations under the License. -import pytest - -try: - from geometry_msgs.msg import PoseStamped as ROSPoseStamped - from nav_msgs.msg import Path as ROSPath -except ImportError: - ROSPoseStamped = None - ROSPath = None - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.nav_msgs.Path import Path @@ -294,98 +285,3 @@ def test_str_representation() -> None: 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)" - - -@pytest.mark.ros -def test_path_from_ros_msg() -> None: - """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): - 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 - - -@pytest.mark.ros -def test_path_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_path_ros_roundtrip() -> None: - """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, strict=False): - 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/sensor_msgs/CameraInfo.py b/dimos/msgs/sensor_msgs/CameraInfo.py index b296e726cb..a371475675 100644 --- a/dimos/msgs/sensor_msgs/CameraInfo.py +++ b/dimos/msgs/sensor_msgs/CameraInfo.py @@ -25,18 +25,6 @@ from dimos_lcm.std_msgs.Header import Header import numpy as np -# Import ROS types -try: - from sensor_msgs.msg import ( # type: ignore[attr-defined] - CameraInfo as ROSCameraInfo, - RegionOfInterest as ROSRegionOfInterest, - ) - from std_msgs.msg import Header as ROSHeader # type: ignore[attr-defined] - - ROS_AVAILABLE = True -except ImportError: - ROS_AVAILABLE = False - from dimos.types.timestamped import Timestamped @@ -280,89 +268,6 @@ def lcm_decode(cls, data: bytes) -> CameraInfo: 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() # type: ignore[no-untyped-call] - - # Set header - ros_msg.header = ROSHeader() # type: ignore[no-untyped-call] - 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() # type: ignore[no-untyped-call] - 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 ( diff --git a/dimos/msgs/sensor_msgs/Image.py b/dimos/msgs/sensor_msgs/Image.py index b0a4017d06..2a8aa2c017 100644 --- a/dimos/msgs/sensor_msgs/Image.py +++ b/dimos/msgs/sensor_msgs/Image.py @@ -37,11 +37,6 @@ from reactivex.observable import Observable -try: - from sensor_msgs.msg import Image as ROSImage # type: ignore[attr-defined] -except ImportError: - ROSImage = None # type: ignore[assignment, misc] - class ImageFormat(Enum): BGR = "BGR" @@ -575,91 +570,6 @@ def lcm_jpeg_decode(cls, data: bytes, **kwargs: Any) -> Image: ), ) - @classmethod - def from_ros_msg(cls, ros_msg: ROSImage) -> Image: - """Create an Image from a ROS sensor_msgs/Image message. - - Args: - ros_msg: ROS Image message - - Returns: - Image instance - """ - # Convert timestamp from ROS header - ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) - - # Parse encoding to determine format and data type - format_info = cls._parse_encoding(ros_msg.encoding) - - # Convert data from ROS message (array.array) to numpy array - data_array = np.frombuffer(ros_msg.data, dtype=format_info["dtype"]) - - # Reshape to image dimensions - if format_info["channels"] == 1: - data_array = data_array.reshape((ros_msg.height, ros_msg.width)) - else: - data_array = data_array.reshape( - (ros_msg.height, ros_msg.width, format_info["channels"]) - ) - - # Crop to center 1/3 of the image (simulate 120-degree FOV from 360-degree) - original_width = data_array.shape[1] - crop_width = original_width // 3 - start_x = (original_width - crop_width) // 2 - end_x = start_x + crop_width - - # Crop the image horizontally to center 1/3 - if len(data_array.shape) == 2: - # Grayscale image - data_array = data_array[:, start_x:end_x] - else: - # Color image - data_array = data_array[:, start_x:end_x, :] - - # Fix color channel order: if ROS sends RGB but we expect BGR, swap channels - # ROS typically uses rgb8 encoding, but OpenCV/our system expects BGR - if format_info["format"] == ImageFormat.RGB: - # Convert RGB to BGR by swapping channels - if len(data_array.shape) == 3 and data_array.shape[2] == 3: - data_array = data_array[:, :, [2, 1, 0]] # RGB -> BGR - format_info["format"] = ImageFormat.BGR - elif format_info["format"] == ImageFormat.RGBA: - # Convert RGBA to BGRA by swapping channels - if len(data_array.shape) == 3 and data_array.shape[2] == 4: - data_array = data_array[:, :, [2, 1, 0, 3]] # RGBA -> BGRA - format_info["format"] = ImageFormat.BGRA - - return cls( - data=data_array, - format=format_info["format"], - frame_id=ros_msg.header.frame_id, - ts=ts, - ) - - @staticmethod - def _parse_encoding(encoding: str) -> dict[str, Any]: - """Translate ROS encoding strings into format metadata.""" - encoding_map = { - "mono8": {"format": ImageFormat.GRAY, "dtype": np.uint8, "channels": 1}, - "mono16": {"format": ImageFormat.GRAY16, "dtype": np.uint16, "channels": 1}, - "rgb8": {"format": ImageFormat.RGB, "dtype": np.uint8, "channels": 3}, - "rgba8": {"format": ImageFormat.RGBA, "dtype": np.uint8, "channels": 4}, - "bgr8": {"format": ImageFormat.BGR, "dtype": np.uint8, "channels": 3}, - "bgra8": {"format": ImageFormat.BGRA, "dtype": np.uint8, "channels": 4}, - "32FC1": {"format": ImageFormat.DEPTH, "dtype": np.float32, "channels": 1}, - "32FC3": {"format": ImageFormat.RGB, "dtype": np.float32, "channels": 3}, - "64FC1": {"format": ImageFormat.DEPTH, "dtype": np.float64, "channels": 1}, - "16UC1": {"format": ImageFormat.DEPTH16, "dtype": np.uint16, "channels": 1}, - "16SC1": {"format": ImageFormat.DEPTH16, "dtype": np.int16, "channels": 1}, - } - - key = encoding.strip() - for candidate in (key, key.lower(), key.upper()): - if candidate in encoding_map: - return dict(encoding_map[candidate]) - - raise ValueError(f"Unsupported encoding: {encoding}") - __all__ = [ "Image", diff --git a/dimos/msgs/sensor_msgs/JointState.py b/dimos/msgs/sensor_msgs/JointState.py index 2936012bcc..9faf0be42c 100644 --- a/dimos/msgs/sensor_msgs/JointState.py +++ b/dimos/msgs/sensor_msgs/JointState.py @@ -18,12 +18,6 @@ from typing import TypeAlias from dimos_lcm.sensor_msgs import JointState as LCMJointState - -try: - from sensor_msgs.msg import JointState as ROSJointState # type: ignore[attr-defined] -except ImportError: - ROSJointState = None # type: ignore[assignment, misc] - from plum import dispatch from dimos.types.timestamped import Timestamped @@ -150,46 +144,3 @@ def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] and self.effort == other.effort and self.frame_id == other.frame_id ) - - @classmethod - def from_ros_msg(cls, ros_msg: ROSJointState) -> JointState: - """Create a JointState from a ROS sensor_msgs/JointState message. - - Args: - ros_msg: ROS JointState message - - Returns: - JointState instance - """ - # Convert timestamp from ROS header - ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) - - return cls( - ts=ts, - frame_id=ros_msg.header.frame_id, - name=list(ros_msg.name), - position=list(ros_msg.position), - velocity=list(ros_msg.velocity), - effort=list(ros_msg.effort), - ) - - def to_ros_msg(self) -> ROSJointState: - """Convert to a ROS sensor_msgs/JointState message. - - Returns: - ROS JointState message - """ - ros_msg = ROSJointState() # type: ignore[no-untyped-call] - - # 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 joint data - ros_msg.name = self.name - ros_msg.position = self.position - ros_msg.velocity = self.velocity - ros_msg.effort = self.effort - - return ros_msg diff --git a/dimos/msgs/sensor_msgs/Joy.py b/dimos/msgs/sensor_msgs/Joy.py index c8c2fbcd3e..3823f132b7 100644 --- a/dimos/msgs/sensor_msgs/Joy.py +++ b/dimos/msgs/sensor_msgs/Joy.py @@ -18,12 +18,6 @@ from typing import TypeAlias from dimos_lcm.sensor_msgs import Joy as LCMJoy - -try: - from sensor_msgs.msg import Joy as ROSJoy # type: ignore[attr-defined] -except ImportError: - ROSJoy = None # type: ignore[assignment, misc] - from plum import dispatch from dimos.types.timestamped import Timestamped @@ -140,42 +134,3 @@ def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] and self.buttons == other.buttons and self.frame_id == other.frame_id ) - - @classmethod - def from_ros_msg(cls, ros_msg: ROSJoy) -> Joy: - """Create a Joy from a ROS sensor_msgs/Joy message. - - Args: - ros_msg: ROS Joy message - - Returns: - Joy instance - """ - # Convert timestamp from ROS header - ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) - - return cls( - ts=ts, - frame_id=ros_msg.header.frame_id, - axes=list(ros_msg.axes), - buttons=list(ros_msg.buttons), - ) - - def to_ros_msg(self) -> ROSJoy: - """Convert to a ROS sensor_msgs/Joy message. - - Returns: - ROS Joy message - """ - ros_msg = ROSJoy() # type: ignore[no-untyped-call] - - # 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 axes and buttons - ros_msg.axes = self.axes - ros_msg.buttons = self.buttons - - return ros_msg diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 60e5ab7675..39a0146e4c 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -16,6 +16,7 @@ import functools import struct +from typing import TYPE_CHECKING, Any # Import LCM types from dimos_lcm.sensor_msgs.PointCloud2 import ( @@ -28,21 +29,6 @@ import open3d.core as o3c # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Transform, Vector3 - -# Import ROS types -try: - from sensor_msgs.msg import ( # type: ignore[attr-defined] - PointCloud2 as ROSPointCloud2, - PointField as ROSPointField, - ) - from std_msgs.msg import Header as ROSHeader # type: ignore[attr-defined] - - ROS_AVAILABLE = True -except ImportError: - ROS_AVAILABLE = False - -from typing import TYPE_CHECKING, Any - from dimos.types.timestamped import Timestamped if TYPE_CHECKING: @@ -753,204 +739,3 @@ def filter_by_height( 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 using numpy for bulk conversion - num_points = ros_msg.width * ros_msg.height - data = ros_msg.data - point_step = ros_msg.point_step - - # Determine byte order - byte_order = ">" if ros_msg.is_bigendian else "<" - - # 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: # type: ignore[operator] - 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 # type: ignore[operator] - 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 # type: ignore[operator] - 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: - mask = np.isfinite(points).all(axis=1) - points = points[mask] # type: ignore[assignment] - - # 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. - - Includes RGB color data if the pointcloud has colors. - - Returns: - ROS PointCloud2 message - """ - if not ROS_AVAILABLE: - raise ImportError("ROS packages not available. Cannot convert to ROS message.") - - ros_msg = ROSPointCloud2() # type: ignore[no-untyped-call] - - # Set header - ros_msg.header = ROSHeader() # type: ignore[no-untyped-call] - 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) - - # Check if pointcloud has colors - has_colors = self.pointcloud.has_colors() - - if has_colors: - # Include RGB field - pack as XYZRGB - ros_msg.fields = [ - ROSPointField(name="x", offset=0, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ROSPointField(name="y", offset=4, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ROSPointField(name="z", offset=8, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ROSPointField(name="rgb", offset=12, datatype=ROSPointField.UINT32, count=1), # type: ignore[no-untyped-call] - ] - ros_msg.point_step = 16 # 3 floats + 1 uint32 - - # Get colors and convert to packed RGB uint32 - colors = np.asarray(self.pointcloud.colors) # (N, 3) in [0, 1] - colors_uint8 = (colors * 255).astype(np.uint8) - rgb_packed = ( - (colors_uint8[:, 0].astype(np.uint32) << 16) - | (colors_uint8[:, 1].astype(np.uint32) << 8) - | colors_uint8[:, 2].astype(np.uint32) - ) - - # Create structured array with x, y, z, rgb - cloud_data = np.zeros( - len(points), - dtype=[("x", np.float32), ("y", np.float32), ("z", np.float32), ("rgb", np.uint32)], - ) - cloud_data["x"] = points[:, 0] - cloud_data["y"] = points[:, 1] - cloud_data["z"] = points[:, 2] - cloud_data["rgb"] = rgb_packed - - ros_msg.data = cloud_data.tobytes() - else: - # No colors - just XYZ - ros_msg.fields = [ - ROSPointField(name="x", offset=0, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ROSPointField(name="y", offset=4, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ROSPointField(name="z", offset=8, datatype=ROSPointField.FLOAT32, count=1), # type: ignore[no-untyped-call] - ] - ros_msg.point_step = 12 # 3 floats * 4 bytes each - - ros_msg.data = points.astype(np.float32).tobytes() - - ros_msg.row_step = ros_msg.point_step * ros_msg.width - - # 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 d66a39727f..0cf51d15c7 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -14,15 +14,6 @@ # limitations under the License. import numpy as np -import pytest - -try: - from sensor_msgs.msg import CameraInfo as ROSCameraInfo, 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 CalibrationProvider, CameraInfo from dimos.utils.path_utils import get_project_root @@ -186,175 +177,6 @@ def test_numpy_matrix_operations() -> None: print("✓ All numpy matrix operations passed!") -@pytest.mark.ros -def test_ros_conversion() -> None: - """Test ROS message conversion preserves CameraInfo data.""" - 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=1234567890.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", "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() -> None: """Test CameraInfo equality comparison.""" print("\nTesting CameraInfo equality...") diff --git a/dimos/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py index 77b47f4983..499c2bb860 100644 --- a/dimos/msgs/sensor_msgs/test_Joy.py +++ b/dimos/msgs/sensor_msgs/test_Joy.py @@ -14,18 +14,6 @@ # limitations under the License. -import pytest - -try: - from sensor_msgs.msg import Joy as ROSJoy - from std_msgs.msg import Header as ROSHeader - - ROS_AVAILABLE = True -except ImportError: - ROSJoy = None - ROSHeader = None - ROS_AVAILABLE = False - from dimos.msgs.sensor_msgs.Joy import Joy @@ -165,38 +153,6 @@ def test_string_representation() -> None: print("✓ Joy string representation test passed") -@pytest.mark.ros -def test_ros_conversion() -> None: - """Test conversion to/from ROS Joy messages.""" - print("Testing Joy ROS conversion...") - - # Create a ROS Joy message - ros_msg = ROSJoy() - ros_msg.header = ROSHeader() - ros_msg.header.stamp.sec = 1234567890 - ros_msg.header.stamp.nanosec = 123456789 - ros_msg.header.frame_id = "ros_gamepad" - ros_msg.axes = [0.25, -0.75, 0.0, 1.0, -1.0] - ros_msg.buttons = [1, 1, 0, 0, 1, 0, 1, 0] - - # Convert from ROS - joy = Joy.from_ros_msg(ros_msg) - assert abs(joy.ts - 1234567890.123456789) < 1e-9 - assert joy.frame_id == "ros_gamepad" - assert joy.axes == [0.25, -0.75, 0.0, 1.0, -1.0] - assert joy.buttons == [1, 1, 0, 0, 1, 0, 1, 0] - - # Convert back to ROS - ros_msg2 = joy.to_ros_msg() - assert ros_msg2.header.frame_id == "ros_gamepad" - assert ros_msg2.header.stamp.sec == 1234567890 - assert abs(ros_msg2.header.stamp.nanosec - 123456789) < 100 # Allow small rounding - assert list(ros_msg2.axes) == [0.25, -0.75, 0.0, 1.0, -1.0] - assert list(ros_msg2.buttons) == [1, 1, 0, 0, 1, 0, 1, 0] - - print("✓ Joy ROS conversion test passed") - - def test_edge_cases() -> None: """Test Joy with edge cases.""" print("Testing Joy edge cases...") diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index 652ff08921..52f9b1bd59 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -15,26 +15,11 @@ import numpy as np -import pytest - -try: - from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, 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 pointcloud2_from_webrtc_lidar from dimos.utils.testing import SensorReplay -# Try to import ROS types for testing -try: - ROS_AVAILABLE = True -except ImportError: - ROS_AVAILABLE = False - def test_lcm_encode_decode() -> None: """Test LCM encode/decode preserves pointcloud data.""" @@ -97,141 +82,6 @@ def test_lcm_encode_decode() -> None: print("✓ LCM encode/decode test passed - all properties preserved!") -@pytest.mark.ros -def test_ros_conversion() -> None: - """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!") - - def test_bounding_box_intersects() -> None: """Test bounding_box_intersects method with various scenarios.""" # Test 1: Overlapping boxes @@ -307,5 +157,4 @@ def test_bounding_box_intersects() -> None: if __name__ == "__main__": test_lcm_encode_decode() - test_ros_conversion() test_bounding_box_intersects() diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py index c11743573f..4421447edf 100644 --- a/dimos/msgs/std_msgs/Bool.py +++ b/dimos/msgs/std_msgs/Bool.py @@ -17,41 +17,12 @@ from dimos_lcm.std_msgs import Bool as LCMBool -try: - from std_msgs.msg import Bool as ROSBool # type: ignore[attr-defined] -except ImportError: - ROSBool = None # type: ignore[assignment, misc] - class Bool(LCMBool): # type: ignore[misc] - """ROS-compatible Bool message.""" + """Bool message.""" msg_name = "std_msgs.Bool" def __init__(self, data: bool = False) -> None: """Initialize Bool with data value.""" self.data = data - - @classmethod - def from_ros_msg(cls, ros_msg: ROSBool) -> "Bool": - """Create a Bool from a ROS std_msgs/Bool message. - - Args: - ros_msg: ROS Bool message - - Returns: - Bool instance - """ - return cls(data=ros_msg.data) - - def to_ros_msg(self) -> ROSBool: - """Convert to a ROS std_msgs/Bool message. - - Returns: - ROS Bool message - """ - if ROSBool is None: - raise ImportError("ROS std_msgs not available") - ros_msg = ROSBool() # type: ignore[no-untyped-call] - ros_msg.data = bool(self.data) - return ros_msg diff --git a/dimos/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py index b07e965e3f..ca87140353 100644 --- a/dimos/msgs/std_msgs/Int8.py +++ b/dimos/msgs/std_msgs/Int8.py @@ -21,41 +21,12 @@ from dimos_lcm.std_msgs import Int8 as LCMInt8 -try: - from std_msgs.msg import Int8 as ROSInt8 # type: ignore[attr-defined] -except ImportError: - ROSInt8 = None # type: ignore[assignment, misc] - class Int8(LCMInt8): # type: ignore[misc] - """ROS-compatible Int32 message.""" + """Int8 message.""" msg_name: ClassVar[str] = "std_msgs.Int8" def __init__(self, data: int = 0) -> None: """Initialize Int8 with data value.""" self.data = data - - @classmethod - def from_ros_msg(cls, ros_msg: ROSInt8) -> "Int8": - """Create a Bool from a ROS std_msgs/Bool message. - - Args: - ros_msg: ROS Int8 message - - Returns: - Int8 instance - """ - return cls(data=ros_msg.data) - - def to_ros_msg(self) -> ROSInt8: - """Convert to a ROS std_msgs/Bool message. - - Returns: - ROS Int8 message - """ - if ROSInt8 is None: - raise ImportError("ROS std_msgs not available") - ros_msg = ROSInt8() # type: ignore[no-untyped-call] - ros_msg.data = self.data - return ros_msg diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py index 13cf579845..7a47a96e6d 100644 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ b/dimos/msgs/tf2_msgs/TFMessage.py @@ -31,15 +31,6 @@ from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - TransformStamped as ROSTransformStamped, - ) - from tf2_msgs.msg import TFMessage as ROSTFMessage # type: ignore[attr-defined] -except ImportError: - ROSTFMessage = None # type: ignore[assignment, misc] - ROSTransformStamped = None # type: ignore[assignment, misc] - from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -132,38 +123,6 @@ def __str__(self) -> str: ) 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() # type: ignore[no-untyped-call] - - # Convert each Transform to ROS TransformStamped - for transform in self.transforms: - ros_msg.transforms.append(transform.to_ros_transform_stamped()) - - return ros_msg - def to_rerun(self) -> RerunMulti: """Convert to a list of rerun Transform3D archetypes. diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index d58f30093b..8567de9988 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -12,15 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pytest - -try: - from geometry_msgs.msg import TransformStamped as ROSTransformStamped - from tf2_msgs.msg import TFMessage as ROSTFMessage -except ImportError: - ROSTransformStamped = None - ROSTFMessage = None - from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 @@ -147,155 +138,3 @@ def test_tfmessage_lcm_encode_decode() -> None: assert ts2.child_frame_id == "target" assert ts2.transform.rotation.z == 0.707 assert ts2.transform.rotation.w == 0.707 - - -@pytest.mark.ros -def test_tfmessage_from_ros_msg() -> None: - """Test creating a TFMessage from a ROS TFMessage message.""" - - 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 - - -@pytest.mark.ros -def test_tfmessage_to_ros_msg() -> None: - """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 - - -@pytest.mark.ros -def test_tfmessage_ros_roundtrip() -> None: - """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, strict=False): - 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/navigation/demo_ros_navigation.py b/dimos/navigation/demo_ros_navigation.py index 733f66c1b7..4919ab0efd 100644 --- a/dimos/navigation/demo_ros_navigation.py +++ b/dimos/navigation/demo_ros_navigation.py @@ -14,13 +14,9 @@ import time -import rclpy - from dimos import core -from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Twist, Vector3 -from dimos.msgs.nav_msgs import Path -from dimos.msgs.sensor_msgs import PointCloud2 -from dimos.navigation.rosnav import ROSNav +from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3 +from dimos.navigation import rosnav from dimos.protocol import pubsub from dimos.utils.logging_config import setup_logger @@ -31,16 +27,7 @@ def main() -> None: pubsub.lcm.autoconf() # type: ignore[attr-defined] dimos = core.start(2) - ros_nav = dimos.deploy(ROSNav) # type: ignore[attr-defined] - - ros_nav.goal_req.transport = core.LCMTransport("/goal", PoseStamped) - ros_nav.pointcloud.transport = core.LCMTransport("/pointcloud_map", PointCloud2) - ros_nav.global_pointcloud.transport = core.LCMTransport("/global_pointcloud", PointCloud2) - ros_nav.goal_active.transport = core.LCMTransport("/goal_active", PoseStamped) - ros_nav.path_active.transport = core.LCMTransport("/path_active", Path) - ros_nav.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist) - - ros_nav.start() + ros_nav = rosnav.deploy(dimos) logger.info("\nTesting navigation in 2 seconds...") time.sleep(2) @@ -48,13 +35,17 @@ def main() -> None: test_pose = PoseStamped( ts=time.time(), frame_id="map", - position=Vector3(2.0, 2.0, 0.0), + position=Vector3(10.0, 10.0, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0), ) - logger.info("Sending navigation goal to: (2.0, 2.0, 0.0)") - success = ros_nav.navigate_to(test_pose, timeout=30.0) - logger.info(f"Navigated successfully: {success}") + logger.info("Sending navigation goal to: (10.0, 10.0, 0.0)") + ros_nav.set_goal(test_pose) + time.sleep(5) + + logger.info("Cancelling goal after 5 seconds...") + cancelled = ros_nav.cancel_goal() + logger.info(f"Goal cancelled: {cancelled}") try: logger.info("\nNavBot running. Press Ctrl+C to stop.") @@ -64,9 +55,6 @@ def main() -> None: logger.info("\nShutting down...") ros_nav.stop() - if rclpy.ok(): # type: ignore[attr-defined] - rclpy.shutdown() - if __name__ == "__main__": main() diff --git a/dimos/navigation/rosnav.py b/dimos/navigation/rosnav.py index 88fa7985eb..f3e8907bd9 100644 --- a/dimos/navigation/rosnav.py +++ b/dimos/navigation/rosnav.py @@ -15,7 +15,7 @@ """ NavBot class for navigation-related functionality. -Encapsulates ROS bridge and topic remapping for Unitree robots. +Encapsulates ROS transport and topic remapping for Unitree robots. """ from collections.abc import Generator @@ -24,40 +24,25 @@ import threading import time -from geometry_msgs.msg import ( # type: ignore[attr-defined] - PointStamped as ROSPointStamped, - PoseStamped as ROSPoseStamped, - TwistStamped as ROSTwistStamped, -) -from nav_msgs.msg import Path as ROSPath # type: ignore[attr-defined] -import rclpy -from rclpy.node import Node from reactivex import operators as ops from reactivex.subject import Subject -from sensor_msgs.msg import ( # type: ignore[attr-defined] - Joy as ROSJoy, - PointCloud2 as ROSPointCloud2, -) -from std_msgs.msg import ( # type: ignore[attr-defined] - Bool as ROSBool, - Int8 as ROSInt8, -) -from tf2_msgs.msg import TFMessage as ROSTFMessage # type: ignore[attr-defined] from dimos import spec from dimos.agents import Reducer, Stream, skill # type: ignore[attr-defined] -from dimos.core import DimosCluster, In, LCMTransport, Module, Out, pSHMTransport, rpc +from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core.module import ModuleConfig +from dimos.core.transport import ROSTransport from dimos.msgs.geometry_msgs import ( PoseStamped, Quaternion, Transform, Twist, + TwistStamped, Vector3, ) from dimos.msgs.nav_msgs import Path -from dimos.msgs.sensor_msgs import PointCloud2 -from dimos.msgs.std_msgs import Bool +from dimos.msgs.sensor_msgs import Joy, PointCloud2 +from dimos.msgs.std_msgs import Bool, Int8 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.navigation.base import NavigationInterface, NavigationState from dimos.utils.logging_config import setup_logger @@ -81,6 +66,7 @@ class ROSNav( config: Config default_config = Config + # Existing ports (default LCM/pSHM transport) goal_req: In[PoseStamped] pointcloud: Out[PointCloud2] @@ -90,12 +76,26 @@ class ROSNav( path_active: Out[Path] cmd_vel: Out[Twist] + # ROS In ports (receiving from ROS topics via ROSTransport) + ros_goal_reached: In[Bool] + ros_cmd_vel: In[TwistStamped] + ros_way_point: In[PoseStamped] + ros_registered_scan: In[PointCloud2] + ros_global_pointcloud: In[PointCloud2] + ros_path: In[Path] + ros_tf: In[TFMessage] + + # ROS Out ports (publishing to ROS topics via ROSTransport) + ros_goal_pose: Out[PoseStamped] + ros_cancel_goal: Out[Bool] + ros_soft_stop: Out[Int8] + ros_joy: Out[Joy] + # Using RxPY Subjects for reactive data flow instead of storing state _local_pointcloud_subject: Subject # type: ignore[type-arg] _global_pointcloud_subject: Subject # type: ignore[type-arg] _current_position_running: bool = False - _spin_thread: threading.Thread | None = None _goal_reach: bool | None = None # Navigation state tracking for NavigationInterface @@ -117,39 +117,7 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] self._navigation_state = NavigationState.IDLE self._goal_reached = False - if not rclpy.ok(): # type: ignore[attr-defined] - rclpy.init() - - self._node = Node("navigation_module") - - # ROS2 Publishers - self.goal_pose_pub = self._node.create_publisher(ROSPoseStamped, "/goal_pose", 10) - self.cancel_goal_pub = self._node.create_publisher(ROSBool, "/cancel_goal", 10) - self.soft_stop_pub = self._node.create_publisher(ROSInt8, "/stop", 10) - self.joy_pub = self._node.create_publisher(ROSJoy, "/joy", 10) - - # ROS2 Subscribers - self.goal_reached_sub = self._node.create_subscription( - ROSBool, "/goal_reached", self._on_ros_goal_reached, 10 - ) - self.cmd_vel_sub = self._node.create_subscription( - ROSTwistStamped, "/cmd_vel", self._on_ros_cmd_vel, 10 - ) - self.goal_waypoint_sub = self._node.create_subscription( - ROSPointStamped, "/way_point", self._on_ros_goal_waypoint, 10 - ) - self.registered_scan_sub = self._node.create_subscription( - ROSPointCloud2, "/registered_scan", self._on_ros_registered_scan, 10 - ) - - self.global_pointcloud_sub = self._node.create_subscription( - ROSPointCloud2, "/terrain_map_ext", self._on_ros_global_pointcloud, 10 - ) - - self.path_sub = self._node.create_subscription(ROSPath, "/path", self._on_ros_path, 10) - self.tf_sub = self._node.create_subscription(ROSTFMessage, "/tf", self._on_ros_tf, 10) - - logger.info("NavigationModule initialized with ROS2 node") + logger.info("NavigationModule initialized") @rpc def start(self) -> None: @@ -157,8 +125,7 @@ def start(self) -> None: self._disposables.add( self._local_pointcloud_subject.pipe( - ops.sample(1.0 / self.config.local_pointcloud_freq), # Sample at desired frequency - ops.map(lambda msg: PointCloud2.from_ros_msg(msg)), # type: ignore[arg-type] + ops.sample(1.0 / self.config.local_pointcloud_freq), ).subscribe( on_next=self.pointcloud.publish, on_error=lambda e: logger.error(f"Lidar stream error: {e}"), @@ -167,64 +134,49 @@ def start(self) -> None: self._disposables.add( self._global_pointcloud_subject.pipe( - ops.sample(1.0 / self.config.global_pointcloud_freq), # Sample at desired frequency - ops.map(lambda msg: PointCloud2.from_ros_msg(msg)), # type: ignore[arg-type] + ops.sample(1.0 / self.config.global_pointcloud_freq), ).subscribe( on_next=self.global_pointcloud.publish, on_error=lambda e: logger.error(f"Map stream error: {e}"), ) ) - # Create and start the spin thread for ROS2 node spinning - self._spin_thread = threading.Thread( - target=self._spin_node, daemon=True, name="ROS2SpinThread" - ) - self._spin_thread.start() + # Subscribe to ROS In ports + self.ros_goal_reached.subscribe(self._on_ros_goal_reached) + self.ros_cmd_vel.subscribe(self._on_ros_cmd_vel) + self.ros_way_point.subscribe(self._on_ros_goal_waypoint) + self.ros_registered_scan.subscribe(self._on_ros_registered_scan) + self.ros_global_pointcloud.subscribe(self._on_ros_global_pointcloud) + self.ros_path.subscribe(self._on_ros_path) + self.ros_tf.subscribe(self._on_ros_tf) self.goal_req.subscribe(self._on_goal_pose) - logger.info("NavigationModule started with ROS2 spinning and RxPY streams") - - def _spin_node(self) -> None: - while self._running and rclpy.ok(): # type: ignore[attr-defined] - try: - rclpy.spin_once(self._node, timeout_sec=0.1) - except Exception as e: - if self._running: - logger.error(f"ROS2 spin error: {e}") + logger.info("NavigationModule started with ROS transport and RxPY streams") - def _on_ros_goal_reached(self, msg: ROSBool) -> None: + def _on_ros_goal_reached(self, msg: Bool) -> None: self._goal_reach = msg.data if msg.data: with self._state_lock: self._goal_reached = True self._navigation_state = NavigationState.IDLE - def _on_ros_goal_waypoint(self, msg: ROSPointStamped) -> None: - dimos_pose = PoseStamped( - ts=time.time(), - frame_id=msg.header.frame_id, - position=Vector3(msg.point.x, msg.point.y, msg.point.z), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - self.goal_active.publish(dimos_pose) + def _on_ros_goal_waypoint(self, msg: PoseStamped) -> None: + self.goal_active.publish(msg) - def _on_ros_cmd_vel(self, msg: ROSTwistStamped) -> None: - self.cmd_vel.publish(Twist.from_ros_msg(msg.twist)) + def _on_ros_cmd_vel(self, msg: TwistStamped) -> None: + self.cmd_vel.publish(Twist(linear=msg.linear, angular=msg.angular)) - def _on_ros_registered_scan(self, msg: ROSPointCloud2) -> None: + def _on_ros_registered_scan(self, msg: PointCloud2) -> None: self._local_pointcloud_subject.on_next(msg) - def _on_ros_global_pointcloud(self, msg: ROSPointCloud2) -> None: + def _on_ros_global_pointcloud(self, msg: PointCloud2) -> None: self._global_pointcloud_subject.on_next(msg) - def _on_ros_path(self, msg: ROSPath) -> None: - dimos_path = Path.from_ros_msg(msg) - dimos_path.frame_id = "base_link" - self.path_active.publish(dimos_path) - - def _on_ros_tf(self, msg: ROSTFMessage) -> None: - ros_tf = TFMessage.from_ros_msg(msg) + def _on_ros_path(self, msg: Path) -> None: + msg.frame_id = "base_link" + self.path_active.publish(msg) + def _on_ros_tf(self, msg: TFMessage) -> None: map_to_world_tf = Transform( translation=Vector3(0.0, 0.0, 0.0), rotation=euler_to_quaternion(Vector3(0.0, 0.0, 0.0)), @@ -236,7 +188,7 @@ def _on_ros_tf(self, msg: ROSTFMessage) -> None: self.tf.publish( self.config.sensor_to_base_link_transform.now(), map_to_world_tf, - *ros_tf.transforms, + *msg.transforms, ) def _on_goal_pose(self, msg: PoseStamped) -> None: @@ -247,31 +199,11 @@ def _on_cancel_goal(self, msg: Bool) -> None: self.stop() def _set_autonomy_mode(self) -> None: - joy_msg = ROSJoy() # type: ignore[no-untyped-call] - joy_msg.axes = [ - 0.0, # axis 0 - 0.0, # axis 1 - -1.0, # axis 2 - 0.0, # axis 3 - 1.0, # axis 4 - 1.0, # axis 5 - 0.0, # axis 6 - 0.0, # axis 7 - ] - joy_msg.buttons = [ - 0, # button 0 - 0, # button 1 - 0, # button 2 - 0, # button 3 - 0, # button 4 - 0, # button 5 - 0, # button 6 - 1, # button 7 - controls autonomy mode - 0, # button 8 - 0, # button 9 - 0, # button 10 - ] - self.joy_pub.publish(joy_msg) + joy_msg = Joy( + axes=[0.0, 0.0, -1.0, 0.0, 1.0, 1.0, 0.0, 0.0], + buttons=[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], + ) + self.ros_joy.publish(joy_msg) logger.info("Setting autonomy mode via Joy message") @skill(stream=Stream.passive, reducer=Reducer.latest) # type: ignore[arg-type] @@ -347,19 +279,14 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool: self._set_autonomy_mode() # Enable soft stop (0 = enable) - soft_stop_msg = ROSInt8() # type: ignore[no-untyped-call] - soft_stop_msg.data = 0 - self.soft_stop_pub.publish(soft_stop_msg) - - ros_pose = pose.to_ros_msg() - self.goal_pose_pub.publish(ros_pose) + self.ros_soft_stop.publish(Int8(data=0)) + self.ros_goal_pose.publish(pose) # Wait for goal to be reached start_time = time.time() while time.time() - start_time < timeout: if self._goal_reach is not None: - soft_stop_msg.data = 2 - self.soft_stop_pub.publish(soft_stop_msg) + self.ros_soft_stop.publish(Int8(data=2)) return self._goal_reach time.sleep(0.1) @@ -377,13 +304,8 @@ def stop_navigation(self) -> bool: """ logger.info("Stopping navigation") - cancel_msg = ROSBool() # type: ignore[no-untyped-call] - cancel_msg.data = True - self.cancel_goal_pub.publish(cancel_msg) - - soft_stop_msg = ROSInt8() # type: ignore[no-untyped-call] - soft_stop_msg.data = 2 - self.soft_stop_pub.publish(soft_stop_msg) + self.ros_cancel_goal.publish(Bool(data=True)) + self.ros_soft_stop.publish(Int8(data=2)) with self._state_lock: self._navigation_state = NavigationState.IDLE @@ -463,12 +385,6 @@ def stop(self) -> None: self._local_pointcloud_subject.on_completed() self._global_pointcloud_subject.on_completed() - if self._spin_thread and self._spin_thread.is_alive(): - self._spin_thread.join(timeout=1.0) - - if hasattr(self, "_node") and self._node: - self._node.destroy_node() # type: ignore[no-untyped-call] - except Exception as e: logger.error(f"Error during shutdown: {e}") finally: @@ -481,13 +397,29 @@ def stop(self) -> None: def deploy(dimos: DimosCluster): # type: ignore[no-untyped-def] nav = dimos.deploy(ROSNav) # type: ignore[attr-defined] - nav.pointcloud.transport = pSHMTransport("/lidar") - nav.global_pointcloud.transport = pSHMTransport("/map") + # Existing ports on LCM transports + nav.pointcloud.transport = LCMTransport("/lidar", PointCloud2) + nav.global_pointcloud.transport = LCMTransport("/map", PointCloud2) nav.goal_req.transport = LCMTransport("/goal_req", PoseStamped) nav.goal_active.transport = LCMTransport("/goal_active", PoseStamped) nav.path_active.transport = LCMTransport("/path_active", Path) nav.cmd_vel.transport = LCMTransport("/cmd_vel", Twist) + # ROS In transports (receiving from ROS navigation stack) + nav.ros_goal_reached.transport = ROSTransport("/goal_reached", Bool) + nav.ros_cmd_vel.transport = ROSTransport("/cmd_vel", TwistStamped) + nav.ros_way_point.transport = ROSTransport("/way_point", PoseStamped) + nav.ros_registered_scan.transport = ROSTransport("/registered_scan", PointCloud2) + nav.ros_global_pointcloud.transport = ROSTransport("/terrain_map_ext", PointCloud2) + nav.ros_path.transport = ROSTransport("/path", Path) + nav.ros_tf.transport = ROSTransport("/tf", TFMessage) + + # ROS Out transports (publishing to ROS navigation stack) + nav.ros_goal_pose.transport = ROSTransport("/goal_pose", PoseStamped) + nav.ros_cancel_goal.transport = ROSTransport("/cancel_goal", Bool) + nav.ros_soft_stop.transport = ROSTransport("/stop", Int8) + nav.ros_joy.transport = ROSTransport("/joy", Joy) + nav.start() return nav diff --git a/dimos/robot/ros_bridge.py b/dimos/robot/ros_bridge.py deleted file mode 100644 index 6e3e78542c..0000000000 --- a/dimos/robot/ros_bridge.py +++ /dev/null @@ -1,210 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import Enum -import logging -import threading -from typing import Any - -try: - import rclpy - from rclpy.executors import SingleThreadedExecutor - from rclpy.node import Node - from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, - ) -except ImportError: - rclpy = None # type: ignore[assignment] - SingleThreadedExecutor = None # type: ignore[assignment, misc] - Node = None # type: ignore[assignment, misc] - QoSProfile = None # type: ignore[assignment, misc] - QoSReliabilityPolicy = None # type: ignore[assignment, misc] - QoSHistoryPolicy = None # type: ignore[assignment, misc] - QoSDurabilityPolicy = None # type: ignore[assignment, misc] - -from dimos.core.resource import Resource -from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic -from dimos.utils.logging_config import setup_logger - -logger = setup_logger(level=logging.INFO) - - -class BridgeDirection(Enum): - """Direction of message bridging.""" - - ROS_TO_DIMOS = "ros_to_dimos" - DIMOS_TO_ROS = "dimos_to_ros" - - -class ROSBridge(Resource): - """Unidirectional bridge between ROS and DIMOS for message passing.""" - - def __init__(self, node_name: str = "dimos_ros_bridge") -> None: - """Initialize the ROS-DIMOS bridge. - - Args: - node_name: Name for the ROS node (default: "dimos_ros_bridge") - """ - if not rclpy.ok(): # type: ignore[attr-defined] - rclpy.init() - - self.node = Node(node_name) - self.lcm = LCM() - self.lcm.start() - - self._executor = SingleThreadedExecutor() - self._executor.add_node(self.node) - - self._spin_thread = threading.Thread(target=self._ros_spin, daemon=True) - self._spin_thread.start() # TODO: don't forget to shut it down - - self._bridges: dict[str, dict[str, Any]] = {} - - self._qos = QoSProfile( # type: ignore[no-untyped-call] - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.VOLATILE, - depth=10, - ) - - logger.info(f"ROSBridge initialized with node name: {node_name}") - - def start(self) -> None: - pass - - def stop(self) -> None: - """Shutdown the bridge and clean up resources.""" - self._executor.shutdown() - self.node.destroy_node() # type: ignore[no-untyped-call] - - if rclpy.ok(): # type: ignore[attr-defined] - rclpy.shutdown() - - logger.info("ROSBridge shutdown complete") - - def _ros_spin(self) -> None: - """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, - remap_topic: str | None = None, - ) -> 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) - 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(dimos_topic_name, dimos_type) - - ros_subscription = None - ros_publisher = None - dimos_subscription = None - - if direction == BridgeDirection.ROS_TO_DIMOS: - - def ros_callback(msg) -> None: # type: ignore[no-untyped-def] - self._ros_to_dimos(msg, dimos_topic, dimos_type, topic_name) - - ros_subscription = self.node.create_subscription( - ros_type, ros_topic_name, ros_callback, self._qos - ) - 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, ros_topic_name, self._qos) - - def dimos_callback(msg, _topic) -> None: # type: ignore[no-untyped-def] - self._dimos_to_ros(msg, ros_publisher, topic_name) - - dimos_subscription = self.lcm.subscribe(dimos_topic, dimos_callback) # type: ignore[arg-type] - logger.info(f" DIMOS → ROS: Subscribing to DIMOS topic {dimos_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, - "ros_topic_name": ros_topic_name, - "dimos_topic_name": dimos_topic_name, - } - - direction_str = { - BridgeDirection.ROS_TO_DIMOS: "ROS → DIMOS", - BridgeDirection.DIMOS_TO_ROS: "DIMOS → ROS", - }[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( - 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) # type: ignore[attr-defined] - self.lcm.publish(dimos_topic, dimos_msg) # type: ignore[arg-type] - - def _dimos_to_ros(self, dimos_msg: Any, ros_publisher, _topic_name: str) -> None: # type: ignore[no-untyped-def] - """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) diff --git a/dimos/robot/test_ros_bridge.py b/dimos/robot/test_ros_bridge.py deleted file mode 100644 index e31be24176..0000000000 --- a/dimos/robot/test_ros_bridge.py +++ /dev/null @@ -1,442 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import threading -import time -import unittest - -import numpy as np -import pytest - -try: - from geometry_msgs.msg import TransformStamped, TwistStamped as ROSTwistStamped - import rclpy - from rclpy.node import Node - from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, PointField - from tf2_msgs.msg import TFMessage as ROSTFMessage -except ImportError: - rclpy = None - Node = None - ROSTwistStamped = None - ROSPointCloud2 = None - PointField = None - ROSTFMessage = None - TransformStamped = None - -from dimos.msgs.geometry_msgs import TwistStamped -from dimos.msgs.sensor_msgs import PointCloud2 -from dimos.msgs.tf2_msgs import TFMessage -from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic -from dimos.robot.ros_bridge import BridgeDirection, ROSBridge - - -@pytest.mark.ros -class TestROSBridge(unittest.TestCase): - """Test suite for ROS-DIMOS bridge.""" - - def setUp(self) -> None: - """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() - - # Create test bridge - self.bridge = ROSBridge("test_ros_bridge") - - # Create test node for publishing/subscribing - self.test_node = Node("test_node") - - # Track received messages - self.ros_messages = [] - self.dimos_messages = [] - self.message_timestamps = {"ros": [], "dimos": []} - - def tearDown(self) -> None: - """Clean up test fixtures.""" - self.test_node.destroy_node() - self.bridge.stop() - if rclpy.ok(): - rclpy.try_shutdown() - - def test_ros_to_dimos_twist(self) -> None: - """Test ROS TwistStamped to DIMOS conversion and transmission.""" - # Set up bridge - self.bridge.add_topic( - "/test_twist", TwistStamped, ROSTwistStamped, BridgeDirection.ROS_TO_DIMOS - ) - - # Subscribe to DIMOS side - lcm = LCM() - lcm.start() - topic = Topic("/test_twist", TwistStamped) - - def dimos_callback(msg, _topic) -> None: - 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) - - lcm.stop() - - def test_dimos_to_ros_twist(self) -> None: - """Test DIMOS TwistStamped to ROS conversion and transmission.""" - # Set up bridge - self.bridge.add_topic( - "/test_twist_reverse", TwistStamped, ROSTwistStamped, BridgeDirection.DIMOS_TO_ROS - ) - - # Subscribe to ROS side - def ros_callback(msg) -> None: - 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) -> None: - """Test that message frequencies are preserved through the bridge.""" - # Set up bridge - self.bridge.add_topic( - "/test_freq", TwistStamped, ROSTwistStamped, BridgeDirection.ROS_TO_DIMOS - ) - - # Subscribe to DIMOS side - lcm = LCM() - lcm.start() - topic = Topic("/test_freq", TwistStamped) - - receive_times = [] - - def dimos_callback(_msg, _topic) -> None: - 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", - ) - - lcm.stop() - - def test_pointcloud_conversion(self) -> None: - """Test PointCloud2 message conversion with numpy optimization.""" - # Set up bridge - self.bridge.add_topic( - "/test_cloud", PointCloud2, ROSPointCloud2, BridgeDirection.ROS_TO_DIMOS - ) - - # Subscribe to DIMOS side - lcm = LCM() - lcm.start() - topic = Topic("/test_cloud", PointCloud2) - - received_cloud = [] - - def dimos_callback(msg, _topic) -> None: - 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) - - lcm.stop() - - def test_tf_high_frequency(self) -> None: - """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) -> None: - received_tfs.append(msg) - receive_times.append(time.time()) - - lcm.subscribe(topic, dimos_callback) - - # Publish TF at high frequency (100Hz) - ros_pub = self.test_node.create_publisher(ROSTFMessage, "/test_tf", 100) - - target_freq = 100 # Hz - period = 1.0 / target_freq - num_messages = 100 # 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", - ) - - # 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", - ) - - lcm.stop() - - def test_bidirectional_bridge(self) -> None: - """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 - ) - - dimos_received = [] - ros_received = [] - - # 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)) - - # ROS subscriber - self.test_node.create_subscription( - ROSTwistStamped, "/dimos_to_ros", lambda msg: ros_received.append(msg), 10 - ) - - # Set up publishers - ros_pub = self.test_node.create_publisher(ROSTwistStamped, "/ros_to_dimos", 10) - topic_d2r = Topic("/dimos_to_ros", TwistStamped) - - # Keep track of whether threads should continue - stop_spinning = threading.Event() - - # Spin the test node in background to receive messages - def spin_test_node() -> None: - 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() - - # Send messages in both directions simultaneously - def send_ros_messages() -> None: - 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 - - def send_dimos_messages() -> None: - 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 - - # Run both senders in parallel - ros_thread = threading.Thread(target=send_ros_messages) - dimos_thread = threading.Thread(target=send_dimos_messages) - - ros_thread.start() - dimos_thread.start() - - ros_thread.join() - dimos_thread.join() - - # 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") - - # Verify message integrity - for i, msg in enumerate(dimos_received[:45]): - self.assertAlmostEqual(msg.linear.x, float(i), places=5) - - for i, msg in enumerate(ros_received[:45]): - self.assertAlmostEqual(msg.twist.linear.y, float(i * 2), places=5) - - -if __name__ == "__main__": - unittest.main() diff --git a/dimos/robot/unitree_webrtc/unitree_b1/connection.py b/dimos/robot/unitree_webrtc/unitree_b1/connection.py index f0cb5317e6..bae4bc0844 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/connection.py +++ b/dimos/robot/unitree_webrtc/unitree_b1/connection.py @@ -28,6 +28,7 @@ from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.std_msgs import Int32 +from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.logging_config import setup_logger from .b1_command import B1Command @@ -52,11 +53,17 @@ class B1ConnectionModule(Module): internally converts to B1Command format, and sends UDP packets at 50Hz. """ - cmd_vel: In[TwistStamped] # Timestamped velocity commands from ROS - mode_cmd: In[Int32] # Mode changes - odom_in: In[Odometry] # External odometry from ROS SLAM/lidar + # LCM ports (inter-module communication) + cmd_vel: In[TwistStamped] + mode_cmd: In[Int32] + odom_in: In[Odometry] - odom_pose: Out[PoseStamped] # Converted pose for internal use + odom_pose: Out[PoseStamped] + + # ROS In ports (receiving from ROS via ROSTransport) + ros_cmd_vel: In[TwistStamped] + ros_odom_in: In[Odometry] + ros_tf: In[TFMessage] def __init__( # type: ignore[no-untyped-def] self, ip: str = "192.168.12.1", port: int = 9090, test_mode: bool = False, *args, **kwargs @@ -111,6 +118,17 @@ def start(self) -> None: unsub = self.odom_in.subscribe(self._publish_odom_pose) self._disposables.add(Disposable(unsub)) + # Subscribe to ROS In ports + if self.ros_cmd_vel: + unsub = self.ros_cmd_vel.subscribe(self.handle_twist_stamped) + self._disposables.add(Disposable(unsub)) + if self.ros_odom_in: + unsub = self.ros_odom_in.subscribe(self._publish_odom_pose) + self._disposables.add(Disposable(unsub)) + if self.ros_tf: + unsub = self.ros_tf.subscribe(self._on_ros_tf) + self._disposables.add(Disposable(unsub)) + # Start threads self.running = True self.watchdog_running = True @@ -284,6 +302,10 @@ def _publish_odom_pose(self, msg: Odometry) -> None: ) self.odom_pose.publish(pose_stamped) + def _on_ros_tf(self, msg: TFMessage) -> None: + """Forward ROS TF messages to the module's TF tree.""" + self.tf.publish(*msg.transforms) + def _watchdog_loop(self) -> None: """Single watchdog thread that monitors command freshness.""" while self.watchdog_running: diff --git a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py index ff608c2b1f..9302e8f66f 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py +++ b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py @@ -26,12 +26,12 @@ from dimos import core from dimos.core.module_coordinator import ModuleCoordinator from dimos.core.resource import Resource +from dimos.core.transport import ROSTransport from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.std_msgs import Int32 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.robot.robot import Robot -from dimos.robot.ros_bridge import BridgeDirection, ROSBridge from dimos.robot.unitree_webrtc.unitree_b1.connection import ( B1ConnectionModule, MockB1ConnectionModule, @@ -40,21 +40,6 @@ from dimos.types.robot_capabilities import RobotCapability from dimos.utils.logging_config import setup_logger -# Handle ROS imports for environments where ROS is not available like CI -try: - from geometry_msgs.msg import ( # type: ignore[attr-defined] - TwistStamped as ROSTwistStamped, - ) - from nav_msgs.msg import Odometry as ROSOdometry # type: ignore[attr-defined] - from tf2_msgs.msg import TFMessage as ROSTFMessage # type: ignore[attr-defined] - - ROS_AVAILABLE = True -except ImportError: - ROSTwistStamped = None # type: ignore[assignment, misc] - ROSOdometry = None # type: ignore[assignment, misc] - ROSTFMessage = None # type: ignore[assignment, misc] - ROS_AVAILABLE = False - logger = setup_logger(level=logging.INFO) @@ -74,7 +59,6 @@ def __init__( output_dir: str | None = None, skill_library: SkillLibrary | None = None, enable_joystick: bool = False, - enable_ros_bridge: bool = True, test_mode: bool = False, ) -> None: """Initialize the B1 robot. @@ -85,7 +69,6 @@ def __init__( output_dir: Directory for saving outputs skill_library: Skill library instance (optional) enable_joystick: Enable pygame joystick control module - enable_ros_bridge: Enable ROS bridge for external control test_mode: Test mode - print commands instead of sending UDP """ super().__init__() @@ -93,12 +76,10 @@ def __init__( self.port = port self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output") self.enable_joystick = enable_joystick - self.enable_ros_bridge = enable_ros_bridge self.test_mode = test_mode self.capabilities = [RobotCapability.LOCOMOTION] self.connection = None self.joystick = None - self.ros_bridge = None self._dimos = ModuleCoordinator(n=2) os.makedirs(self.output_dir, exist_ok=True) @@ -122,6 +103,11 @@ def start(self) -> None: self.connection.odom_in.transport = core.LCMTransport("/state_estimation", Odometry) # type: ignore[attr-defined] self.connection.odom_pose.transport = core.LCMTransport("/odom", PoseStamped) # type: ignore[attr-defined] + # Configure ROS transports for connection + self.connection.ros_cmd_vel.transport = ROSTransport("/cmd_vel", TwistStamped) # type: ignore[attr-defined] + self.connection.ros_odom_in.transport = ROSTransport("/state_estimation", Odometry) # type: ignore[attr-defined] + self.connection.ros_tf.transport = ROSTransport("/tf", TFMessage) # type: ignore[attr-defined] + # Deploy joystick move_vel control if self.enable_joystick: from dimos.robot.unitree_webrtc.unitree_b1.joystick_module import JoystickModule @@ -136,43 +122,12 @@ def start(self) -> None: self.connection.idle() # type: ignore[attr-defined] # Start in IDLE mode for safety logger.info("B1 started in IDLE mode (safety)") - # Deploy ROS bridge if enabled (matching G1 pattern) - if self.enable_ros_bridge: - self._deploy_ros_bridge() - logger.info(f"UnitreeB1 initialized - UDP control to {self.ip}:{self.port}") if self.enable_joystick: logger.info("Pygame joystick module enabled for testing") - if self.enable_ros_bridge: - logger.info("ROS bridge enabled for external control") def stop(self) -> None: self._dimos.stop() - if self.ros_bridge: - self.ros_bridge.stop() - - def _deploy_ros_bridge(self) -> None: - """Deploy and configure ROS bridge (matching G1 implementation).""" - self.ros_bridge = ROSBridge("b1_ros_bridge") # type: ignore[assignment] - - # Add /cmd_vel topic from ROS to DIMOS - self.ros_bridge.add_topic( # type: ignore[attr-defined] - "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS - ) - - # Add /state_estimation topic from ROS to DIMOS (external odometry) - self.ros_bridge.add_topic( # type: ignore[attr-defined] - "/state_estimation", Odometry, ROSOdometry, direction=BridgeDirection.ROS_TO_DIMOS - ) - - # Add /tf topic from ROS to DIMOS - self.ros_bridge.add_topic( # type: ignore[attr-defined] - "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS - ) - - self.ros_bridge.start() # type: ignore[attr-defined] - - logger.info("ROS bridge deployed: /cmd_vel, /state_estimation, /tf (ROS → DIMOS)") # Robot control methods (standard interface) def move(self, twist_stamped: TwistStamped, duration: float = 0.0) -> None: @@ -212,10 +167,6 @@ def main() -> None: parser.add_argument("--ip", default="192.168.12.1", help="Robot IP address") parser.add_argument("--port", type=int, default=9090, help="UDP port") parser.add_argument("--joystick", action="store_true", help="Enable pygame joystick control") - parser.add_argument("--ros-bridge", action="store_true", default=True, help="Enable ROS bridge") - parser.add_argument( - "--no-ros-bridge", dest="ros_bridge", action="store_false", help="Disable ROS bridge" - ) parser.add_argument("--output-dir", help="Output directory for logs/data") parser.add_argument( "--test", action="store_true", help="Test mode - print commands instead of UDP" @@ -228,7 +179,6 @@ def main() -> None: port=args.port, output_dir=args.output_dir, enable_joystick=args.joystick, - enable_ros_bridge=args.ros_bridge, test_mode=args.test, ) @@ -256,10 +206,7 @@ def main() -> None: # Manual control example print("\nB1 Robot ready for commands") print("Use robot.idle(), robot.stand(), robot.walk() to change modes") - if args.ros_bridge: - print("ROS bridge active - listening for /cmd_vel and /state_estimation") - else: - print("Use robot.move(TwistStamped(...)) to send velocity commands") + print("ROS topics active via ROSTransport: /cmd_vel, /state_estimation, /tf") print("Press Ctrl+C to exit\n") import time