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=1target_servox = 90target_servoy = 25self.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))
​# 处理 AprilTagsif len(tags) == 1:x, y, w, h = tags[0].bboxif math.fabs(180 - (x + w/2)) > 20:#调试方块半径    Debug Block Radiusself.xservo_pid.SystemOutput = x + w/2self.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 = 180if target_servox < 0:target_servox = 0self.bot.Ctrl_Servo(1, target_servox)
​if math.fabs(180 - (y + h/2)) > 20: #调试方块半径    Debug Block Radiusself.yservo_pid.SystemOutput = y + h/2self.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 = 110if target_servoy < 0:target_servoy = 0self.bot.Ctrl_Servo(2, target_servoy)# 更新并显示 FPSself.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()