亚博microros小车-原生ubuntu支持系列：23 人脸识别追踪

背景知识：

本节跟上一篇的物体识别追踪类似，换了opencv的函数来做人脸识别

函数定义如下：

detectMultiScale(image, scaleFactor, minNeighbors, flags, minSize, maxSize)

scaleFactor参数控制每个图像序列的缩放比例。该参数决定了在每个图像序列中检测窗口的大小。默认值为1.1，表示每次图像被缩小10%。较小的值可以捕捉更多的细节，但也会增加计算量。较大的值可以加快检测速度，但可能会错过一些目标。

minNeighbors参数定义了每个目标至少应该有多少个邻居，才能被认为是一个目标。该参数用于过滤检测到的目标。较大的值可以过滤掉一些误检测，但可能会导致一些目标被漏检。

flags参数用于定义检测模式。它可以是以下几个值的组合：

• CASCADE_SCALE_IMAGE：使用缩放图像进行检测（默认值）。
• CASCADE_FIND_BIGGEST_OBJECT：只检测最大的目标。
• CASCADE_DO_ROUGH_SEARCH：快速搜索模式。

minSize参数用于指定检测目标的最小尺寸。目标小于该尺寸的将被忽略。该参数可以用于过滤一些过小的目标。

maxSize参数用于指定检测目标的最大尺寸。目标大于该尺寸的将被忽略。该参数可以用于过滤一些过大的目标。

1、程序说明

运行程序后，当人脸展现在画面中时，当出现方框围住人脸时，云台摄像头会跟随人脸的移动而移动。

src/yahboom_esp32ai_car/yahboom_esp32ai_car/目录下新建文face_fllow.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 std_msgs.msg import Int32, Bool,UInt16
#common lib
import os
import threading
import math
import cv2
from yahboom_esp32ai_car.astra_common import *
from yahboomcar_msgs.msg import Position
from cv_bridge import CvBridge
from std_msgs.msg import Int32, Bool,UInt16from rclpy.time import Time
import datetime
from ament_index_python.packages import get_package_share_directory #获取shares目录绝对路径
print("import done")class faceTracker(Node):def __init__(self,name):super().__init__(name)#create the publisherself.pub_Servo1 = self.create_publisher(Int32,"servo_s1" , 10)self.pub_Servo2 = self.create_publisher(Int32,"servo_s2" , 10)#create the subscriber#self.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 = 1500self.Center_x = 0self.Center_y = 0self.Center_r = 0self.Center_prevx = 0self.Center_prevr = 0self.prev_time = 0self.prev_dist = 0self.prev_angular = 0self.Joy_active = Falseself.Robot_Run = Falseself.img_flip = Falseself.dist = []self.encoding = ['8UC3']self.linear_PID = (20.0, 0.0, 1.0)self.angular_PID = (0.5, 0.0, 2.0)self.scale = 1000self.end = 0self.PWMServo_X = 0self.PWMServo_Y = 0self.s1_init_angle = Int32()self.s1_init_angle.data = self.PWMServo_Xself.s2_init_angle = Int32()self.s2_init_angle.data = self.PWMServo_Yself.PID_init()self.declare_param()self.last_stamp = Noneself.new_seconds = 0self.fps_seconds = 1#self.capture = cv.VideoCapture(0)#self.timer = self.create_timer(0.001, self.on_timer)#ESP32_wifiself.bridge = CvBridge()self.sub_img = self.create_subscription(CompressedImage, '/espRos/esp32camera', self.handleTopic, 1) #获取esp32传来的图像#确保角度正常处于中间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)print("init done")def declare_param(self):#PIDself.declare_parameter("Kp",20)self.Kp = self.get_parameter('Kp').get_parameter_value().integer_valueself.declare_parameter("Ki",0)self.Ki = self.get_parameter('Ki').get_parameter_value().integer_valueself.declare_parameter("Kd",0.9)self.Kd = self.get_parameter('Kd').get_parameter_value().integer_valuedef get_param(self):self.Kd = self.get_parameter('Kd').get_parameter_value().integer_valueself.Ki = self.get_parameter('Ki').get_parameter_value().integer_valueself.Kp = self.get_parameter('Kp').get_parameter_value().integer_valueself.linear_PID = (self.Kp,self.Ki,self.Kd)# def on_timer(self):#     self.get_param()#     global m #     m=0#     global n#     n=0#     ret, frame = self.capture.read()#     action = cv.waitKey(10) & 0xFF#     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)#     face_patterns = cv2.CascadeClassifier('/root/yahboomcar_ws/src/yahboomcar_astra/yahboomcar_astra/haarcascade_frontalface_default.xml')#     faces = face_patterns.detectMultiScale(frame , scaleFactor=1.1, minNeighbors=5, minSize=(100, 100))#     if len(faces)>0:#         for (x, y, w, h) in faces:#             m=x#             n=y#             cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)#         self.execute(m,n)#     cv.imshow('frame', frame)#     if action == ord('q') or action == 113:#         self.capture.release()#         cv.destroyAllWindows()#ESP32_wifidef handleTopic(self, msg):self.get_param()global m m=0global nn=0self.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 = cv2.resize(frame, (640, 480))action = cv2.waitKey(10) & 0xFF#face_patterns = cv2.CascadeClassifier(get_package_share_directory('yahboom_esp32ai_car')+'/resource/haarcascade_frontalface_default.xml')faces = face_patterns.detectMultiScale(frame , scaleFactor=1.1, minNeighbors=5, minSize=(100, 100))if len(faces)>0:for (x, y, w, h) in faces:m=xn=ycv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)self.execute(m,n)end = time.time()fps = 1/((end - start)+self.fps_seconds) text = "FPS : " + str(int(fps))cv2.putText(frame, text, (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,255), 2)cv2.imshow('frame', frame)if action == ord('q') or action == 113:cv2.destroyAllWindows()def PID_init(self):self.PID_controller = 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)])self.pub_Servo1.publish(self.s1_init_angle)self.pub_Servo2.publish(self.s2_init_angle)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.Center_x = msg.anglexself.Center_y = msg.angleyself.Center_r = msg.distancedef execute(self, point_x, point_y):[x_Pid, y_Pid] = self.PID_controller.update([point_x - 320, point_y - 240])if abs(x_Pid) <1 and abs(y_Pid)<0.5:  #在死区直接不动了returnif self.img_flip == True:self.PWMServo_X += x_Pidself.PWMServo_Y += y_Pidelse:self.PWMServo_X  -= x_Pidself.PWMServo_Y  += y_Pidif self.PWMServo_X  >= 40:self.PWMServo_X  = 40elif self.PWMServo_X  <= -40:self.PWMServo_X  = -40if 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)class simplePID:'''very simple discrete PID controller'''def __init__(self, target, P, I, D):'''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.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 updateself.integrator = self.integrator + (error * deltaT)I = self.integrator# derivative is difference in error / time since last updateD = (error - self.last_error) / deltaTself.last_error = errorself.timeOfLastCall = currentTime# return controll signalreturn self.Kp * P + self.Ki * I + self.Kd * Ddef main():rclpy.init()face_Tracker = faceTracker("FaceTracker")try:rclpy.spin(face_Tracker)except KeyboardInterrupt:passfinally:face_Tracker.destroy_node()rclpy.shutdown()

