亚博microros小车-原生ubuntu支持系列：21 颜色追踪

背景知识

这个测试例子用到了很多opencv的函数，举个例子。

  #cv2.findContours函数来找到二值图像中的轮廓。
        #参数：
                #参数1：输  入的二值图像。通常是经过阈值处理后的图像，例如在颜色过滤之后生成的掩码。
                #参数2(cv2.RETR_EXTERNAL)：轮廓的检索模式。有几种模式可选，常用的包括：
                   # cv2.RETR_EXTERNAL：只检测最外层的轮廓。
                   # cv2.RETR_LIST：检测所有的轮廓并保存到列表中。
                   # cv2.RETR_CCOMP：检测所有轮廓并将其组织为两层的层次结构。
                   # cv2.RETR_TREE：检测所有轮廓并重构整个轮廓层次结构。
               # 参数3(cv2.CHAIN_APPROX_SIMPLE)：轮廓的近似方法。有两种方法可选，常用的有：
                   # cv2.CHAIN_APPROX_SIMPLE：压缩水平、垂直和对角线方向上的所有轮廓，只保留端点。
                   # cv2.CHAIN_APPROX_NONE：保留所有的轮廓点。
        #返回值：      contours：包含检测到的轮廓的列表。每个轮廓由一系列点组成。
#                   _（下划线）：层次信息，通常在后续处理中可能会用到。在这里，我们通常用下划线表示我们不关心这个返回值。

我第一次看代码的时候，发现有些没理解，主要是靠deepseek 搜索了加了注释。

src/yahboom_esp32ai_car/yahboom_esp32ai_car/目录下新建astra_common.py

#!/usr/bin/env python
# encoding: utf-8
import time
import cv2 as cv
import numpy as np



def write_HSV(wf_path, value):
    with open(wf_path, "w") as wf:
        wf_str = str(value[0][0]) + ', ' + str(
            value[0][1]) + ', ' + str(value[0][2]) + ', ' + str(
            value[1][0]) + ', ' + str(value[1][1]) + ', ' + str(
            value[1][2])
        wf.write(wf_str)
        wf.flush()


def read_HSV(rf_path):
    rf = open(rf_path, "r+")
    line = rf.readline()
    if len(line) == 0: return ()
    list = line.split(',')
    if len(list) != 6: return ()
    hsv = ((int(list[0]), int(list[1]), int(list[2])),
           (int(list[3]), int(list[4]), int(list[5])))
    rf.flush()
    return hsv


# 定义函数，第一个参数是缩放比例，第二个参数是需要显示的图片组成的元组或者列表
# Define the function, the first parameter is the zoom ratio, and the second parameter is a tuple or list of pictures to be displayed
def ManyImgs(scale, imgarray):
    rows = len(imgarray)  # 元组或者列表的长度 Length of tuple or list
    cols = len(imgarray[0])  # 如果imgarray是列表，返回列表里第一幅图像的通道数，如果是元组，返回元组里包含的第一个列表的长度
    # If imgarray is a list, return the number of channels of the first image in the list, if it is a tuple, return the length of the first list contained in the tuple
    # print("rows=", rows, "cols=", cols)
    # 判断imgarray[0]的类型是否是list,
    # 是list，表明imgarray是一个元组，需要垂直显示
    # Determine whether the type of imgarray[0] is list,
    # It is a list, indicating that imgarray is a tuple and needs to be displayed vertically
    rowsAvailable = isinstance(imgarray[0], list)
    # 第一张图片的宽高
    # The width and height of the first picture
    width = imgarray[0][0].shape[1]
    height = imgarray[0][0].shape[0]
    # print("width=", width, "height=", height)
    # 如果传入的是一个元组
    # If the incoming is a tuple
    if rowsAvailable:
        for x in range(0, rows):
            for y in range(0, cols):
                # 遍历元组，如果是第一幅图像，不做变换
                # Traverse the tuple, if it is the first image, do not transform
                if imgarray[x][y].shape[:2] == imgarray[0][0].shape[:2]:
                    imgarray[x][y] = cv.resize(imgarray[x][y], (0, 0), None, scale, scale)
                # 将其他矩阵变换为与第一幅图像相同大小，缩放比例为scale
                # Transform other matrices to the same size as the first image, and the zoom ratio is scale
                else:
                    imgarray[x][y] = cv.resize(imgarray[x][y], (imgarray[0][0].shape[1], imgarray[0][0].shape[0]), None,
                                               scale, scale)
                # 如果图像是灰度图，将其转换成彩色显示
                # If the image is grayscale, convert it to color display
                if len(imgarray[x][y].shape) == 2:
                    imgarray[x][y] = cv.cvtColor(imgarray[x][y], cv.COLOR_GRAY2BGR)
        # 创建一个空白画布，与第一张图片大小相同
        # Create a blank canvas, the same size as the first picture
        imgBlank = np.zeros((height, width, 3), np.uint8)
        hor = [imgBlank] * rows  # 与第一张图片大小相同，与元组包含列表数相同的水平空白图像
        # The same size as the first picture, and the same number of horizontal blank images as the tuple contains the list
        for x in range(0, rows):
            # 将元组里第x个列表水平排列
            # Arrange the x-th list in the tuple horizontally
            hor[x] = np.hstack(imgarray[x])
        ver = np.vstack(hor)  # 将不同列表垂直拼接 Concatenate different lists vertically
    # 如果传入的是一个列表 If the incoming is a list
    else:
        # 变换操作，与前面相同
        # Transformation operation, same as before
        for x in range(0, rows):
            if imgarray[x].shape[:2] == imgarray[0].shape[:2]:
                imgarray[x] = cv.resize(imgarray[x], (0, 0), None, scale, scale)
            else:
                imgarray[x] = cv.resize(imgarray[x], (imgarray[0].shape[1], imgarray[0].shape[0]), None, scale, scale)
            if len(imgarray[x].shape) == 2:
                imgarray[x] = cv.cvtColor(imgarray[x], cv.COLOR_GRAY2BGR)
        # 将列表水平排列
        # Arrange the list horizontally
        hor = np.hstack(imgarray)
        ver = hor
    return ver


