diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index 5ac90d48..8ed96fe5 100644 --- a/aruco_pose/CMakeLists.txt +++ b/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() diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 5859b516..350a573c 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -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); diff --git a/aruco_pose/test/basic.txt b/aruco_pose/test/basic.txt index d9517388..c990a427 100644 --- a/aruco_pose/test/basic.txt +++ b/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 diff --git a/aruco_pose/test/test_node_failure.py b/aruco_pose/test/test_node_failure.py new file mode 100755 index 00000000..5a669884 --- /dev/null +++ b/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) diff --git a/aruco_pose/test/test_node_failure.test b/aruco_pose/test/test_node_failure.test new file mode 100644 index 00000000..e2e24af9 --- /dev/null +++ b/aruco_pose/test/test_node_failure.test @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/aruco_pose/test/test_node_failure.txt b/aruco_pose/test/test_node_failure.txt new file mode 100644 index 00000000..49a6d134 --- /dev/null +++ b/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! \ No newline at end of file diff --git a/aruco_pose/test/test_parser_empty_map.py b/aruco_pose/test/test_parser_empty_map.py new file mode 100755 index 00000000..ad562abd --- /dev/null +++ b/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) diff --git a/aruco_pose/test/test_parser_empty_map.test b/aruco_pose/test/test_parser_empty_map.test new file mode 100644 index 00000000..17d17796 --- /dev/null +++ b/aruco_pose/test/test_parser_empty_map.test @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/aruco_pose/test/test_parser_empty_map.txt b/aruco_pose/test/test_parser_empty_map.txt new file mode 100644 index 00000000..4090d655 --- /dev/null +++ b/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 diff --git a/aruco_pose/test/test_parser_pass.py b/aruco_pose/test/test_parser_pass.py new file mode 100755 index 00000000..43d7667f --- /dev/null +++ b/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) diff --git a/aruco_pose/test/test_parser_pass.test b/aruco_pose/test/test_parser_pass.test new file mode 100644 index 00000000..a92cdde6 --- /dev/null +++ b/aruco_pose/test/test_parser_pass.test @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/aruco_pose/test/test_parser_pass.txt b/aruco_pose/test/test_parser_pass.txt new file mode 100644 index 00000000..71a6bf35 --- /dev/null +++ b/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 \ No newline at end of file