basic.py

Go to the documentation of this file.

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
from visualization_msgs.msg import MarkerArray as VisMarkerArray


@pytest.fixture
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

def test_markers(node):
    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
    assert len(markers.markers) == 5
    assert markers.header.frame_id == 'main_camera_optical'

    assert markers.markers[0].id == 2
    assert markers.markers[0].length == approx(0.33)
    assert markers.markers[0].pose.position.x == approx(0.36706567854)
    assert markers.markers[0].pose.position.y == approx(0.290484516644)
    assert markers.markers[0].pose.position.z == approx(2.18787602301)
    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
    assert markers.markers[0].c1.x == approx(415.557739258)
    assert markers.markers[0].c1.y == approx(335.557739258)
    assert markers.markers[0].c2.x == approx(509.442260742)
    assert markers.markers[0].c2.y == approx(335.557739258)
    assert markers.markers[0].c3.x == approx(509.442260742)
    assert markers.markers[0].c3.y == approx(429.442260742)
    assert markers.markers[0].c4.x == approx(415.557739258)
    assert markers.markers[0].c4.y == approx(429.442260742)

    assert markers.markers[4].id == 3
    assert markers.markers[4].length == approx(0.1)
    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
    assert markers.markers[4].pose.position.z == approx(0.585767514823)
    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
    assert markers.markers[4].c1.x == approx(129.557723999)
    assert markers.markers[4].c1.y == approx(49.557723999)
    assert markers.markers[4].c2.x == approx(223.442276001)
    assert markers.markers[4].c2.y == approx(49.557723999)
    assert markers.markers[4].c3.x == approx(223.442276001)
    assert markers.markers[4].c3.y == approx(143.442276001)
    assert markers.markers[4].c4.x == approx(129.557723999)
    assert markers.markers[4].c4.y == approx(143.442276001)

    assert markers.markers[1].id == 1
    assert markers.markers[1].length == approx(0.33)
    assert markers.markers[3].id == 4
    assert markers.markers[3].length == approx(0.33)

    assert markers.markers[2].id == 100
    assert markers.markers[2].length == approx(0.33)
    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
    assert markers.markers[2].pose.position.z == approx(2.94611272668)
    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
    assert markers.markers[2].c1.x == approx(52.557723999)
    assert markers.markers[2].c1.y == approx(205.557723999)
    assert markers.markers[2].c2.x == approx(113.442276001)
    assert markers.markers[2].c2.y == approx(205.557723999)
    assert markers.markers[2].c3.x == approx(113.442276001)
    assert markers.markers[2].c3.y == approx(265.442260742)
    assert markers.markers[2].c4.x == approx(52.557723999)
    assert markers.markers[2].c4.y == approx(265.442260742)

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_map_markers_frames(node, tf_buffer):
    stamp = rospy.get_rostime()
    timeout = rospy.Duration(5)

    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
    assert marker_1.transform.translation.x == approx(0)
    assert marker_1.transform.translation.y == approx(0)
    assert marker_1.transform.translation.z == approx(0)

    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
    assert marker_4.transform.translation.x == approx(1)
    assert marker_4.transform.translation.y == approx(1)
    assert marker_4.transform.translation.z == approx(0)

    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
    assert marker_12.transform.translation.x == approx(0.2)
    assert marker_12.transform.translation.y == approx(0.5)
    assert marker_12.transform.translation.z == approx(0)

def test_visualization(node):
    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
    assert len(vis.markers) == 11

def test_debug(node):
    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
    assert img.width == 640
    assert img.height == 480
    assert img.header.frame_id == 'main_camera_optical'

def test_map(node):
    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
    assert pose.header.frame_id == 'main_camera_optical'
    assert pose.pose.pose.position.x == approx(-0.629167753342)
    assert pose.pose.pose.position.y == approx(0.293822650809)
    assert pose.pose.pose.position.z == approx(2.12641343155)
    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
    assert pose.pose.pose.orientation.w == approx(0.0482143590507)

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')

def test_map_markers(node):
    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
    assert markers.markers[0].id == 1
    assert markers.markers[1].id == 2
    assert markers.markers[2].id == 3
    assert markers.markers[3].id == 4
    assert markers.markers[4].id == 10
    assert markers.markers[5].id == 11
    assert markers.markers[6].id == 12

    assert markers.markers[0].pose.position.x == 0
    assert markers.markers[0].pose.position.y == 0
    assert markers.markers[0].pose.position.z == 0
    assert markers.markers[0].pose.orientation.x == 0
    assert markers.markers[0].pose.orientation.y == 0
    assert markers.markers[0].pose.orientation.z == 0
    assert markers.markers[0].pose.orientation.w == 1
    assert markers.markers[0].length == approx(0.33)

    assert markers.markers[1].pose.position.x == 1
    assert markers.markers[1].pose.position.y == 0
    assert markers.markers[1].pose.position.z == 0
    assert markers.markers[1].pose.orientation.x == 0
    assert markers.markers[1].pose.orientation.y == 0
    assert markers.markers[1].pose.orientation.z == 0
    assert markers.markers[1].pose.orientation.w == 1
    assert markers.markers[1].length == approx(0.33)

    assert markers.markers[2].pose.position.x == 0
    assert markers.markers[2].pose.position.y == 1
    assert markers.markers[2].pose.position.z == 0
    assert markers.markers[2].pose.orientation.x == 0
    assert markers.markers[2].pose.orientation.y == 0
    assert markers.markers[2].pose.orientation.z == 0
    assert markers.markers[2].pose.orientation.w == 1
    assert markers.markers[2].length == approx(0.33)

    assert markers.markers[3].pose.position.x == 1
    assert markers.markers[3].pose.position.y == 1
    assert markers.markers[3].pose.position.z == 0
    assert markers.markers[3].pose.orientation.x == 0
    assert markers.markers[3].pose.orientation.y == 0
    assert markers.markers[3].pose.orientation.z == 0
    assert markers.markers[3].pose.orientation.w == 1
    assert markers.markers[3].length == approx(0.33)

    assert markers.markers[4].pose.position.x == approx(0.5)
    assert markers.markers[4].pose.position.y == 2
    assert markers.markers[4].pose.position.z == 0
    assert markers.markers[4].pose.orientation.x == 0
    assert markers.markers[4].pose.orientation.y == 0
    assert markers.markers[4].pose.orientation.z == approx(0.5646424733950354)
    assert markers.markers[4].pose.orientation.w == approx(0.8253356149096783)
    assert markers.markers[4].length == approx(0.5)

def test_map_visualization(node):
    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

def test_map_debug(node):
    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)