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 @@
apriltag
image_transport
cv_bridge
+ yaml-cpp
+ ament_index_cpp
ament_lint_auto
ament_cmake_clang_format
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
#endif
+#include
#include
#include
#include
@@ -18,6 +19,11 @@
// apriltag
#include "tag_functions.hpp"
#include
+#include
+#include
+
+
+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;