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 @@ +