Apriltag filtering

apriltag_ros/config/detector_config.yaml

@@ -0,0 +1,18 @@
+legal_tags:
+  - 1
+  - 2
+  - 3
+  - 4
+  - 5
+  - 6
+  - 7
+  - 8
+  - 9
+  - 10
+  - 11
+  - 12
+  - 13
+  - 14
+  - 15
+  - 16
+

apriltag_ros/include/apriltag_ros/apriltag_detector_node.h

@@ -32,6 +32,8 @@ class ApriltagDetectorNode {
   boost::mutex connect_mutex_;
   dr::Server<ConfigT> cfg_server_;
 
+  std::set<int> legal_tags_;
+
   ros::Publisher pub_tags_;
   it::Publisher pub_disp_;
 

apriltag_ros/launch/detector.launch

@@ -9,6 +9,7 @@
     <param name="family" type="int" value="0"/>
     <param name="type" type="int" value="0"/>
     <param name="black_border" type="int" value="$(arg border_size)"/>
+    <rosparam file="$(find apriltag_ros)/config/detector_config.yaml" command="load"/>
 
     <remap from="~image" to="$(arg image)"/>
     <remap from="~tags" to="tags"/>

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<int> _legal_tags_vec;
+  pnh_.param<std::vector<int>>("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));
 
@@ -31,18 +39,15 @@ void ApriltagDetectorNode::ImageCb(const ImageConstPtr &image_msg) {
 
   // Detect
   auto apriltags = detector_->Detect(gray);
-
+  
   // Publish apriltags
   auto apriltag_array_msg = boost::make_shared<ApriltagArrayStamped>();
   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};
+
   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  );
     }
@@ -64,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");