ROS2+OpenCV综合应用--10. AprilTag标签码追踪

1. 简介

        apriltag标签码追踪是在apriltag标签码识别的基础上，增加了小车摄像头云台运动的功能，摄像头会保持标签码在视觉中间而运动，根据这一特性，从而实现标签码追踪功能。

2. 启动

2.1 程序启动前的准备

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

2.2 程序说明

        程序启动后，摄像头捕获到图像，将标签码放入摄像头画面，系统会识别并框出标签码的四个顶点，并显示标签码的ID号。然后缓慢移动积木块的位置，摄像头云台会跟着积木块移动。

注意：积木块移动时，标签码要对着摄像头，并且移动速度不可以太快，避免摄像头云台跟不上。

2.3 程序启动

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

./docker_ros2.sh

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

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

ros2 launch yahboomcar_apriltag apriltag_tracking.launch.py

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
​
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import yahboomcar_apriltag.fps as fps
import numpy as np
from yahboomcar_apriltag.vutils import draw_tags
from dt_apriltags import Detector
from yahboomcar_apriltag.PID import PositionalPID
from Raspbot_Lib import Raspbot
import math
​
class TagTrackingNode(Node):
    def __init__(self):
        super().__init__('tag_tracking_node')
        # 初始化 Raspbot 实例
        self.bot = Raspbot()
        self.bridge = CvBridge()
        self.xservo_pid = PositionalPID(0.6, 0.2, 0.01)  # PID控制器用于X轴
        self.yservo_pid = PositionalPID(0.8, 0.6, 0.01)  # PID控制器用于Y轴
        self.numx=self.numy=1
        target_servox = 90
        target_servoy = 25
        self.bot.Ctrl_Servo(1,target_servox)
        self.bot.Ctrl_Servo(2,target_servoy)
        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)
        self.fps = fps.FPS()  # 帧率统计器
​
        self.subscription = self.create_subscription(
            Image,
            '/image_raw',
            self.image_callback,
            100)
        self.subscription  
​
    def image_callback(self, ros_image):
        # cv_bridge 
        try:
            cv_image = self.bridge.imgmsg_to_cv2(ros_image, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().error(f"Failed to convert image: {e}")
            return
​
        # 使用 AprilTags 检测器
        tags = self.at_detector.detect(cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY), False, None, 0.025)
        tags = sorted(tags, key=lambda tag: tag.tag_id)
​
        # 绘制标签
        result_image = draw_tags(cv_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
​
        
        # 处理 AprilTags
        if len(tags) == 1:
            x, y, w, h = tags[0].bbox
            if math.fabs(180 - (x + w/2)) > 20:#调试方块半径    Debug Block Radius
                self.xservo_pid.SystemOutput = x + w/2
                self.xservo_pid.SetStepSignal(350)
                self.xservo_pid.SetInertiaTime(0.01, 0.1)
                target_valuex =  int(1000+self.xservo_pid.SystemOutput)
                target_servox = int((target_valuex)/10)
                #self.get_logger().info('x = {}'.format([x + w/2]))
                #self.get_logger().info('joints_x = {} {}'.format([target_servox],[target_valuex]))
                if target_servox > 180:
                    target_servox = 180
                if target_servox < 0:
                    target_servox = 0
                self.bot.Ctrl_Servo(1, target_servox)
​
            if math.fabs(180 - (y + h/2)) > 20: #调试方块半径    Debug Block Radius
                self.yservo_pid.SystemOutput = y + h/2
                self.yservo_pid.SetStepSignal(220)
                self.yservo_pid.SetInertiaTime(0.01, 0.1)
                target_valuey = int(650+self.yservo_pid.SystemOutput)
                target_servoy = int((target_valuey)/10)
                #self.get_logger().info('joints_y = {} {}'.format([target_servoy],[target_valuey]))                
                if target_servoy > 110:
                    target_servoy = 110
                if target_servoy < 0:
                    target_servoy = 0
                self.bot.Ctrl_Servo(2, target_servoy)
                
        # 更新并显示 FPS
        self.fps.update()
        self.fps.show_fps(result_image)
        cv2.imshow("result_image", result_image)
        key = cv2.waitKey(1)
        if key != -1:
            cv2.destroyAllWindows()
​
def main(args=None):
    rclpy.init(args=args)
​
    tag_tracking_node = TagTrackingNode()
​
    try:
        rclpy.spin(tag_tracking_node)
    except KeyboardInterrupt:
        tag_tracking_node.bot.Ctrl_Servo(1, 90)
        tag_tracking_node.bot.Ctrl_Servo(2, 25)
        pass
​
if __name__ == '__main__':
    main()