diff --git a/aruco_pose/test/basic.py b/aruco_pose/test/basic.py
index 6d3f29af..3be80838 100755
--- a/aruco_pose/test/basic.py
+++ b/aruco_pose/test/basic.py
@@ -1,6 +1,8 @@
import rospy
import pytest
+import tf2_ros
+import tf2_geometry_msgs
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import Image
from aruco_pose.msg import MarkerArray
@@ -11,6 +13,12 @@ from visualization_msgs.msg import MarkerArray as VisMarkerArray
def node():
return rospy.init_node('aruco_pose_test', anonymous=True)
+@pytest.fixture
+def tf_buffer():
+ buf = tf2_ros.Buffer()
+ tf2_ros.TransformListener(buf)
+ return buf
+
def approx(expected):
return pytest.approx(expected, abs=1e-4) # compare floats more roughly
@@ -60,6 +68,16 @@ def test_markers(node):
assert markers.markers[2].id == 4
assert markers.markers[2].length == approx(0.33)
+def test_markers_frames(node, tf_buffer):
+ marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
+ assert marker_2.transform.translation.x == approx(0.36706567854)
+ assert marker_2.transform.translation.y == approx(0.290484516644)
+ assert marker_2.transform.translation.z == approx(2.18787602301)
+ assert marker_2.transform.rotation.x == approx(0.993997406299)
+ assert marker_2.transform.rotation.y == approx(-0.00532003481626)
+ assert marker_2.transform.rotation.z == approx(-0.107390951553)
+ assert marker_2.transform.rotation.w == approx(0.0201999263402)
+
def test_visualization(node):
vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
assert len(vis.markers) == 9
diff --git a/aruco_pose/test/basic.test b/aruco_pose/test/basic.test
index 80722a2e..83bdd0c1 100644
--- a/aruco_pose/test/basic.test
+++ b/aruco_pose/test/basic.test
@@ -13,6 +13,7 @@
+