### KCF物体跟随
#### 1、程序功能说明
程序启动后，可以通过鼠标在画面中框选需要跟随的物体，按下 空格键 ，小车进入跟踪模式。小车会与被跟踪的物体保持0.4米的距离，并且时刻保证被追踪的物体保持在画面中心。按下R键则进入选择模式，按下Q键则退出程序。。

**⚠️该功能对框选区域大的物体，跟随效果会比较好，缓慢移动否则容易丢失目标。**

### 2、程序代码参考路径

```
/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/kcf/KCF_Tracker.py
/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/astra_common.py
```
mono_Tracker.py

主要是完成物体检测与追踪，根据检测到的物体中心坐标，计算出小车前进的转弯角度和前进距离速度，发布控制舵机角度、电机控制速度数据到小车。

astra_common.py

函数（Tracker）类：实现多种OpenCV追踪算法（BOOSTING、KCF、CSRT等）

提供追踪器初始化（initWorking）和实时更新（track）接口

### 3、程序启动
#### 3.1、启动命令
终端输入：

```
#启动深度相机数据
ros2 launch ascamera hp60c.launch.py
#启动小车底盘、雷达
ros2 launch yahboomcar_bringup laser_bringup_launch.py
#启动颜色跟随程序
ros2 run yahboomcar_depth KCF_Tracker
```
程序启动后，会出现以下画面：

![](/media/202511/6903c6d7a4e6488b8e8132a4942474494806.png)
使用鼠标在画面中框选需要追踪的物体，（如果容易跟丢的话建议框选大一点明显一点的物体）

![](/media/202511/07af9daecd5948528b48f8eba3a556743218.png)
然后，按下空格键进入跟随模式，缓慢移动物体，小车会跟着移动并且与物体保持1m的距离。
![](/media/202511/aedb14fb600b4f8781b4c84f85a946963344.png)
### 3.2、动态参数调节
终端输入，`ros2 run rqt_reconfigure rqt_reconfigure`
![](/media/202511/333809d2359b44878b3f21098d1b52c85138.png)

☑️ 修改完参数，点击GUI空白处写入参数值。注意只会生效当次启动，永久生效需要将参数修改到源码当中。

参数解析：

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

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

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

【scale】：PID比例缩放。

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

* 初始化KCF追踪器

* 订阅深度话题，获取图像

* 通过鼠标交互选择追踪目标

* 计算目标中心坐标并发布

* 使用PID算法计算舵机角度、前进速度并发布控制指令

部分核心代码如下

```
#追踪初始化
self.tracker_type = 'KCF'

#距离计算 #Distance calculation
points = [
        (int(self.cy), int(self.cx)),         # 中心点
        (int(self.cy +1), int(self.cx +1)),  # 右下偏移点
        (int(self.cy - 1), int(self.cx - 1))   # 左上偏移点
    ]
valid_depths = []
for y, x in points:
    depth_val = depth_image_info[y][x]
    if depth_val != 0:
        valid_depths.append(depth_val)
if valid_depths:
    dist = int(sum(valid_depths) / len(valid_depths))
else:
    dist = 0
#控制执行 #Control execution
if dist > 0.2:  # 有效距离阈值(0.2米) Effective distance threshold (0.2 meters)
    self.execute(self.Center_x, dist)  # 执行控制 Execution control
# 根据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)
```

16、深度相机KCF物体跟随