From 588e4a1f45ef7be373524522da35cb111735b153 Mon Sep 17 00:00:00 2001 From: EmmeMWSL Date: Thu, 14 Nov 2024 19:56:03 -0500 Subject: [PATCH 1/3] feat: add config for legal tags --- apriltag_ros/config/detector_config.yaml | 4 ++++ apriltag_ros/launch/detector.launch | 1 + apriltag_ros/src/apriltag_detector_node.cpp | 11 ++++++----- 3 files changed, 11 insertions(+), 5 deletions(-) create mode 100644 apriltag_ros/config/detector_config.yaml diff --git a/apriltag_ros/config/detector_config.yaml b/apriltag_ros/config/detector_config.yaml new file mode 100644 index 0000000..ac368ca --- /dev/null +++ b/apriltag_ros/config/detector_config.yaml @@ -0,0 +1,4 @@ +legal_tags: + - 1 + - 2 + - 3 \ No newline at end of file diff --git a/apriltag_ros/launch/detector.launch b/apriltag_ros/launch/detector.launch index 4d08c2f..0aac2d3 100644 --- a/apriltag_ros/launch/detector.launch +++ b/apriltag_ros/launch/detector.launch @@ -9,6 +9,7 @@ + diff --git a/apriltag_ros/src/apriltag_detector_node.cpp b/apriltag_ros/src/apriltag_detector_node.cpp index 7e1a3ef..f05433f 100644 --- a/apriltag_ros/src/apriltag_detector_node.cpp +++ b/apriltag_ros/src/apriltag_detector_node.cpp @@ -31,14 +31,15 @@ void ApriltagDetectorNode::ImageCb(const ImageConstPtr &image_msg) { // Detect auto apriltags = detector_->Detect(gray); - + // Publish apriltags auto apriltag_array_msg = boost::make_shared(); apriltag_array_msg->header = image_msg->header; - // this should be configurable but the idea is if they are adding more apriltags mid season we have more problems to worry about - // check if it is within range - // @TODO make this configurable - constexpr int LEGAL_TAGS[16] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; + + // Load config for legal tags + std::vector LEGAL_TAGS; + pnh_.param>("legal_tags", LEGAL_TAGS, {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}); + apriltag_ros::ApriltagVec filtered_tags; for (const auto &tag : apriltags) { // if tag id is found From 8fca01b64bd95eb0b241017ea60524af42c7b679 Mon Sep 17 00:00:00 2001 From: EmmeMWSL Date: Thu, 14 Nov 2024 20:04:04 -0500 Subject: [PATCH 2/3] fix: uncomment line that makes config feature work --- apriltag_ros/launch/detector.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apriltag_ros/launch/detector.launch b/apriltag_ros/launch/detector.launch index 0aac2d3..0bebc7c 100644 --- a/apriltag_ros/launch/detector.launch +++ b/apriltag_ros/launch/detector.launch @@ -9,7 +9,7 @@ - + From 31fe4f2657a4644d1711f26a0e64dd7f7e06cffc Mon Sep 17 00:00:00 2001 From: EmmeMWSL Date: Thu, 21 Nov 2024 19:36:14 -0500 Subject: [PATCH 3/3] refactor: store legal tag config as set --- apriltag_ros/config/detector_config.yaml | 16 +++++++++++++++- .../apriltag_ros/apriltag_detector_node.h | 2 ++ apriltag_ros/src/apriltag_detector_node.cpp | 15 ++++++++++----- 3 files changed, 27 insertions(+), 6 deletions(-) diff --git a/apriltag_ros/config/detector_config.yaml b/apriltag_ros/config/detector_config.yaml index ac368ca..f24c937 100644 --- a/apriltag_ros/config/detector_config.yaml +++ b/apriltag_ros/config/detector_config.yaml @@ -1,4 +1,18 @@ legal_tags: - 1 - 2 - - 3 \ No newline at end of file + - 3 + - 4 + - 5 + - 6 + - 7 + - 8 + - 9 + - 10 + - 11 + - 12 + - 13 + - 14 + - 15 + - 16 + \ No newline at end of file diff --git a/apriltag_ros/include/apriltag_ros/apriltag_detector_node.h b/apriltag_ros/include/apriltag_ros/apriltag_detector_node.h index a428b47..cc2587e 100644 --- a/apriltag_ros/include/apriltag_ros/apriltag_detector_node.h +++ b/apriltag_ros/include/apriltag_ros/apriltag_detector_node.h @@ -32,6 +32,8 @@ class ApriltagDetectorNode { boost::mutex connect_mutex_; dr::Server cfg_server_; + std::set legal_tags_; + ros::Publisher pub_tags_; it::Publisher pub_disp_; diff --git a/apriltag_ros/src/apriltag_detector_node.cpp b/apriltag_ros/src/apriltag_detector_node.cpp index f05433f..293ec83 100644 --- a/apriltag_ros/src/apriltag_detector_node.cpp +++ b/apriltag_ros/src/apriltag_detector_node.cpp @@ -14,6 +14,14 @@ using apriltag_msgs::ApriltagArrayStamped; ApriltagDetectorNode::ApriltagDetectorNode(const ros::NodeHandle &pnh) : pnh_(pnh), it_(pnh_), cfg_server_(pnh) { + + // Load config for legal apriltags + std::vector _legal_tags_vec; + pnh_.param>("legal_tags", _legal_tags_vec, {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}); + for (int legal_tag : _legal_tags_vec) { + legal_tags_.emplace(legal_tag); + } + cfg_server_.setCallback( boost::bind(&ApriltagDetectorNode::ConfigCb, this, _1, _2)); @@ -36,14 +44,10 @@ void ApriltagDetectorNode::ImageCb(const ImageConstPtr &image_msg) { auto apriltag_array_msg = boost::make_shared(); apriltag_array_msg->header = image_msg->header; - // Load config for legal tags - std::vector LEGAL_TAGS; - pnh_.param>("legal_tags", LEGAL_TAGS, {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}); - apriltag_ros::ApriltagVec filtered_tags; for (const auto &tag : apriltags) { // if tag id is found - if (std::find(std::begin(LEGAL_TAGS), std::end(LEGAL_TAGS), tag.id) != std::end(LEGAL_TAGS)) { + if (legal_tags_.count(tag.id) != 0) { filtered_tags.push_back(tag); // ROS_INFO_STREAM_THROTTLE(1, "Allowing tag with id " << tag.id ); } @@ -65,6 +69,7 @@ void ApriltagDetectorNode::ImageCb(const ImageConstPtr &image_msg) { } void ApriltagDetectorNode::ConfigCb(ConfigT &config, int level) { + if (level < 0) { ROS_INFO("%s: %s", pnh_.getNamespace().c_str(), "Initializing reconfigure server");