亚博microros小车-原生ubuntu支持系列：22 物体识别追踪

背景知识

跟上一个颜色追踪类似。也是基于opencv的，不过背后的算法有很多

BOOSTING：算法原理类似于Haar cascades (AdaBoost)，是一种很老的算法。这个算法速度慢并且不是很准。
MIL：比BOOSTING准一点。
KCF：速度比BOOSTING和MIL更快，与BOOSTING和MIL一样不能很好地处理遮挡问题。
CSRT：比KCF更准一些，但是速度比KCF稍慢。
MedianFlow：对于快速移动的目标和外形变化迅速的目标效果不好。
TLD：会产生较多的false-positives。
MOSSE：算法速度非常快，但是准确率比不上KCF和CSRT。在一些追求算法速度的场合很适用。
GOTURN：OpenCV中自带的唯一一个基于深度学习的算法。运行算法需要提前下载好模型文件。

对算法背后的原理干兴趣的，推荐看看大佬的

几个目标跟踪算法_视频目标跟踪算法-CSDN博客


    def initWorking(self, frame, box):
        '''
        Tracker work initialization 追踪器工作初始化
        frame:初始化追踪画面
        box:追踪的区域
        '''
        if not self.tracker: raise Exception("追踪器未初始化Tracker is not initialized")
        status = self.tracker.init(frame, box)
        #if not status: raise Exception("追踪器工作初始化失败Failed to initialize tracker job")
        self.coord = box
        self.isWorking = True

    def track(self, frame):
        if self.isWorking:#更新跟踪器
            status, self.coord = self.tracker.update(frame)
            if status:#如果跟踪成功，则绘制跟踪框：获取的坐标位置（x,y,w,h）画框就是（x,y）,(x+w,y+h)
                p1 = (int(self.coord[0]), int(self.coord[1]))
                p2 = (int(self.coord[0] + self.coord[2]), int(self.coord[1] + self.coord[3]))
                cv.rectangle(frame, p1, p2, (255, 0, 0), 2, 1)
                return frame, p1, p2
            else:
                # 跟踪失败
		# Tracking failed
                cv.putText(frame, "Tracking failure detected", (100, 80), cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
                return frame, (0, 0), (0, 0)
        else: return frame, (0, 0), (0, 0)

#!/usr/bin/env python3
# encoding: utf-8
import getpass
import threading
from yahboom_esp32ai_car.astra_common import *
from sensor_msgs.msg import CompressedImage,Image
from std_msgs.msg import Int32, Bool,UInt16
import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge

from rclpy.time import Time
import datetime
from ament_index_python.packages import get_package_share_directory #获取shares目录绝对路径


class mono_Tracker(Node):
    def __init__(self,name):
        super().__init__(name)
        self.pub_Servo1 = self.create_publisher(Int32,"servo_s1" , 10)#舵机控制
        self.pub_Servo2 = self.create_publisher(Int32,"servo_s2" , 10) 
        #初始化参数
        self.declare_param()
        self.target_servox = 0 #目标中心点x坐标
        self.target_servoy = 10 #目标中心点y坐标
        self.point_pose = (0, 0, 0)  #目标点位置 （x,y,z）
        self.circle = (0, 0, 0) #目标点圆信息 （x,y,radius）
        self.hsv_range = ()  #hsv 颜色范围
        self.dyn_update = True
        self.select_flags = False
        self.gTracker_state = False
        self.windows_name = 'frame'
        self.cols, self.rows = 0, 0 #鼠标选择区域坐标
        self.Mouse_XY = (0, 0) #鼠标点击坐标
        self.index = 2
        self.end = 0
        self.color = color_follow()
    
        self.tracker_types = ['BOOSTING', 'MIL', 'KCF']
        self.tracker_type = ['KCF'] 
        self.VideoSwitch = True
        self.img_flip = False

        self.last_stamp = None
        self.new_seconds = 0
        self.fps_seconds = 1

        ser1_angle = Int32()#舵机初始角度
        ser1_angle.data = int(self.target_servox)
        ser2_angle = Int32()
        ser2_angle.data = int(self.target_servoy)

        #确保角度正常处于中间
        for i in range(10):
            self.pub_Servo1.publish(ser1_angle)
            self.pub_Servo2.publish(ser2_angle)
            time.sleep(0.1)
        


        self.hsv_text = get_package_share_directory('yahboom_esp32ai_car')+'/resource/colorHSV.text'
        self.mono_PID = (12, 0, 0.9)
        self.scale = 1000
        self.PID_init()

        print("OpenCV Version: ",cv.__version__)
        self.gTracker = Tracker(tracker_type=self.tracker_type)
        self.tracker_type = self.tracker_types[self.index]
        self.Track_state = 'init'

        #USB
        #self.capture = cv.VideoCapture(0)
        #self.timer = self.create_timer(0.001, self.on_timer)

        #ESP32_wifi
        self.bridge = CvBridge()
        self.sub_img = self.create_subscription(
            CompressedImage, '/espRos/esp32camera', self.handleTopic, 1) #获取esp32传来的图像


    def declare_param(self):
        #PID
        self.declare_parameter("Kp",12)
        self.Kp = self.get_parameter('Kp').get_parameter_value().integer_value
        self.declare_parameter("Ki",0)
        self.Ki = self.get_parameter('Ki').get_parameter_value().integer_value
        self.declare_parameter("Kd",0.9)
        self.Kd = self.get_parameter('Kd').get_parameter_value().integer_value
    
    def get_param(self):
        self.Kd = self.get_parameter('Kd').get_parameter_value().integer_value
        self.Ki = self.get_parameter('Ki').get_parameter_value().integer_value
        self.Kp = self.get_parameter('Kp').get_parameter_value().integer_value
        self.mono_PID = (self.Kp,self.Ki,self.Kd)
        

    def cancel(self):     
        self.Reset()
        if self.VideoSwitch==False: self.__sub_img.unregister()
        cv.destroyAllWindows()

    # USB
    # def on_timer(self):
    #     self.get_param()
    #     ret, frame = self.capture.read()
    #     action = cv.waitKey(10) & 0xFF
    #     frame, binary =self.process(frame, action)
    #     start = time.time()
    #     fps = 1 / (start - self.end)
    #     text = "FPS : " + str(int(fps))
    #     self.end = start
    #     cv.putText(frame, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
    #     if len(binary) != 0: cv.imshow('frame', ManyImgs(1, ([frame, binary])))
    #     else:cv.imshow('frame', frame)
    #     if action == ord('q') or action == 113:
    #         self.capture.release()
    #         cv.destroyAllWindows()

    #ESP32_wifi  图像回调函数
    def handleTopic(self, msg):
        self.last_stamp = msg.header.stamp  
        if self.last_stamp:
            total_secs = Time(nanoseconds=self.last_stamp.nanosec, seconds=self.last_stamp.sec).nanoseconds
            delta = datetime.timedelta(seconds=total_secs * 1e-9)
            seconds = delta.total_seconds()*100

            if self.new_seconds != 0:
                self.fps_seconds = seconds - self.new_seconds

            self.new_seconds = seconds#保留这次的值

        self.get_param()
        start = time.time()
        frame = self.bridge.compressed_imgmsg_to_cv2(msg)#图像格式转换
        frame = cv.resize(frame, (640, 480))

        action = cv.waitKey(10) & 0xFF #获取按键事件
        frame, binary =self.process(frame, action)#核心处理逻辑
        
        
        end = time.time()
        fps = 1 / ((end - start)+self.fps_seconds)
        
        text = "FPS : " + str(int(fps))
        cv.putText(frame, text, (10,20), cv.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,255), 2)
        cv.imshow('frame', frame)

        if action == ord('q') or action == 113:
            cv.destroyAllWindows()




    def Reset(self):
        self.hsv_range = ()
        self.circle = (0, 0, 0)
        self.Mouse_XY = (0, 0)
        self.Track_state = 'init'
        self.target_servox = 0
        self.target_servoy = 10
    
    #控制舵机
    def execute(self, point_x, point_y):
        # rospy.loginfo("point_x: {}, point_y: {}".format(point_x, point_y))
        #通过PID计算舵机调整量（参数是计算目标位置与图像中心的偏差，图像中心坐标（320，240））
        [x_Pid, y_Pid] = self.PID_controller.update([point_x - 320, point_y - 240])
        if self.img_flip == True:#根据图像翻转标识调整方向
            self.target_servox -= x_Pid
            self.target_servoy += y_Pid
        else:
            self.target_servox -= x_Pid
            self.target_servoy += y_Pid
        #角度控制（保护舵机）    
        if self.target_servox >= 45:
            self.target_servox = 45
        elif self.target_servox <= -45:
            self.target_servox = -45
        if self.target_servoy >= 20:
            self.target_servoy = 20
        elif self.target_servoy <= -90:
            self.target_servoy = -90
        print("servo1",self.target_servox)
        servo1_angle = Int32()
        servo1_angle.data = int(self.target_servox)
        servo2_angle = Int32()
        servo2_angle.data = int(self.target_servoy)
        self.pub_Servo1.publish(servo1_angle)
        self.pub_Servo2.publish(servo2_angle)

    def dynamic_reconfigure_callback(self, config, level):
        self.scale = config['scale']
        self.mono_PID = (config['Kp'], config['Ki'], config['Kd'])
        self.hsv_range = ((config['Hmin'], config['Smin'], config['Vmin']),
                          (config['Hmax'], config['Smax'], config['Vmax']))
        self.PID_init()
        return config

    def PID_init(self):
        self.PID_controller = simplePID(
            [0, 0],
            [self.mono_PID[0] / float(self.scale), self.mono_PID[0] / float(self.scale)],
            [self.mono_PID[1] / float(self.scale), self.mono_PID[1] / float(self.scale)],
            [self.mono_PID[2] / float(self.scale), self.mono_PID[2] / float(self.scale)])
    #鼠标回调，除了param其他都是自动获取  
    def onMouse(self, event, x, y, flags, param):
        if event == 1:#左键点击
            self.Track_state = 'init'
            self.select_flags = True
            self.Mouse_XY = (x,y)
        if event == 4:#左键放开
            self.select_flags = False
            self.Track_state = 'identify'
        if self.select_flags == True:
            self.cols = min(self.Mouse_XY[0], x), min(self.Mouse_XY[1], y)
            self.rows = max(self.Mouse_XY[0], x), max(self.Mouse_XY[1], y)
            self.Roi_init = (self.cols[0], self.cols[1], self.rows[0], self.rows[1])
    #图像处理主流程
    def process(self, rgb_img, action):
        # param action: [113 or 'q':退出]，[114 or 'r':重置]，[105 or 'i'：识别]，[32：开始追踪]
        rgb_img = cv.resize(rgb_img, (640, 480))
        binary = []
        if self.img_flip == True: rgb_img = cv.flip(rgb_img, 1)#图像翻转
        #按键处理
        if action == 32: self.Track_state = 'tracking'
        elif action == ord('i') or action == 105: self.Track_state = "identify"
        elif action == ord('r') or action == 114: self.Reset()
        elif action == ord('q') or action == 113: self.cancel()
        if self.Track_state == 'init':#初始化状态
            cv.namedWindow(self.windows_name, cv.WINDOW_AUTOSIZE)
            cv.setMouseCallback(self.windows_name, self.onMouse, 0)
            if self.select_flags == True:#绘制选择区域
                cv.line(rgb_img, self.cols, self.rows, (255, 0, 0), 2)
                cv.rectangle(rgb_img, self.cols, self.rows, (0, 255, 0), 2)
                if self.Roi_init[0] != self.Roi_init[2] and self.Roi_init[1] != self.Roi_init[3]:
                    if self.tracker_type == "color": rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)#提取roi区域hsv范围
                    self.gTracker_state = True
                    self.dyn_update = True
                else: self.Track_state = 'init'
        if self.Track_state != 'init':#跟踪模式
            if self.tracker_type == "color" and len(self.hsv_range) != 0:
                rgb_img, binary, self.circle = self.color.object_follow(rgb_img, self.hsv_range)#颜色追踪
                if self.dyn_update == True:
                    params = {'Hmin': self.hsv_range[0][0], 'Hmax': self.hsv_range[1][0],
                              'Smin': self.hsv_range[0][1], 'Smax': self.hsv_range[1][1],
                              'Vmin': self.hsv_range[0][2], 'Vmax': self.hsv_range[1][2]}
                    self.dyn_client.update_configuration(params)#更新动态参数
                    self.dyn_update = False
            if self.tracker_type != "color":#其他跟踪模式
                if self.gTracker_state == True:
                    Roi = (self.Roi_init[0], self.Roi_init[1], self.Roi_init[2] - self.Roi_init[0], self.Roi_init[3] - self.Roi_init[1])
                    self.gTracker = Tracker(tracker_type=self.tracker_type)
                    self.gTracker.initWorking(rgb_img, Roi)
                    self.gTracker_state = False
                rgb_img, (targBegin_x, targBegin_y), (targEnd_x, targEnd_y) = self.gTracker.track(rgb_img)#执行追踪
                center_x = targEnd_x / 2 + targBegin_x / 2
                center_y = targEnd_y / 2 + targBegin_y / 2
                width = targEnd_x - targBegin_x
                high = targEnd_y - targBegin_y
                self.point_pose = (center_x, center_y, min(width, high))
        if self.Track_state == 'tracking':#执行追踪
            if self.circle[2] != 0: threading.Thread(target=self.execute, args=(self.circle[0], self.circle[1])).start()
            if self.point_pose[0] != 0 and self.point_pose[1] != 0: threading.Thread(target=self.execute, args=(self.point_pose[0], self.point_pose[1])).start()
        if self.tracker_type != "color": cv.putText(rgb_img, " Tracker", (260, 20), cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
        return rgb_img, binary

class simplePID:
    '''very simple discrete PID controller'''

    def __init__(self, target, P, I, D):
        '''Create a discrete PID controller
        each of the parameters may be a vector if they have the same length
        Args:
        target (double) -- the target value(s)
        P, I, D (double)-- the PID parameter
        '''
        # check if parameter shapes are compatabile.
        if (not (np.size(P) == np.size(I) == np.size(D)) or ((np.size(target) == 1) and np.size(P) != 1) or (
                np.size(target) != 1 and (np.size(P) != np.size(target) and (np.size(P) != 1)))):
            raise TypeError('input parameters shape is not compatable')
       # rospy.loginfo('P:{}, I:{}, D:{}'.format(P, I, D))
        self.Kp = np.array(P)
        self.Ki = np.array(I)
        self.Kd = np.array(D)
        self.last_error = 0
        self.integrator = 0
        self.timeOfLastCall = None
        self.setPoint = np.array(target)
        self.integrator_max = float('inf')

    def update(self, current_value):
        '''Updates the PID controller.
        Args:
            current_value (double): vector/number of same legth as the target given in the constructor
        Returns:
            controll signal (double): vector of same length as the target
        '''
        current_value = np.array(current_value)
        if (np.size(current_value) != np.size(self.setPoint)):
            raise TypeError('current_value and target do not have the same shape')
        if (self.timeOfLastCall is None):
            # the PID was called for the first time. we don't know the deltaT yet
            # no controll signal is applied
            self.timeOfLastCall = time.perf_counter()
            return np.zeros(np.size(current_value))
        error = self.setPoint - current_value#计算误差
        P = error
        currentTime = time.perf_counter()
        deltaT = (currentTime - self.timeOfLastCall)
        # integral of the error is current error * time since last update  计算I
        self.integrator = self.integrator + (error * deltaT)
        I = self.integrator
        # derivative is difference in error / time since last update 计算P
        D = (error - self.last_error) / deltaT
        self.last_error = error
        self.timeOfLastCall = currentTime
        # return controll signal 计算输出
        return self.Kp * P + self.Ki * I + self.Kd * D


def main():
    rclpy.init()
    mono_tracker = mono_Tracker("monoIdentify")
    try:
        rclpy.spin(mono_tracker)
    except KeyboardInterrupt:
        pass
    finally:
        mono_tracker.destroy_node()
        rclpy.shutdown()

关键功能：

支持多种跟踪算法（KCF/BOOSTING/MIL）
颜色跟踪模式可自动提取HSV范围
多线程控制确保流畅性
完善的异常处理机制
实时显示处理帧率

这个系统可以实现对运动目标或特定颜色物体的自动跟踪，通过PID算法保持目标在画面中心位置，典型应用于智能监控、机器人视觉跟随等场景。这个做上一篇的颜色追踪类似：亚博microros小车-原生ubuntu支持系列：21 颜色追踪-CSDN博客

都是根据目标的中心坐标和来计算s1、s2舵机转动角度，然后发布给底盘。

程序说明

程序启动后，通过鼠标选中需要跟踪的物体，按下空格键，小车的云台舵机进入跟踪模式。小车云台会跟随被跟踪的物体移动，并且时刻保证被追踪的物体保持在画面中心。

测试

启动小车代理

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8090 -v4

启动图像代理

docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 9999 -v4

（选做）调整舵机

ros2 run yahboom_esp32_mediapipe control_servo

启动脚本

ros2 run yahboom_esp32ai_car mono_Tracker

启动之后，进入选择模式，用鼠标选中目标所在位置，如下图所示，松开即开始识别。

把我的镇宅之宝小黄鸭拿出来了

键盘按键控制：

【r】：选择模式，可用鼠标选择要识别目标的区域，如上图。

【q】：退出程序。

【空格键】：目标追踪；在跟随的时候缓慢移动目标即可，移动太快将会丢失目标。

按下空格能看到通过pid算出来的运动角度。

ohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 run yahboom_esp32ai_car mono_Tracker 
OpenCV Version:  4.11.0
servo1 0.0
servo1 1.6740220783505821
servo1 2.981003373069086
servo1 4.277003373069086
servo1 5.418249937784921
servo1 6.018073510048205
servo1 6.0159973543603105
servo1 5.71860954302583
servo1 4.775629931904761
servo1 3.8326030635678214
servo1 2.9206030635678215
servo1 1.9081850712547614
servo1 0.7822878210318385
servo1 0.18899249274645924
servo1 -0.2834846440358629
servo1 -0.7540859436953311
servo1 -1.234085943695331
servo1 -1.6484508720535367
servo1 -2.1135512406099104
servo1 -2.5695512406099104
servo1 -3.058080514019669
servo1 -3.538080514019669
servo1 -4.171699309150052
servo1 -4.530007470444837
servo1 -4.800294859267008
servo1 -4.8453324923276115
servo1 -4.865291304517047
servo1 -4.94373365090143
servo1 -5.136952220684027
servo1 -5.469228000644295
servo1 -6.027853281852513
servo1 -6.710800306564447
servo1 -7.6552821486406
servo1 -8.052589780593427
servo1 -9.223112314977138
servo1 -10.319161974550278
servo1 -11.591078854236214
servo1 -12.416221902691895
servo1 -13.360740214236708
servo1 -14.51497408279646
servo1 -14.780085181765932
servo1 -14.310165362425591
servo1 -14.15717444041632
servo1 -14.435688173088373
servo1 -15.32025424018754
servo1 -16.531503721252605
servo1 -17.836778372955706
servo1 -19.017197794387748
servo1 -20.28310463824062
servo1 -21.166754300006183
servo1 -21.242287643352327
servo1 -21.1565011124945
servo1 -21.707825159200315
servo1 -23.013047473734893
servo1 -24.58221911876973
servo1 -26.81213675078672
servo1 -28.374622533573962
servo1 -29.871810297867736
servo1 -31.217257596323204
servo1 -32.31216336848393
servo1 -33.601930296065284
servo1 -34.03160970476731