### 1、程序功能说明
程序启动后，默认跟随的颜色是红色，可以按下 `r/R`（不区分大小写） 键进入选色模式，通过鼠标选定一种颜色，画面会锁定这个颜色，按下 `空格键` ，进入跟随模式。小车时刻保证被跟随的物体保持在画面中心，并且和物体保持0.4米。按下 `q/Q键` 则是退出程序。
### 2、程序代码参考路径

```
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/Advanced/colorTracker.py
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/astra_common.py
```
* colorTracker.py
主要是完成图像处理，根据被跟随颜色物体的中心坐标，和深度距离信息，计算出小车要前进的距离和转弯角度，发布控制方向、前进数据到小车。

* astra_common.py
颜色追踪（color_follow类）：通过HSV颜色空间识别和追踪特定颜色的物体
图像处理工具：包括多图像拼接显示（ManyImgs函数）、形状检测（is_regular_square函数）
文件读写功能：用于保存/读取HSV颜色范围
### 3、程序启动
#### 3.1、启动命令
终端输入，

```
#启动深度相机数据
ros2 launch ascamera hp60c.launch.py
#启动小车底盘
ros2 launch yahboomcar_bringup yahboomcar_bringup_M1_launch.py
#启动颜色跟随程序
ros2 run yahboomcar_depth depth_colorTracker
```
以追踪默认红色为例，程序启动后，按下空格，会出现以下画面

![](/media/202511/fb831168681140fa9e8d3fb85b9444ec1629.png)
然后按下键盘上的r/R键进入选色模式，用鼠标框出一片区域（该区域只能有一种颜色），
然后，按下空格键进入跟随模式，缓慢移动物体，小车会跟随移动，注意的是物体的深度信息如果是0的话小车会停下来，所以选择的物体建议大一点这样深度信息会更好。 
#### 3.2、动态参数调节
终端输入，
`ros2 run rqt_reconfigure rqt_reconfigure`

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

参数解析：

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

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

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

【scale】：PID比例缩放。

【Hmin】、【Smin】、【Vmin】：表示HSV的最低值

【Hmax】、【Smax】、【Vmax】：表示HSV的最大值
#### 4、核心代码
#### 4.1、colorTracker.py
这个程序主要有以下几个功能：

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

* 获取键盘鼠标事件，用于切换模式和取色；

* 处理图像得到物体的中心坐标，深度距离信息。

* 计算距离PID，中心坐标PID，获得小车前进速度距离、转弯角度。

部分核心代码如下，

```
# 重要的函数，获取到x值、y值和distance_值Important functions to get x value, y value and distance_ value
def process(self, rgb_img, action):

#目标中心点深度测量 Target center point depth measurement
if self.Center_r > 5:
    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 - 1), int(self.Center_x - 1))   # 左上偏移点 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)
    if valid_depths:
        dist = int(sum(valid_depths) / len(valid_depths))
    else:
        dist = 0
    self.dist = dist
   #当距离大于0.2米且启动状态为True时，调用 execute 方法控制机器人运动
#When the distance is greater than 0.2 meters and the start state is True, call the execute method to control the robot movement
   if dist > 0.2 and self.Start_state == True:
    self.execute(self.Center_x, dist)
    
# 根据x值、y值，使用PID算法，计算运动速度，转弯角度 According to the x value and y value, use the PID algorithm to calculate the movement speed and turning angle
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)
```

14、深度相机颜色跟随