diff --git a/apriltag_ros/config/detector_config.yaml b/apriltag_ros/config/detector_config.yaml new file mode 100644 index 0000000..f24c937 --- /dev/null +++ b/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 + \ 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/launch/detector.launch b/apriltag_ros/launch/detector.launch index 4d08c2f..0bebc7c 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..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)); @@ -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(); 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");