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;