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. 源码

python 复制代码
#!/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()
相关推荐
杨小扩5 小时前
第4章:实战项目一 打造你的第一个AI知识库问答机器人 (RAG)
人工智能·机器人
whaosoft-1435 小时前
51c~目标检测~合集4
人工智能
雪兽软件5 小时前
2025 年网络安全与人工智能发展趋势
人工智能·安全·web安全
元宇宙时间6 小时前
全球发展币GDEV:从中国出发,走向全球的数字发展合作蓝图
大数据·人工智能·去中心化·区块链
小黄人20257 小时前
自动驾驶安全技术的演进与NVIDIA的创新实践
人工智能·安全·自动驾驶
ZStack开发者社区8 小时前
首批 | 云轴科技ZStack加入施耐德电气技术本地化创新生态
人工智能·科技·云计算
千宇宙航8 小时前
闲庭信步使用图像验证平台加速FPGA的开发:第六课——测试图案的FPGA实现
图像处理·计算机视觉·fpga开发
X Y O8 小时前
神经网络初步学习3——数据与损失
人工智能·神经网络·学习
唯创知音9 小时前
玩具语音方案选型决策OTP vs Flash 的成本功耗与灵活性
人工智能·语音识别
Jamence9 小时前
多模态大语言模型arxiv论文略读(151)
论文阅读·人工智能·语言模型·自然语言处理·论文笔记