成都网站制作scgckj/长沙seo咨询

背景知识

这个测试例子用到了很多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 npdef 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 listcols = 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 verticallyrowsAvailable = isinstance(imgarray[0], list)# 第一张图片的宽高# The width and height of the first picturewidth = imgarray[0][0].shape[1]height = imgarray[0][0].shape[0]# print("width=", width, "height=", height)# 如果传入的是一个元组# If the incoming is a tupleif rowsAvailable:for x in range(0, rows):for y in range(0, cols):# 遍历元组，如果是第一幅图像，不做变换# Traverse the tuple, if it is the first image, do not transformif 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 scaleelse: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 displayif 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 pictureimgBlank = 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 listfor x in range(0, rows):# 将元组里第x个列表水平排列# Arrange the x-th list in the tuple horizontallyhor[x] = np.hstack(imgarray[x])ver = np.vstack(hor)  # 将不同列表垂直拼接 Concatenate different lists vertically# 如果传入的是一个列表 If the incoming is a listelse:# 变换操作，与前面相同# Transformation operation, same as beforefor 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 horizontallyhor = np.hstack(imgarray)ver = horreturn verclass color_follow:def __init__(self):'''初始化一些参数Initialize some parameters'''self.Center_x = 0self.Center_y = 0self.Center_r = 0def object_follow(self, img, hsv_msg):src = img.copy()# 由颜色范围创建NumPy数组# Create NumPy array from color rangesrc = 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 rangemask = cv.inRange(src, lower, upper)color_mask = cv.bitwise_and(src, src, mask=mask)# 将图像转为灰度图# Convert the image to grayscalegray_img = cv.cvtColor(color_mask, cv.COLOR_RGB2GRAY)# 获取不同形状的结构元素，定义一个用于形态学操作的“探针”，决定操作影响的区域大小和形状。代码是矩形的。内核越大，处理的区域范围越大（可能过度平滑细节）。# Get structure elements of different shapeskernel = cv.getStructuringElement(cv.MORPH_RECT, (5, 5))# 形态学闭操作，形态学闭运算是 先膨胀（Dilation）后腐蚀（Erosion） 的组合操作，目的是消除图像中的微小暗区（如孔洞）或断裂区域，同时保持主体结构的完整性。# Morphological closed operationgray_img = cv.morphologyEx(gray_img, cv.MORPH_CLOSE, kernel)# 图像二值化操作# Image binarization operationret, 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)#兼容不同版本opencvif len(find_contours) == 3:#OpenCV v3、v4-pre 或 v4-alphacontours = find_contours[1]else:# OpenCV v2.4、v4-beta 或 v4-officialcontours = 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 coilself.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 = 0self.Center_y = 0self.Center_r = 0return 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,40,150) '''H = [];S = [];V = []# 将彩色图转成HSV# Convert color image to HSVHSV = cv.cvtColor(img, cv.COLOR_BGR2HSV)# 画矩形框# Draw a rectangular frame# cv.rectangle(img, (Roi[0], Roi[1]), (Roi[2], Roi[3]), (0, 255, 0), 2)# 依次取出每行每列的H,S,V值放入容器中# Take out the H, S, V values of each row and each column in turn and put them into the containerfor i in range(Roi[0], Roi[2]):for j in range(Roi[1], Roi[3]):H.append(HSV[j, i][0])S.append(HSV[j, i][1])V.append(HSV[j, i][2])# 分别计算出H,S,V的最大最小# Calculate the maximum and minimum of H, S, and V respectivelyH_min = min(H); H_max = max(H)S_min = min(S); S_max = 253V_min = min(V); V_max = 255# HSV范围调整# HSV range adjustmentif H_max + 5 > 255: H_max = 255else: H_max += 5if H_min - 5 < 0: H_min = 0else: H_min -= 5if S_min - 20 < 0: S_min = 0else: S_min -= 20if V_min - 20 < 0: V_min = 0else: V_min -= 20lowerb = 'lowerb : (' + str(H_min) + ' ,' + str(S_min) + ' ,' + str(V_min) + ')'upperb = 'upperb : (' + str(H_max) + ' ,' + str(S_max) + ' ,' + str(V_max) + ')'txt1 = 'Learning ...'txt2 = 'OK !!!'if S_min < 5 or V_min < 5:cv.putText(img, txt1, (30, 50), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)else:cv.putText(img, txt2, (30, 50), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)cv.putText(img, lowerb, (150, 30), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)cv.putText(img, upperb, (150, 50), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)hsv_range = ((int(H_min), int(S_min), int(V_min)), (int(H_max), int(S_max), int(V_max)))return img, hsv_rangeclass simplePID:def __init__(self, kp, ki, kd):self.kp = kpself.ki = kiself.kd = kdself.targetpoint = 0self.intergral = 0self.derivative = 0self.prevError = 0def compute(self, target, current):error = target - currentself.intergral += errorself.derivative = error - self.prevErrorself.targetpoint = self.kp * error + self.ki * self.intergral + self.kd * self.derivativeself.prevError = errorreturn self.targetpointdef reset(self):self.targetpoint = 0self.intergral = 0self.derivative = 0self.prevError = 0class Tracker(object):'''追踪者模块,用于追踪指定目标Tracker module, used to track the specified target'''def __init__(self, tracker_type="BOOSTING"):'''初始化追踪器种类Initialize the type of tracker'''# 获得opencv版本# Get the opencv version(major_ver, minor_ver, subminor_ver) = (cv.__version__).split('.')self.tracker_type = tracker_typeself.isWorking = False# 构造追踪器# Construct trackerif int(major_ver) < 3: self.tracker = cv.Tracker_create(tracker_type)else:if tracker_type == 'BOOSTING': self.tracker = cv.TrackerBoosting_create()if tracker_type == 'MIL': self.tracker = cv.TrackerMIL_create()if tracker_type == 'KCF': self.tracker = cv.TrackerKCF_create()if tracker_type == 'TLD': self.tracker = cv.TrackerTLD_create()if tracker_type == 'MEDIANFLOW': self.tracker = cv.TrackerMedianFlow_create()if tracker_type == 'GOTURN': self.tracker = cv.TrackerGOTURN_create()if tracker_type == 'MOSSE': self.tracker = cv.TrackerMOSSE_create()if tracker_type == "CSRT": self.tracker = cv.TrackerCSRT_create()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 = boxself.isWorking = Truedef track(self, frame):if self.isWorking:status, self.coord = self.tracker.update(frame)if status: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, p2else:# 跟踪失败# Tracking failedcv.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)

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

