From fe41e22c791c0d5014f3bafe448741a5c6662218 Mon Sep 17 00:00:00 2001
From: Brent Yi <yibrenth@gmail.com>
Date: Wed, 22 Jun 2016 15:04:19 -0700
Subject: [PATCH] Added parameter for selecting tag family

---
 apriltags_ros/launch/example.launch     |  7 +++++--
 apriltags_ros/src/apriltag_detector.cpp | 27 +++++++++++++++++++++++--
 2 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/apriltags_ros/launch/example.launch b/apriltags_ros/launch/example.launch
index 02d2afd..94dfde5 100644
--- a/apriltags_ros/launch/example.launch
+++ b/apriltags_ros/launch/example.launch
@@ -3,10 +3,13 @@
     <!-- Remap topic required by the node to custom topics -->
     <remap from="image_rect" to="image_rect" />
     <remap from="camera_info" to="camera_info" />
-    
+
     <!-- Optional: Subscribe to the compressed stream-->
     <param name="image_transport" type="str" value="compressed" />
-    
+
+    <!-- Select the tag family: 16h5, 25h7, 25h9, 36h9, or 36h11(default) -->
+    <param name="tag_family" type="str" value="36h11" />
+
     <!-- Describe the tags -->
     <rosparam param="tag_descriptions">[
       {id: 0, size: 0.163513},
diff --git a/apriltags_ros/src/apriltag_detector.cpp b/apriltags_ros/src/apriltag_detector.cpp
index 960670f..88ab334 100644
--- a/apriltags_ros/src/apriltag_detector.cpp
+++ b/apriltags_ros/src/apriltag_detector.cpp
@@ -32,8 +32,31 @@ AprilTagDetector::AprilTagDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh): i
     sensor_frame_id_ = "";
   }
 
-  AprilTags::TagCodes tag_codes = AprilTags::tagCodes36h11;
-  tag_detector_= boost::shared_ptr<AprilTags::TagDetector>(new AprilTags::TagDetector(tag_codes));
+  std::string tag_family;
+  pnh.param<std::string>("tag_family", tag_family, "36h11");
+
+  const AprilTags::TagCodes* tag_codes;
+  if(tag_family == "16h5"){
+    tag_codes = &AprilTags::tagCodes16h5;
+  }
+  else if(tag_family == "25h7"){
+    tag_codes = &AprilTags::tagCodes25h7;
+  }
+  else if(tag_family == "25h9"){
+    tag_codes = &AprilTags::tagCodes25h9;
+  }
+  else if(tag_family == "36h9"){
+    tag_codes = &AprilTags::tagCodes36h9;
+  }
+  else if(tag_family == "36h11"){
+    tag_codes = &AprilTags::tagCodes36h11;
+  }
+  else{
+    ROS_WARN("Invalid tag family specified; defaulting to 36h11");
+    tag_codes = &AprilTags::tagCodes36h11;
+  }
+
+  tag_detector_= boost::shared_ptr<AprilTags::TagDetector>(new AprilTags::TagDetector(*tag_codes));
   image_sub_ = it_.subscribeCamera("image_rect", 1, &AprilTagDetector::imageCb, this);
   image_pub_ = it_.advertise("tag_detections_image", 1);
   detections_pub_ = nh.advertise<AprilTagDetectionArray>("tag_detections", 1);