aruco_pose: add basic tests

aruco_pose/package.xml

@@ -27,6 +27,7 @@
   <depend>geometry_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>rostest</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/test/basic.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+from pytest import approx
+import rospy
+import rostest
+
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+def to_dict(msg):
+    return {s: getattr(msg, s) for s in msg.__slots__}
+
+
+class TestArucoDetect(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_detect', anonymous=True)
+
+    def test_markers(self):
+        markers = rospy.wait_for_message('/aruco_detect/markers', MarkerArray, timeout=5)
+        assert len(markers.markers) == 4
+        assert markers.header.frame_id == 'main_camera_optical'
+
+        assert markers.markers[0].id == 2
+        assert markers.markers[0].pose.pose.position.x == approx(0.36706567854)
+        assert markers.markers[0].pose.pose.position.y == approx(0.290484516644)
+        assert markers.markers[0].pose.pose.position.z == approx(2.18787602301)
+        assert markers.markers[0].pose.pose.orientation.x == approx(0.993997406299)
+        assert markers.markers[0].pose.pose.orientation.y == approx(-0.00532003481626)
+        assert markers.markers[0].pose.pose.orientation.z == approx(-0.107390951553)
+        assert markers.markers[0].pose.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[3].id == 3
+        assert markers.markers[3].pose.pose.position.x == approx(-0.1805169666)
+        assert markers.markers[3].pose.pose.position.y == approx(-0.200697302327)
+        assert markers.markers[3].pose.pose.position.z == approx(0.585767514823)
+        assert markers.markers[3].pose.pose.orientation.x == approx(-0.961738074009)
+        assert markers.markers[3].pose.pose.orientation.y == approx(-0.0375180244707)
+        assert markers.markers[3].pose.pose.orientation.z == approx(-0.0115387773672)
+        assert markers.markers[3].pose.pose.orientation.w == approx(0.271144115664)
+        assert markers.markers[3].c1.x == approx(129.557723999)
+        assert markers.markers[3].c1.y == approx(49.557723999)
+        assert markers.markers[3].c2.x == approx(223.442276001)
+        assert markers.markers[3].c2.y == approx(49.557723999)
+        assert markers.markers[3].c3.x == approx(223.442276001)
+        assert markers.markers[3].c3.y == approx(143.442276001)
+        assert markers.markers[3].c4.x == approx(129.557723999)
+        assert markers.markers[3].c4.y == approx(143.442276001)
+
+        assert markers.markers[1].id == 1
+        assert markers.markers[2].id == 4
+
+    def test_visualization(self):
+        vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+
+    # def test_transforms(self):
+    #     pass
+
+rostest.rosrun('aruco_pose', 'test_aruco_detect', TestArucoDetect, sys.argv)

aruco_pose/test/basic.test

@@ -0,0 +1,26 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/map.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="length_override/3" value="0.1"/>
+        <param name="estimate_poses" value="true"/>
+    </node>
+
+    <!-- <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
+        <remap from="markers" to="aruco_detect/markers"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+    </node> -->
+
+    <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>
+</launch>

aruco_pose/test/camera_info.yaml

@@ -0,0 +1,21 @@
+# some random camera calibration for testing
+image_width: 640
+image_height: 480
+camera_name: test_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [643.229809, 0.000000, 356.811289, 0.000000, 644.318982, 299.150067, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.422907, 0.202567, 0.000781, 0.000447, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [567.010193, 0.000000, 366.677428, 0.000000, 0.000000, 594.591980, 307.043423, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]