diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index 287905573d..a47c58337c 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -53,7 +53,7 @@ def __repr__(self) -> str: return f"Transform(translation={self.translation!r}, rotation={self.rotation!r})" def __str__(self) -> str: - return f"Transform:\n Translation: {self.translation}\n Rotation: {self.rotation}" + return f"Transform:\n {self.frame_id} -> {self.child_frame_id} Translation: {self.translation}\n Rotation: {self.rotation}" def __eq__(self, other) -> bool: """Check if two transforms are equal.""" diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index 866082b967..e069305bca 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import time import numpy as np @@ -331,6 +332,38 @@ def test_transform_from_pose(): assert transform.child_frame_id == "base_link" # passed as first argument +# validating results from example @ +# https://foxglove.dev/blog/understanding-ros-transforms +def test_transform_from_ros(): + """Test converting PoseStamped to Transform""" + test_time = time.time() + pose_stamped = PoseStamped( + ts=test_time, + frame_id="base_link", + position=Vector3(1, -1, 0), + orientation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), + ) + transform_base_link_to_arm = Transform.from_pose("arm_base_link", pose_stamped) + + transform_arm_to_end = Transform.from_pose( + "end", + PoseStamped( + ts=test_time, + frame_id="arm_base_link", + position=Vector3(1, 1, 0), + orientation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), + ), + ) + + print(transform_base_link_to_arm) + print(transform_arm_to_end) + + end_effector_global_pose = transform_base_link_to_arm + transform_arm_to_end + + assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) + assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) + + def test_transform_from_pose_stamped(): """Test converting PoseStamped to Transform""" # Create a PoseStamped with position, orientation, timestamp and frame diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index 5c9489c87d..8bbd53d714 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -14,13 +14,45 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import time +import pytest + from dimos.core import TF from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Vector3 from dimos.protocol.tf.tf import MultiTBuffer, TBuffer +# from https://foxglove.dev/blog/understanding-ros-transforms +def test_tf_ros_example(): + tf = TF() + + base_link_to_arm = Transform( + translation=Vector3(1.0, -1.0, 0.0), + rotation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), + frame_id="base_link", + child_frame_id="arm", + ts=time.time(), + ) + + arm_to_end = Transform( + translation=Vector3(1.0, 1.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation + frame_id="arm", + child_frame_id="end_effector", + ts=time.time(), + ) + + tf.publish(base_link_to_arm, arm_to_end) + time.sleep(0.2) + + end_effector_global_pose = tf.get("base_link", "end_effector") + + assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) + assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) + + def test_tf_main(): """Test TF broadcasting and querying between two TF instances. If you run foxglove-bridge this will show up in the UI"""