#ros lib
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
from sensor_msgs.msg import CompressedImage, LaserScan, Image
from yahboomcar_msgs.msg import Position
from cv_bridge import CvBridge
#common lib
import os
import threading
import math
from yahboom_esp32ai_car.astra_common import * #自定义函数
from yahboomcar_msgs.msg import Positionfrom rclpy.time import Time
import datetime
from ament_index_python.packages import get_package_share_directory #获取shares目录绝对路径print("import finish")
cv_edition = cv.__version__
print("cv_edition: ",cv_edition)
class Color_Identify(Node):def __init__(self,name):super().__init__(name)#create a publisher 创建发布者self.pub_position = self.create_publisher(Position,"/Current_point", 10)# 发布目标位置self.pub_cmdVel = self.create_publisher(Twist, '/cmd_vel', 10) #发布速度指令self.pub_img = self.create_publisher(Image,'/image_raw',500)  #发布处理后的图self.bridge = CvBridge() #opencv 和ros图像转换工具#初始化变量self.index = 2self.Roi_init = () #roi区域坐标self.hsv_range = () #hsv 颜色范围self.end = 0 #fps 计算用self.circle = (0, 0, 0) #检测到目标圆信息（x,y,radius）self.point_pose = (0, 0, 0) #目标点位置 （x,y,z）self.dyn_update = Trueself.Start_state = Trueself.select_flags = Falseself.gTracker_state = Falseself.windows_name = 'frame'self.Track_state = 'identify' #跟踪状态，['init','identify','tracking']self.color = color_follow() #颜色跟踪类self.cols, self.rows = 0, 0 #鼠标选择区域坐标self.Mouse_XY = (0, 0) #鼠标点击坐标self.declare_param()#hsv 声明参数self.hsv_text =    get_package_share_directory('yahboom_esp32ai_car')+'/resource/colorHSV.text'#self.timer = self.create_timer(0.001, self.on_timer)def declare_param(self):#HSVself.declare_parameter("Hmin",0)self.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_valueself.declare_parameter("Smin",85)self.Smin = self.get_parameter('Smin').get_parameter_value().integer_valueself.declare_parameter("Vmin",126)self.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_valueself.declare_parameter("Hmax",9)self.Hmax = self.get_parameter('Hmax').get_parameter_value().integer_valueself.declare_parameter("Smax",253)self.Smax = self.get_parameter('Smax').get_parameter_value().integer_valueself.declare_parameter("Vmax",253)self.Vmax = self.get_parameter('Vmax').get_parameter_value().integer_valueself.declare_parameter('refresh',False)self.refresh = self.get_parameter('refresh').get_parameter_value().bool_value#改成my_picturc里面使用即可# def on_timer(self):#     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, (30, 30), cv.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 200), 1)#     if len(binary) != 0: cv.imshow('frame', ManyImgs(1, ([frame, binary])))#     else:cv.imshow('frame', frame)#     msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")#     self.pub_img.publish(msg)#     if action == ord('q') or action == 113:#         self.capture.release()#         cv.destroyAllWindows()# 主逻辑，执行颜色识别、跟踪逻辑        def process(self, rgb_img, action):self.get_param()rgb_img = cv.resize(rgb_img, (640, 480))binary = []if action != 255: self.get_logger().info(f'action={action}')if action == 32: self.Track_state = 'tracking' #空格键值elif action == ord('i') or action == ord('I'): self.Track_state = "identify"#识别elif action == ord('r') or action == ord('R'): self.Reset() #重置elif action == ord('q') or action == ord('Q'): self.cancel() #取消self.get_logger().info(f'Track_state={self.Track_state}')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:#绘制roi区域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]:rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)#提取roi区域的HSV范围self.gTracker_state = Trueself.dyn_update = Trueelse: self.Track_state = 'init'elif self.Track_state == "identify":if os.path.exists(self.hsv_text): self.hsv_range = read_HSV(self.hsv_text)  # 从文件加载HSV范围else: self.Track_state = 'init'if self.Track_state != 'init':if len(self.hsv_range) != 0:rgb_img, binary, self.circle = self.color.object_follow(rgb_img, self.hsv_range)#颜色追踪if self.dyn_update == True:#参数更新write_HSV(self.hsv_text, self.hsv_range) # 保存HSV范围到文件self.Hmin  = rclpy.parameter.Parameter('Hmin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][0])self.Smin  = rclpy.parameter.Parameter('Smin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][1])self.Vmin  = rclpy.parameter.Parameter('Vmin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][2])self.Hmax  = rclpy.parameter.Parameter('Hmax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][0])self.Smax  = rclpy.parameter.Parameter('Smax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][1])self.Vmax  = rclpy.parameter.Parameter('Vmax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][2])all_new_parameters = [self.Hmin,self.Smin,self.Vmin,self.Hmax,self.Smax,self.Vmax]self.set_parameters(all_new_parameters)self.dyn_update = False     if self.Track_state == 'tracking':self.Start_state = Trueif self.circle[2] != 0: threading.Thread(target=self.execute, args=(self.circle[0], self.circle[1], self.circle[2])).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], self.point_pose[2])).start()else:if self.Start_state == True:self.pub_cmdVel.publish(Twist())#停止self.Start_state = Falsereturn rgb_img, binary#发布目标位置def execute(self, x, y, z):position = Position()position.anglex = x * 1.0position.angley = y * 1.0position.distance = z * 1.0self.pub_position.publish(position)def get_param(self):#hsvself.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_valueself.Smin = self.get_parameter('Smin').get_parameter_value().integer_valueself.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_valueself.Hmax = self.get_parameter('Hmax').get_parameter_value().integer_valueself.Smax = self.get_parameter('Smax').get_parameter_value().integer_valueself.Vmax = self.get_parameter('Vmax').get_parameter_value().integer_valueself.refresh = self.get_parameter('refresh').get_parameter_value().bool_valuedef Reset(self):self.hsv_range = ()self.circle = (0, 0, 0)self.Mouse_XY = (0, 0)self.Track_state = 'init'for i in range(3): self.pub_position.publish(Position())print("succes!!!")def cancel(self):print("Shutting down this node.")cv.destroyAllWindows()#鼠标回调函数，除了param其他都是自动获取    def onMouse(self, event, x, y, flags, param):if event == 1:#左键点击self.Track_state = 'init'self.select_flags = Trueself.Mouse_XY = (x, y)if event == 4:#左键放开self.select_flags = Falseself.Track_state = 'mouse'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])class MY_Picture(Node):def __init__(self, name):super().__init__(name)self.bridge = CvBridge()self.sub_img = self.create_subscription(CompressedImage, '/espRos/esp32camera', self.handleTopic, 1) #获取esp32传来的图像self.color_identify = Color_Identify("ColorIdentify")self.last_stamp = Noneself.new_seconds = 0self.fps_seconds = 1#图像回调    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).nanosecondsdelta = datetime.timedelta(seconds=total_secs * 1e-9)seconds = delta.total_seconds()*100if self.new_seconds != 0:self.fps_seconds = seconds - self.new_secondsself.new_seconds = seconds#保留这次的值start = time.time()frame = self.bridge.compressed_imgmsg_to_cv2(msg)#图像转换frame = cv.resize(frame, (640, 480))action = cv.waitKey(10) & 0xFFframe, binary =self.color_identify.process(frame, action)#执行处理逻辑if len(binary) != 0: cv.imshow('frame', ManyImgs(1, ([frame, binary])))else:cv.imshow('frame', frame)msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")self.color_identify.pub_img.publish(msg)       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)if len(binary) != 0: cv.imshow('frame', ManyImgs(1, ([frame, binary])))else:cv.imshow('frame', frame)if action == ord('q') or action == 113:cv.destroyAllWindows()#rclpy.spin(self.QRdetect)def main():rclpy.init()esp_img = MY_Picture("My_Picture")print("start it")try:rclpy.spin(esp_img)except KeyboardInterrupt:passfinally:esp_img.destroy_node()rclpy.shutdown()

