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);