aruco_map: map parser improvements (#118)

* aruco_map: Improve parser

* aruco_map: Use marker id for map visualization

* aruco_pose: Add parser pass test

* aruco_map: Code style

* aruco_pose: Add more test cases

* aruco_map: Better message handling

* aruco_map: Be more informative about bad lines

* aruco_map: Add failure mode tests

* aruco_map: Be less strict about map contents

* aruco_pose: Restructure tests

* aruco_map: Don't use marker id in visualization

* aruco_map: Check for marker uniqueness

* aruco_pose: Use board data to reject duplicate markers

* aruco_pose/test: Spelling fixes

aruco_pose/CMakeLists.txt

@@ -216,4 +216,7 @@ target_link_libraries(aruco_pose
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
 endif()

aruco_pose/src/aruco_map.cpp

@@ -36,6 +36,7 @@
 #include <sensor_msgs/Image.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <algorithm>
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
@@ -270,10 +271,50 @@ publish_debug:
 
 			std::istringstream s(line);
 
-			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
-				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
 				continue;
 			}
+
+			if (first == '#') {
+				ROS_DEBUG("aruco_map: Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				ROS_ERROR("aruco_map: Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}
 
@@ -339,6 +380,19 @@ publish_debug:
 	void addMarker(int id, double length, double x, double y, double z,
 				   double yaw, double pitch, double roll)
 	{
+		// Check whether the id is in range for current dictionary
+		int num_markers = board_->dictionary->bytesList.rows;
+		if (num_markers <= id) {
+			ROS_ERROR("aruco_map: Marker id %d is not in dictionary; current dictionary contains %d markers. "
+			          "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
+					  id, num_markers);
+			return;
+		}
+		// Check if marker is already in the board
+		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
+			ROS_ERROR("aruco_map: Marker id %d is already in the map", id);
+			return;
+		}
 		// Create transform
 		tf::Quaternion q;
 		q.setRPY(roll, pitch, yaw);

aruco_pose/test/basic.txt

@@ -1,7 +1,11 @@
+# Default markers
 1	0.33	0	0	0	0	0	0
 2	0.33	1	0	0	0	0	0
 3	0.33	0	1	0	0	0	0
 4	0.33	1	1	0	0	0	0
+# Marker with non-zero yaw rotation
 10	0.5	0.5	2	0	1.2	0	0
+# Marker with non-zero pitch and roll rotation
 11	0.2	0.5	0.5	0	0.0	-1	1
+# Marker with yaw, pitch and roll rotation
 12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

aruco_pose/test/test_node_failure.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+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
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)
+
+    def test_node_failure(self):
+        try:
+            markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+            did_post_message = True
+        except rospy.exceptions.ROSException:
+            did_post_message = False
+        self.assertFalse(did_post_message)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map_fail_dict" pkg="aruco_pose" type="test_node_failure.py"/>
+</launch>

aruco_pose/test/test_node_failure.txt

@@ -0,0 +1,4 @@
+# Any garbage data (pretty much anything apart from a comment starting with a hash starting
+# with a hash sign or a number) will be interpreted as garbage data; the node should fail
+# after reading it.
+// Don't try to put your comments this way, it will kill your node!

aruco_pose/test/test_parser_empty_map.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+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
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)
+
+
+    def test_node_failure(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        self.assertEquals(len(markers.markers), 0)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_empty_map.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_empty_map.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map_incomplete" pkg="aruco_pose" type="test_parser_empty_map.py"/>
+</launch>

aruco_pose/test/test_parser_empty_map.txt

@@ -0,0 +1,6 @@
+# Failing markers: Not enough parameters to add a marker
+1
+2 1
+3 0.5 1
+# Failing markers: Marker IDs outside of dictionary range
+1001 1 1 0

aruco_pose/test/test_parser_pass.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+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
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_pass', anonymous=True)
+
+    def test_markers(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        
+        self.assertEqual(len(markers.markers), 6)
+# FIXME: visual marker id is not ArUco marker id
+#        self.assertEqual(markers.markers[0].id, 1)
+#        self.assertEqual(markers.markers[1].id, 2)
+#        self.assertEqual(markers.markers[2].id, 3)
+#        self.assertEqual(markers.markers[3].id, 4)
+
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[1].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.z, 1, places=7)
+        
+        self.assertAlmostEqual(markers.markers[2].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.z, 0.5, places=7)
+        
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[4].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.y, 0.5, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[5].pose.position.x, 2.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.y, 0.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.z, 0, places=7)
+
+        self.assertAlmostEqual(markers.markers[0].scale.x, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[0].scale.y, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.x, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.y, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.x, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.y, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.x, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.y, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.x, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.y, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.x, 0.35, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.y, 0.35, places=7)
+
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')
+
+    # def test_transforms(self):
+    #     pass
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_pass.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_pass.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map" pkg="aruco_pose" type="test_parser_pass.py"/>
+</launch>

aruco_pose/test/test_parser_pass.txt

@@ -0,0 +1,23 @@
+# Parser test #1 - correct file
+# 1. Commentary test
+#Commentary text without space after pound sign
+# Commentary text with space after pound sign
+    # Commentary text with spaces before pound sign
+	# Commentary text with tab before pound sign
+			# Text with tabs before pound sign
+# Empty line test:
+
+# All-whitespace line test:
+		    
+# 2. Default coordinates test
+# Fully filled marker description, tab-delimited:
+1	0.33	0	0	0	0	0	0
+# Fully filled marker description, space-delimited:
+2 0.225 1 1 1 0 0 0
+# Default roll, pitch, yaw angles
+3 0.45 1 0 0.5
+# Default roll, pitch, yaw, z
+4 0.15 0 1
+# Inline commentary
+5 0.25 1 0.5# Comment straight after digit
+6 0.35 2.2 0.2 # Comment with a space after digit