### 1、程序功能说明
程序启动后，小车会识别出来的人脸并框选，之后小车会持续跟随人脸保持在画面中心，并且和识别到的人脸保持0.4米。按下 q/Q键 则是退出程序。
### 2、程序代码参考路径
`/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/Advanced/faceFollow.py`

主要完成人脸检测，根据人脸中心坐标、人脸距小车距离来计算小车前进距离和转弯角度，并发布前进、转弯数据到小车。
### 3、程序启动
#### 3.1、启动命令
终端输入：

```
#启动深度相机数据
ros2 launch ascamera hp60c.launch.py
#启动小车底盘
ros2 launch yahboomcar_bringup yahboomcar_bringup_M1_launch.py
#启动人脸跟随程序
ros2 run yahboomcar_depth depth_faceFollow
```
当检测到人脸时，会自动框出人脸区域，小车开始跟随。(**注意：小车如果运行一顿一顿效果不好时，可以更换一个深度信息更好识别的人脸照片，这是因为如果人脸深度信息为0时小车会停下来)**
程序启动后，会出现以下画面

![](/media/202511/eced146a8dea41528fd1c1797ca552153592.png)
框选人脸并且有深度信息的时候小车会进行跟随运动。
#### 3.2、动态参数调节
终端输入，
```
ros2 run rqt_reconfigure rqt_reconfigure
```

![](/media/202511/eabb98d22ad345edaea97d90b76481558578.png)
☑️ 修改完参数，点击GUI空白处写入参数值。注意只会生效当次启动，永久生效需要将参数修改到源码当中。

参数解析：

【linear_Kp】、【linear_Ki】、【linear_Kd】：小车跟随过程中的线速度PID调控。

【angular_Kp】、【angular_Ki】、【angular_Kd】：小车跟随过程中的角速度PID调控。

【minDist】：跟随距离，一直保持这个距离。

【scale】：PID比例缩放。

4、核心代码
4.1、depth_faceFollow.py
这个程序主要有以下几个功能：

订阅深度相机话题，获取话题图像；
使用OpenCV的人脸检测器检测人脸；
处理图像得到人脸图像的中心坐标，深度距离信息。
计算距离PID，中心坐标PID，获得小车前进速度距离、转弯角度。
部分核心代码如下，

```
#获取检测到人脸的方框大小
#Get the size of the box where the face is detected
faces = self.face_patterns.detectMultiScale(result_image , scaleFactor=1.2, minNeighbors=10, minSize=(50, 50))
#单人脸处理逻辑
#Single face processing logic
if len(faces) == 1:
    #人脸中心计算 #Face center calculation
    for (x, y, w, h) in faces:
        self.Center_x = x + w // 2  # X中心点 X center point
        self.Center_y = y + h // 2  # Y中心点 Y center point
        self.Center_r = w * h // 100  # 面积半径 Area radius
        cv.rectangle(result_image, (x, y), (x+w, y+h), (0,255,0), 2) # 绘制绿框 Draw a green frame
   #深度信息获取 Deep information acquisition
   points = [
    (int(self.Center_y), int(self.Center_x)),     # 中心点 Center Point
    (int(self.Center_y-1), int(self.Center_x-1)), # 右下偏移点 Lower right offset point
    (int(self.Center_y-3), int(self.Center_x-3))  # 左上偏移点 Upper left offset point
]
    valid_depths = []
    for y, x in points:
        depth_val = depth_image_info[y][x]
        if depth_val != 0:  
            valid_depths.append(depth_val)
    #机器人控制 Robot control
   if dist > 0.2:  # 有效距离阈值 Effective distance threshold
        self.execute(self.Center_x, dist)  # 执行控制逻辑 Execution control logic
        
# 根据x值、y值，使用PID算法，计算运动速度，转弯角度
# Calculate the movement speed and turning angle using the PID algorithm based on the x and y values
def execute(self, rgb_img, action):
    #PID计算偏差 PID calculation deviation
    linear_x = self.linear_pid.compute(dist, self.minDist)
    angular_z = self.angular_pid.compute(point_x,320)
    # 小车停车区间，速度为0 The car is in the parking area and the speed is 0
    if abs(dist - self.minDist) < 30: linear_x = 0
    if abs(point_x - 320.0) < 30: angular_z = 0
    twist = Twist()
    ...
    # 将计算后的速度信息发布 Publish the calculated speed information
    self.pub_cmdVel.publish(twist)
 
```

15、深度相机人脸跟随