跟上一篇物体识别追踪亚博microros小车-原生ubuntu支持系列：22 物体识别追踪-CSDN博客 类似，不在逐行添加注释。

启动小车代理

 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 face_fllow  

当识别人脸会框选，并且二维云台跟随人脸移动，并且终端打印移动角度

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 run yahboom_esp32ai_car face_fllow 
import done
init done
servo1 -1.870886676904979
servo1 -3.3315064420857414
servo1 -3.8893113859842563
servo1 -4.829714884815847
servo1 -5.531316057727904
servo1 -8.270412263987003
servo1 -11.28949508811162
servo1 -14.827409042746957
servo1 -17.313175450074567
servo1 -18.47017653776488
servo1 -16.712259138142592
servo1 -15.463574207134664
servo1 -14.357884507043561
servo1 -13.564746266546917
servo1 -13.171587288582277
servo1 -13.336302497052168
servo1 -13.420888481020322
servo1 -13.046533845705673
servo1 -12.81813413213153
servo1 -12.975443377657772
servo1 -14.095269899667365
servo1 -16.35166785571329
servo1 -18.550105463860874
servo1 -20.953949676274878
servo1 -24.326369597501582
servo1 -26.64879470036452
servo1 -25.940573656940963
servo1 -25.216060965804687
servo1 -24.53637502577782
servo1 -24.821431609929228
servo1 -26.048312267650324
servo1 -27.281132833944664
servo1 -28.032283744044005
servo1 -28.896142802075563
servo1 -30.097106599839762
servo1 -30.374701850607238
servo1 -29.952807303311566

rqt查看节点如下