From b542851b24281d1196b0491c1f8d3dd905a2a0c7 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Wed, 26 Jun 2019 23:00:49 +0300
Subject: [PATCH] aruco_pose: fix crashing the nodelet if markers on the map
 are to small

---
 aruco_pose/CMakeLists.txt     |  1 +
 aruco_pose/src/draw.cpp       |  1 +
 aruco_pose/test/largemap.py   | 28 ++++++++++++++++++++++++++++
 aruco_pose/test/largemap.test | 13 +++++++++++++
 aruco_pose/test/largemap.txt  | 11 +++++++++++
 5 files changed, 54 insertions(+)
 create mode 100755 aruco_pose/test/largemap.py
 create mode 100644 aruco_pose/test/largemap.test
 create mode 100644 aruco_pose/test/largemap.txt

diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt
index 8ed96fe5..9b69dc55 100644
--- a/aruco_pose/CMakeLists.txt
+++ b/aruco_pose/CMakeLists.txt
@@ -219,4 +219,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_parser_pass.test)
   add_rostest(test/test_parser_empty_map.test)
   add_rostest(test/test_node_failure.test)
+  add_rostest(test/largemap.test)
 endif()
diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp
index 70754b5c..3077ca48 100644
--- a/aruco_pose/src/draw.cpp
+++ b/aruco_pose/src/draw.cpp
@@ -87,6 +87,7 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
+		side = std::max(side, 10);
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
 
diff --git a/aruco_pose/test/largemap.py b/aruco_pose/test/largemap.py
new file mode 100755
index 00000000..4ed31f55
--- /dev/null
+++ b/aruco_pose/test/largemap.py
@@ -0,0 +1,28 @@
+#!/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 TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_largemap', anonymous=True)
+
+    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_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)
diff --git a/aruco_pose/test/largemap.test b/aruco_pose/test/largemap.test
new file mode 100644
index 00000000..4025af62
--- /dev/null
+++ b/aruco_pose/test/largemap.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/largemap.txt"/>
+    </node>
+
+    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+</launch>
diff --git a/aruco_pose/test/largemap.txt b/aruco_pose/test/largemap.txt
new file mode 100644
index 00000000..184ca2b4
--- /dev/null
+++ b/aruco_pose/test/largemap.txt
@@ -0,0 +1,11 @@
+0 0.2 0 0 0 0 0 0
+1 0.2 10 0 0 0 0 0
+2 0.2 20 0 0 0 0 0
+3 0.2 30 0 0 0 0 0
+4 0.2 40 0 0 0 0 0
+5 0.2 50 0 0 0 0 0
+6 0.2 60 0 0 0 0 0
+7 0.2 70 0 0 0 0 0
+8 0.2 80 0 0 0 0 0
+9 0.2 90 0 0 0 0 0
+10 0.2 100 0 0 0 0 0