class color_follow:
    def __init__(self):
        '''
        初始化一些参数
	    Initialize some parameters
        '''
        self.Center_x = 0
        self.Center_y = 0
        self.Center_r = 0

    def object_follow(self, img, hsv_msg):
        src = img.copy()
        # 由颜色范围创建NumPy数组
        # Create NumPy array from color range
        src = cv.cvtColor(src, cv.COLOR_BGR2HSV)
        lower = np.array(hsv_msg[0], dtype="uint8")
        upper = np.array(hsv_msg[1], dtype="uint8")
        # 根据特定颜色范围创建mask,inRange的作用是根据阈值进行二值化:阈值内的像素设置为白色(255)，阈值外的设置为黑色(0)
        # Create a mask based on a specific color range
        mask = cv.inRange(src, lower, upper)
        color_mask = cv.bitwise_and(src, src, mask=mask)
        # 将图像转为灰度图
        # Convert the image to grayscale
        gray_img = cv.cvtColor(color_mask, cv.COLOR_RGB2GRAY)
        # 获取不同形状的结构元素，定义一个用于形态学操作的“探针”，决定操作影响的区域大小和形状。代码是矩形的。内核越大，处理的区域范围越大（可能过度平滑细节）。
        # Get structure elements of different shapes
        kernel = cv.getStructuringElement(cv.MORPH_RECT, (5, 5))
        # 形态学闭操作，形态学闭运算是 先膨胀（Dilation）后腐蚀（Erosion） 的组合操作，目的是消除图像中的微小暗区（如孔洞）或断裂区域，同时保持主体结构的完整性。
        # Morphological closed operation
        gray_img = cv.morphologyEx(gray_img, cv.MORPH_CLOSE, kernel)
        # 图像二值化操作
        # Image binarization operation
        ret, binary = cv.threshold(gray_img, 10, 255, cv.THRESH_BINARY)
        # 获取轮廓点集(坐标)
        # Get the set of contour points (coordinates)
        find_contours = cv.findContours(binary, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
        #兼容不同版本opencv
        if len(find_contours) == 3:#OpenCV v3、v4-pre 或 v4-alpha
            contours = find_contours[1]
        else:# OpenCV v2.4、v4-beta 或 v4-official
            contours = find_contours[0]
        if len(contours) != 0:#检测到轮廓
            areas = []
            for c in range(len(contours)): areas.append(cv.contourArea(contours[c]))
            max_id = areas.index(max(areas))
            max_rect = cv.minAreaRect(contours[max_id])#计算最小外接矩形
            max_box = cv.boxPoints(max_rect)#获取矩形顶点坐标
            max_box = np.int0(max_box)#转为整数，便于绘图
            #计算物体的最小外接圆，参数为轮廓的点集
            (color_x, color_y), color_radius = cv.minEnclosingCircle(max_box)
            # 将检测到的颜色用原形线圈标记出来
            # Mark the detected color with the original shape coil
            self.Center_x = int(color_x)
            self.Center_y = int(color_y)
            self.Center_r = int(color_radius)
            cv.circle(img, (self.Center_x, self.Center_y), self.Center_r, (255, 0, 255), 2)#画圆显示物体的圆形轮廓
            cv.circle(img, (self.Center_x, self.Center_y), 2, (0, 0, 255), -1)# 画红色实心圆作为中心点
        else:
            self.Center_x = 0
            self.Center_y = 0
            self.Center_r = 0
        return img, binary, (self.Center_x, self.Center_y, self.Center_r)

    def Roi_hsv(self, img, Roi):
        '''
        获取某一区域的HSV的范围
        Get the range of HSV in a certain area
        :param img: Color map 彩色图 
        :param Roi:  (x_min, y_min, x_max, y_max)
        Roi=(290,280,350,340)
        :return: 图像和HSV的范围 例如：(0,0,90)(177,40,150)
	         Image and HSV range E.g：(0,0,90)(177,