Added parameter for selecting tag family

RIVeR-Lab · Jun 22, 2016 · fe41e22 · fe41e22
1 parent a27e67a
commit fe41e22
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 4 deletions.
diff --git 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
@@ -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);