主要功能模块说明：

1. Color_Identify 类

   • 实现颜色识别与跟踪的核心逻辑

   • 支持动态ROI选择、HSV参数调整

   • 通过ROS参数服务器管理HSV范围

   • 提供目标位置发布和运动控制功能

2. MY_Picture 类

   • 主图像处理节点

   • 订阅摄像头图像话题

   • 集成颜色识别功能

   • 实现FPS计算与图像显示

3. 关键技术点

   • OpenCV颜色空间转换（BGR->HSV）

   • 形态学操作（开闭运算去噪）

   • 轮廓检测与最小外接圆计算

   • ROS2动态参数管理

   • 多线程控制指令发布

4. 工作流程

   1. 初始化节点和订阅者/发布者

   2. 接收摄像头图像

   3. 颜色识别处理（支持ROI选择）

   4. 目标位置计算

   5. 控制指令生成与发布

   6. 实时图像显示与状态反馈

#ros lib
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
from sensor_msgs.msg import CompressedImage, LaserScan, Image
from yahboomcar_msgs.msg import Position
from std_msgs.msg import Int32, Bool,UInt16
from cv_bridge import CvBridge
#common lib
import os
import threading
import math
import time
from yahboom_esp32ai_car.astra_common import *
from yahboomcar_msgs.msg import Positionprint("import done")class color_Tracker(Node):def __init__(self,name):super().__init__(name)#create the publisherself.pub_cmdVel = self.create_publisher(Twist,'/cmd_vel',10)#速度控制self.pub_Servo1 = self.create_publisher(Int32,"servo_s1" , 10)#舵机1角度控制self.pub_Servo2 = self.create_publisher(Int32,"servo_s2" , 10)#舵机2角度控制#create the subscriberself.sub_depth = self.create_subscription(Image,"/image_raw", self.depth_img_Callback, 1)#图像self.sub_JoyState = self.create_subscription(Bool,'/JoyState',  self.JoyStateCallback,1)self.sub_position = self.create_subscription(Position,"/Current_point",self.positionCallback,1)#目标位置self.bridge = CvBridge()#初始化变量self.minDist = 1500  #最小追踪距离（mm）self.Center_x = 0  #目标中心点x坐标self.Center_y = 0  #目标中心点y坐标self.Center_r = 0  #目标半径self.Center_prevx = 0 #前一帧目标x坐标self.Center_prevr = 0 #前一帧目标半径self.prev_time = 0 #时间戳self.prev_dist = 0self.prev_angular = 0self.Joy_active = Falseself.Robot_Run = False  #运行状态self.img_flip = False #图像翻转表示self.dist = [] self.encoding = ['8UC3']self.linear_PID = (8.0, 0.0, 1.0) #线性pid参数(kp,ki,kd)self.angular_PID = (0.5, 0.0, 2.0) #角度pid参数self.scale = 1000 #pid缩放系数self.PID_init()self.PWMServo_X = 0  #舵机1当前角度self.PWMServo_Y = 10 #舵机2当前角度self.s1_init_angle = Int32()  #舵机1 初始角度self.s1_init_angle.data = self.PWMServo_Xself.s2_init_angle = Int32()  #舵机2 初始角度self.s2_init_angle.data = self.PWMServo_Yself.declare_param() #声明参数#确保角度正常处于中间for i in range(10):self.pub_Servo1.publish(self.s1_init_angle)self.pub_Servo2.publish(self.s2_init_angle)time.sleep(0.1) #发布间隔0.1秒print("init done")def declare_param(self):self.declare_parameter("linear_Kp",20.0)self.linear_Kp = self.get_parameter('linear_Kp').get_parameter_value().double_valueself.declare_parameter("linear_Ki",0.0)self.linear_Ki = self.get_parameter('linear_Ki').get_parameter_value().double_valueself.declare_parameter("linear_Kd",2.0)self.linear_Kd = self.get_parameter('linear_Kd').get_parameter_value().double_valueself.declare_parameter("angular_Kp",0.5)self.angular_Kp = self.get_parameter('angular_Kp').get_parameter_value().double_valueself.declare_parameter("angular_Ki",0.0)self.angular_Ki = self.get_parameter('angular_Ki').get_parameter_value().double_valueself.declare_parameter("angular_Kd",2.0)self.angular_Kd = self.get_parameter('angular_Kd').get_parameter_value().double_valueself.declare_parameter("scale",1000)self.scale = self.get_parameter('scale').get_parameter_value().integer_valueself.declare_parameter("minDistance",1.0)self.minDistance = self.get_parameter('minDistance').get_parameter_value().double_valueself.declare_parameter('angular_speed_limit',5.0)self.angular_speed_limit = self.get_parameter('angular_speed_limit').get_parameter_value().double_valueself.pub_Servo1.publish(self.s1_init_angle)self.pub_Servo2.publish(self.s2_init_angle)#从参数服务器获取最新参数值def get_param(self):self.linear_Kp = self.get_parameter('linear_Kp').get_parameter_value().double_valueself.linear_Ki = self.get_parameter('linear_Ki').get_parameter_value().double_valueself.linear_Kd = self.get_parameter('linear_Kd').get_parameter_value().double_valueself.angular_Kp = self.get_parameter('angular_Kp').get_parameter_value().double_valueself.angular_Ki = self.get_parameter('angular_Ki').get_parameter_value().double_valueself.angular_Kd = self.get_parameter('angular_Kd').get_parameter_value().double_valueself.scale = self.get_parameter('scale').get_parameter_value().integer_valueself.minDistance = self.get_parameter('minDistance').get_parameter_value().double_valueself.linear_PID = (self.linear_Kp, self.linear_Ki, self.linear_Kd)self.angular_PID = (self.angular_Kp, self.angular_Ki, self.angular_Kd)self.minDist = self.minDistance * 1000 #转换最小距离单位（米->毫米）def PID_init(self):# self.linear_pid = simplePID(self.linear_PID[0] / 1000.0, self.linear_PID[1] / 1000.0, self.linear_PID[2] / 1000.0)#self.angular_pid = simplePID(self.angular_PID[0] / 100.0, self.angular_PID[1] / 100.0, self.angular_PID[2] / 100.0)# PID参数除以scale进行归一化处理self.linear_pid = simplePID([0, 0],#目标值占位符[self.linear_PID[0]  / float(self.scale), self.linear_PID[0]  / float(self.scale)],[self.linear_PID[1]  / float(self.scale), self.linear_PID[1]  / float(self.scale)],[self.linear_PID[2]  / float(self.scale), self.linear_PID[2]  / float(self.scale)])def depth_img_Callback(self, msg):#图像回调if not isinstance(msg, Image): returndepthFrame = self.bridge.imgmsg_to_cv2(msg, desired_encoding=self.encoding[0])#图像转换self.action = cv.waitKey(1) #获取键盘输入if self.Center_r != 0:#检测到有效目标（半径！=0）now_time = time.time()if now_time - self.prev_time > 5:#超过5秒未更新则重置if self.Center_prevx == self.Center_x and self.Center_prevr == self.Center_r: self.Center_r = 0self.prev_time = now_timedistance = [0, 0, 0, 0, 0]#距离数据 5点平均if 0 < int(self.Center_y - 3) and int(self.Center_y + 3) < 480 and 0 < int(self.Center_x - 3) and int(self.Center_x + 3) < 640:#检测目标区域是否在图像范围内print("depthFrame: ", len(depthFrame), len(depthFrame[0])) #在目标周围5个点采样距离值distance[0] = depthFrame[int(self.Center_y - 3)][int(self.Center_x - 3)]distance[1] = depthFrame[int(self.Center_y + 3)][int(self.Center_x - 3)]distance[2] = depthFrame[int(self.Center_y - 3)][int(self.Center_x + 3)]distance[3] = depthFrame[int(self.Center_y + 3)][int(self.Center_x + 3)]distance[4] = depthFrame[int(self.Center_y)][int(self.Center_x)]distance_ = 1000.0 #默认值num_depth_points = 5for i in range(5):if 40 < distance[i].all() < 80000: distance_ += distance[i] #有效距离过滤else: num_depth_points -= 1 #无效点计数-1if num_depth_points == 0: distance_ = self.minDist #没有有效点，使用最小近距离else: distance_ /= num_depth_points #计算平均距离#print("Center_x: {}, Center_y: {}, distance_: {}".format(self.Center_x, self.Center_y, distance_))self.execute(self.Center_x, self.Center_y)#执行舵机控制self.Center_prevx = self.Center_xself.Center_prevr = self.Center_relse: #无有效目标停止if self.Robot_Run ==True:self.pub_cmdVel.publish(Twist()) self.Robot_Run = Falseif self.action == ord('q') or self.action == 113: self.cleanup()def JoyStateCallback(self, msg):#手柄if not isinstance(msg, Bool): returnself.Joy_active = msg.dataself.pub_cmdVel.publish(Twist())def positionCallback(self, msg):#目标位置回调if not isinstance(msg, Position): returnself.get_logger().info(f'positionCallback msg:{str(msg)}')self.Center_x = msg.anglexself.Center_y = msg.angleyself.Center_r = msg.distancedef cleanup(self):self.pub_cmdVel.publish(Twist())print ("Shutting down this node.")cv.destroyAllWindows()#舵机控制    def execute(self, point_x, point_y):#通过PID计算舵机调整量（参数是计算目标位置与图像中心的偏差，图像中心坐标（320，240））[x_Pid, y_Pid] = self.linear_pid .update([point_x - 320, point_y - 240])if self.img_flip == True:#根据图像翻转标识调整方向self.PWMServo_X += x_Pidself.PWMServo_Y += y_Pidelse:self.PWMServo_X  -= x_Pidself.PWMServo_Y  += y_Pid#角度限制（保护舵机）if self.PWMServo_X  >= 45:self.PWMServo_X  = 45elif self.PWMServo_X  <= -45:self.PWMServo_X  = -45if self.PWMServo_Y >= 20:self.PWMServo_Y = 20elif self.PWMServo_Y <= -90:self.PWMServo_Y = -90#rospy.loginfo("target_servox: {}, target_servoy: {}".format(self.target_servox, self.target_servoy))print("servo1",self.PWMServo_X)   servo1_angle = Int32()servo1_angle.data = int(self.PWMServo_X)servo2_angle = Int32()servo2_angle.data = int(self.PWMServo_Y)self.pub_Servo1.publish(servo1_angle)self.pub_Servo2.publish(servo2_angle)#PID控制器 
class simplePID:'''very simple discrete PID controller'''def __init__(self, target, P, I, D):#初始化pid'''Create a discrete PID controllereach of the parameters may be a vector if they have the same lengthArgs: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 = 0self.integrator = 0self.timeOfLastCall = Noneself.setPoint = np.array(target)self.integrator_max = float('inf')def update(self, current_value):'''Updates the PID controller. 更新PID控制器状态并返回控制量Args:current_value (double): vector/number of same legth as the target given in the constructorReturns: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 appliedself.timeOfLastCall = time.perf_counter()return np.zeros(np.size(current_value))error = self.setPoint - current_valueP = errorcurrentTime = time.perf_counter()deltaT = (currentTime - self.timeOfLastCall)# integral of the error is current error * time since last update  积分项计算self.integrator = self.integrator + (error * deltaT)I = self.integrator# derivative is difference in error / time since last update 微分项计算D = (error - self.last_error) / deltaTself.last_error = errorself.timeOfLastCall = currentTime# return controll signal 输出控制量return self.Kp * P + self.Ki * I + self.Kd * Ddef main():rclpy.init()color_tracker = color_Tracker("ColorTracker")print("start it")rclpy.spin(color_tracker)

