Merge pull request #538 from dimensionalOS/ros_transform_test

Validating transforms with ros examples

Former-commit-id: 4dfc722 [formerly e192231]
Former-commit-id: 1a6a287

Author: spomichter

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."""

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

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 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"""