Install GeographicLibs datasets in Clover CMakeLists.txt

.github/workflows/build.yml

@@ -15,14 +15,6 @@ jobs:
   #       run: |
   #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
   noetic:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: Native Noetic build
-        run: |
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
-
-  noetic-raw:
     runs-on: ubuntu-latest
     container: ros:noetic-ros-base
     defaults:

aruco_pose/CMakeLists.txt

@@ -251,4 +251,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
   add_rostest(test/crash_opencv.test)
+  add_rostest(test/duplicate.test)
 endif()

aruco_pose/src/aruco_detect.cpp

@@ -187,6 +187,8 @@ private:
 
 			array_.markers.reserve(ids.size());
 			aruco_pose::Marker marker;
+			vector<geometry_msgs::TransformStamped> transforms;
+			transforms.reserve(ids.size());
 			geometry_msgs::TransformStamped transform;
 			transform.header.stamp = msg->header.stamp;
 			transform.header.frame_id = msg->header.frame_id;
@@ -204,20 +206,33 @@ private:
 						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
 					}
 
-					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
 						// check if such static transform is in the map
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
-							transform.transform.rotation = marker.pose.orientation;
-							fillTranslation(transform.transform.translation, tvecs[i]);
-							br_->sendTransform(transform);
+							// check if a markers with that id is already added
+							bool send = true;
+							for (auto &t : transforms) {
+								if (t.child_frame_id == transform.child_frame_id) {
+									send = false;
+									break;
+								}
+							}
+							if (send) {
+								transform.transform.rotation = marker.pose.orientation;
+								fillTranslation(transform.transform.translation, tvecs[i]);
+								transforms.push_back(transform);
+							}
 						}
 					}
 				}
 				array_.markers.push_back(marker);
 			}
+
+			if (send_tf_) {
+				br_->sendTransform(transforms);
+			}
 		}
 
 		markers_pub_.publish(array_);

aruco_pose/test/duplicate.py

@@ -0,0 +1,8 @@
+import pytest
+import subprocess
+
+def test_no_tf_repeated_data():
+    # `/rosout` acts weirdly inside rostest, so using a subprocess
+    cmd = """python -c 'import rospy, tf; rospy.init_node("foo"); listener = tf.TransformListener(); rospy.sleep(2)'"""
+    output = str(subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT))
+    assert 'TF_REPEATED_DATA' not in output, 'TF_REPEATED_DATA was logged on duplicate markers'

aruco_pose/test/duplicate.test

@@ -0,0 +1,21 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.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" required="true"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+        <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="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
+    </node>
+
+	<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

clover/CMakeLists.txt

@@ -46,6 +46,14 @@ find_package(OpenCV ${_opencv_version} REQUIRED
     imgproc
 )
 
+# Download GeographicLib datasets
+message(STATUS "Downloading GeographicLib datasets to ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}")
+add_custom_target(download_geographiclib_datasets ALL
+  COMMAND geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib egm96-5
+  COMMAND geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib egm96
+  COMMAND geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib emm2015
+)
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 

clover/launch/mavros.launch

@@ -44,6 +44,9 @@
         <!-- remap rangefinder -->
         <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
+        <!-- path to find GoegraphicLib datasets -->
+        <env name="GEOGRAPHICLIB_DATA" value="$(eval env('CMAKE_PREFIX_PATH').split(':')[0] + '/share/GeographicLib')"/>
+
         <rosparam param="plugin_whitelist">
             - altitude
             - command

clover/test/basic.test

@@ -4,6 +4,7 @@
     <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
         <param name="fcu_url" value="udp://@127.0.1:14557"/>
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
+        <env name="GEOGRAPHICLIB_DATA" value="$(eval env('CMAKE_PREFIX_PATH').split(':')[0] + '/share/GeographicLib')"/>
     </node>
 
     <node name="visualization" pkg="mavros_extras" type="visualization" required="true">