• 控制流程

  • 订阅颜色识别节点发布的/Current_point目标位置

  • 通过深度图像计算目标距离

  • 使用PID控制器计算舵机调整量

  • 发布舵机控制指令实现云台跟踪

• 关键技术点

  • 深度数据处理：在目标区域采样多点深度值进行平均

  • 抗抖动设计：5秒无更新自动重置目标状态

  • 舵机平滑控制：PID输出限幅防止突变

  • 动态参数配置：通过ROS参数服务器实时调整PID参数

• colorHSV.py

  主要是完成图像处理，计算出被追踪物体的中心坐标。

• colorTracker.py

  主要是根据被追踪物体的中心坐标和深度信息，计算出舵机运动角度数据给底盘。

 

程序说明

MicroROS机器人颜色追踪，具备可以随时识别多种颜色，并自主储存当前识别的颜色，控制小车云台追随检测识别的颜色。

MicroROS机器人的颜色追踪还可以实现HSV实时调控的功能，通过调节HSV的高低阈值，过滤掉干扰的颜色，使得方块在复杂的环境中能够非常理想的被识别出来，如果在取色中效果不理想的话，这个时候需要将小车移动到不同环境下校准一下，以达到可以在复杂环境中，识别我们所需的颜色。

运行：

#小车代理
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 colorHSV

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 run yahboom_esp32ai_car colorHSV
import finish
cv_edition:  4.11.0
start it
[INFO] [1738674982.309149874] [ColorIdentify]: Track_state=identify
[INFO] [1738674982.524295927] [ColorIdentify]: Track_state=identify
[INFO] [1738674982.803851561] [ColorIdentify]: Track_state=identify
[INFO] [1738674982.973336708] [ColorIdentify]: Track_state=identify
...
[INFO] [1738675048.670016124] [ColorIdentify]: action=114
succes!!!
[INFO] [1738675048.673837152] [ColorIdentify]: Track_state=init
[INFO] [1738675048.985173614] [ColorIdentify]: Track_state=init
[INFO] [1738675053.159352295] [ColorIdentify]: Track_state=mouse
[INFO] [1738675053.755473605] [ColorIdentify]: Track_state=mouse
[INFO] [1738675054.460844705] [ColorIdentify]: Track_state=mouse
[INFO] [1738675054.804728880] [ColorIdentify]: Track_state=mouse
[INFO] [1738675055.570826483] [ColorIdentify]: Track_state=mouse
[INFO] [1738675251.874084436] [ColorIdentify]: action=32
[INFO] [1738675251.875798862] [ColorIdentify]: Track_state=tracking
[INFO] [1738675252.089742888] [ColorIdentify]: Track_state=tracking
[INFO] [1738675252.282038043] [ColorIdentify]: Track_state=tracking
[INFO] [1738675252.507568662] [ColorIdentify]: Track_state=tracking
[INFO] [1738675252.734285724] [ColorIdentify]: Track_state=tracking
[INFO] [1738675252.956037454] [ColorIdentify]: Track_state=tracking
[INFO] [1738675253.283054540] [ColorIdentify]: Track_state=tracking

程序启动后，会出现以下画面，

然后按下键盘上的r/R键进入选色模式，用鼠标框出一片区域（该区域只能有一中颜色），

选完后的效果

然后，按下空格键进入追踪模式，缓慢移动物体，能看到机器人云台会追踪色块运动。

遇到的问题

OpenCV中waitKey()函数失效问题

就是获取不到按键，导致没法切换。我问了下deepseek.

真的很棒，OpenCV的GUI窗口没有被聚焦。
waitKey() 函数只有在窗口获得焦点的时候才有效，如果焦点在电脑其他窗口上，OpenCV是不会接受按键事件的。鼠标点击GUI窗口就可以获得焦点。

对比下deepseek.我真的真的被自己蠢哭了。亚博官网的代码可能有bug,需要自己去验证下。

另外，测试过程中不能快了，速度快了摄像头跟不上移动就丢了，得慢慢移动才有效果。

以上。