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 @@
-
+
-
+
+
+
+
[
{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(new AprilTags::TagDetector(tag_codes));
+ std::string tag_family;
+ pnh.param("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(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("tag_detections", 1);