mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-26 21:19:35 +00:00
62 lines
2.4 KiB
Python
Executable File
62 lines
2.4 KiB
Python
Executable File
import rospy
|
|
import pytest
|
|
|
|
from sensor_msgs.msg import Image
|
|
from aruco_pose.msg import MarkerArray
|
|
from visualization_msgs.msg import MarkerArray as VisMarkerArray
|
|
|
|
|
|
@pytest.fixture
|
|
def node():
|
|
return rospy.init_node('aruco_pose_test', anonymous=True)
|
|
|
|
def approx(expected):
|
|
return pytest.approx(expected, abs=1e-4) # compare floats more roughly
|
|
|
|
def test_markers(node):
|
|
markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
|
|
assert len(markers.markers) == 6
|
|
|
|
assert markers.markers[0].pose.position.x == approx(0)
|
|
assert markers.markers[0].pose.position.y == approx(0)
|
|
assert markers.markers[0].pose.position.z == approx(0)
|
|
|
|
assert markers.markers[1].pose.position.x == approx(1)
|
|
assert markers.markers[1].pose.position.y == approx(1)
|
|
assert markers.markers[1].pose.position.z == approx(1)
|
|
|
|
assert markers.markers[2].pose.position.x == approx(1)
|
|
assert markers.markers[2].pose.position.y == approx(0)
|
|
assert markers.markers[2].pose.position.z == approx(0.5)
|
|
|
|
assert markers.markers[3].pose.position.x == approx(0)
|
|
assert markers.markers[3].pose.position.y == approx(1)
|
|
assert markers.markers[3].pose.position.z == approx(0)
|
|
|
|
assert markers.markers[4].pose.position.x == approx(1)
|
|
assert markers.markers[4].pose.position.y == approx(0.5)
|
|
assert markers.markers[4].pose.position.z == approx(0)
|
|
|
|
assert markers.markers[5].pose.position.x == approx(2.2)
|
|
assert markers.markers[5].pose.position.y == approx(0.2)
|
|
assert markers.markers[5].pose.position.z == approx(0)
|
|
|
|
assert markers.markers[0].scale.x == approx(0.33)
|
|
assert markers.markers[0].scale.y == approx(0.33)
|
|
assert markers.markers[1].scale.x == approx(0.225)
|
|
assert markers.markers[1].scale.y == approx(0.225)
|
|
assert markers.markers[2].scale.x == approx(0.45)
|
|
assert markers.markers[2].scale.y == approx(0.45)
|
|
assert markers.markers[3].scale.x == approx(0.15)
|
|
assert markers.markers[3].scale.y == approx(0.15)
|
|
assert markers.markers[4].scale.x == approx(0.25)
|
|
assert markers.markers[4].scale.y == approx(0.25)
|
|
assert markers.markers[5].scale.x == approx(0.35)
|
|
assert markers.markers[5].scale.y == approx(0.35)
|
|
|
|
def test_map_image(node):
|
|
img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
|
|
assert img.width == 2000
|
|
assert img.height == 2000
|
|
assert img.encoding in ('mono8', 'rgb8')
|