From 06b48ca9fd926992c9874704c73d808335831c8e Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Mon, 10 Jun 2024 23:00:23 +0200
Subject: [PATCH] load tag config

---
 CMakeLists.txt       |  7 +++++++
 package.xml          |  2 ++
 src/AprilTagNode.cpp | 12 ++++++++++++
 3 files changed, 21 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 47d991a..db25bbf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,6 +43,7 @@ find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(Threads REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core calib3d)
 find_package(apriltag 3.2 REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 if(cv_bridge_VERSION VERSION_GREATER_EQUAL 3.3.0)
     add_compile_definitions(cv_bridge_HPP)
@@ -110,9 +111,11 @@ ament_target_dependencies(AprilTagNode
     tf2_ros
     image_transport
     cv_bridge
+    ament_index_cpp
 )
 target_link_libraries(AprilTagNode
     apriltag::apriltag
+    yaml-cpp::yaml-cpp
     tags
     pose_estimation
 )
@@ -128,6 +131,10 @@ install(TARGETS AprilTagNode
     LIBRARY DESTINATION lib
 )
 
+install(DIRECTORY tag
+    DESTINATION share/${PROJECT_NAME}
+)
+
 install(DIRECTORY cfg
     DESTINATION share/${PROJECT_NAME}
 )
diff --git a/package.xml b/package.xml
index dcc4df5..a0023d9 100644
--- a/package.xml
+++ b/package.xml
@@ -19,6 +19,8 @@
   <depend>apriltag</depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
+  <depend>yaml-cpp</depend>
+  <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp
index 5f646a3..4a1afa6 100644
--- a/src/AprilTagNode.cpp
+++ b/src/AprilTagNode.cpp
@@ -7,6 +7,7 @@
 #else
 #include <cv_bridge/cv_bridge.h>
 #endif
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <image_transport/camera_subscriber.hpp>
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -18,6 +19,11 @@
 // apriltag
 #include "tag_functions.hpp"
 #include <apriltag.h>
+#include <filesystem>
+#include <yaml-cpp/yaml.h>
+
+
+namespace fs = std::filesystem;
 
 
 #define IF(N, V) \
@@ -145,6 +151,12 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
         for(size_t i = 0; i < ids.size(); i++) { tag_sizes[ids[i]] = sizes[i]; }
     }
 
+    // tag_family
+    const fs::path ros_pkg_share_path =
+        ament_index_cpp::get_package_share_directory("apriltag_ros");
+    const fs::path tag_config_path = ros_pkg_share_path / "tag" / (tag_family + ".yaml");
+    const YAML::Node tag_config = YAML::LoadFile(tag_config_path);
+
     if(tag_fun.count(tag_family)) {
         tf = tag_fun.at(tag_family).first();
         tf_destructor = tag_fun.at(tag_family).second;