ROS2+OpenCV综合应用--9. AprilTag标签码识别

1. 简介

Apriltag是一种常用于机器视觉中的编码标志，它具有较高的识别率和可靠性，可用于各种任务，包括增强现实、机器人和相机校准。

2. 启动

2.1 程序启动前的准备

本次apriltag标签码使用的是TAG36H11格式，出厂已配套相关标签码，并贴在积木块上，需要将积木块拿出来放置到摄像头画面下识别。

2.2 程序说明

程序启动后，摄像头捕获到图像，将标签码放入摄像头画面，系统会识别并框出标签码的四个顶点，并显示标签码的ID号。

2.3 程序启动

打开一个终端输入以下指令进入docker，

./docker_ros2.sh

出现以下界面就是进入docker成功

在docker终端输入以下命令启动程序

ros2 run yahboomcar_apriltag apriltag_identify

3. 源码

#!/usr/bin/env python3
# encoding: utf-8
import cv2 as cv
import time
from dt_apriltags import Detector
from yahboomcar_apriltag.vutils import draw_tags
import logging
import yahboomcar_apriltag.logger_config as logger_config
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Float32MultiArray
​
class ApriltagIdentify(Node):
    def __init__(self):
        super().__init__('apriltag_identify_node')
        logger_config.setup_logger()
        self.image = None
        self.at_detector = Detector(searchpath=['apriltags'],
                                    families='tag36h11',
                                    nthreads=8,
                                    quad_decimate=2.0,
                                    quad_sigma=0.0,
                                    refine_edges=1,
                                    decode_sharpening=0.25,
                                    debug=0)
        
        # AprilTag positions
        self.publisher_ = self.create_publisher(Float32MultiArray, 'apriltag_positions', 10)
        # AprilTag ID
        self.single_tag_id_publisher_ = self.create_publisher(String, 'single_apriltag_id', 10)
​
    def getApriltagPosMsg(self, image):
        self.image = cv.resize(image, (640, 480))
        msg = Float32MultiArray()
        try:
            tags = self.at_detector.detect(cv.cvtColor(
                self.image, cv.COLOR_BGR2GRAY), False, None, 0.025)
            tags = sorted(tags, key=lambda tag: tag.tag_id)
            if len(tags) > 0:
                for tag in tags:
                    point_x = tag.center[0]
                    point_y = tag.center[1]
                    (a, b) = (round(((point_x - 320) / 4000), 5),
                            round(((480 - point_y) / 3000) * 0.7+0.15, 5))
                    msg.data.extend([tag.tag_id, a, b])
​
                self.image = draw_tags(self.image, tags, corners_color=(
                    0, 0, 255), center_color=(0, 255, 0))
        except Exception as e:
            logging.info('getApriltagPosMsg e = {}'.format(e))
        
        return self.image, msg
​
    def getSingleApriltagID(self, image):
        self.image = cv.resize(image, (640, 480))
        tagId = ""
        try:
            tags = self.at_detector.detect(cv.cvtColor(
                self.image, cv.COLOR_BGR2GRAY), False, None, 0.025)
            tags = sorted(tags, key=lambda tag: tag.tag_id)
            if len(tags) == 1:
                tagId = str(tags[0].tag_id)
                self.image = draw_tags(self.image, tags, corners_color=(
                    0, 0, 255), center_color=(0, 255, 0))
        except Exception as e:
            logging.info('getSingleApriltagID e = {}'.format(e))
        
        return self.image, tagId
​
    def detect_and_publish(self):
        capture = cv.VideoCapture(0)
        capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)
        capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
​
        t_start = time.time()
        m_fps = 0
        try:
            while capture.isOpened():
                action = cv.waitKey(10) & 0xFF
                if action == ord('q'):
                    break
                ret, img = capture.read()
                img, data = self.getApriltagPosMsg(img)
                
                # AprilTag positions
                self.publisher_.publish(data)
                
                # AprilTag ID
                _, single_tag_id = self.getSingleApriltagID(img)
                if single_tag_id:
                    single_tag_msg = String()
                    single_tag_msg.data = single_tag_id
                    self.single_tag_id_publisher_.publish(single_tag_msg)
​
                m_fps += 1
                fps = m_fps / (time.time() - t_start)
                if (time.time() - t_start) >= 2000:
                    t_start = time.time()
                    m_fps = fps
                text = "FPS: " + str(int(fps))
                cv.putText(img, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
                cv.imshow('img', img)
        except Exception as e:
            logging.error('Exception occurred: {}'.format(e))
        finally:
            capture.release()
            cv.destroyAllWindows()
​
def main(args=None):
    rclpy.init(args=args)
    apriltag_identify = ApriltagIdentify()
    try:
        apriltag_identify.detect_and_publish()
    except KeyboardInterrupt:
        pass
​
if __name__ == '__main__':
    main()