### 视觉循迹自动驾驶
### 1、程序功能说明
程序启动后，调整摄像头的俯仰角，把摄像头往下掰动，使得摄像头可以看到线，然后点击图像窗口，按下r键进选色模式；接着在画面中的线的区域内，框出所需要巡线的颜色，松开鼠标后会自动加载处理后的图像；最后按下空格键开启巡线功能。小车在运行过程中，遇到障碍物会停下并且蜂鸣器会响；开启手柄控制程序后，手柄上R2键可以暂停小车运动。
### 2、程序代码参考路径

```
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/Advanced/follow_line.py
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_depth/yahboomcar_depth/follow_common.py
```
* follow_line.py

主要完成图像处理，根据标定后的颜色获取中心坐标，订阅雷达话题数据。计算出小车前进的转弯角度、计算和障碍物的距离停下来警告，发布前进、转弯、蜂鸣器数据给小车

* follow_common.py

文件读写功能：用于保存/读取HSV颜色范围

图像处理工具：包括多图像拼接显示（ManyImgs函数）

颜色巡线（line_follow类）：通过HSV颜色空间获取提取颜色中心坐标和半径

ROI获取HSV：通过提roi范围来获取该范围内的hsv值
### 3、程序启动
#### 3.1、启动命令
终端输入，

```
#启动深度相机数据
ros2 launch ascamera hp60c.launch.py
#启动小车底盘、雷达
ros2 launch yahboomcar_bringup laser_bringup_launch.py
#启动颜色跟随程序
ros2 run yahboomcar_depth depth_follow_line
```
程序运行之后会出现以下画面，以巡绿线为例，

![](/media/202511/b62eec84d1ab4751b1dde3e5b49edbe09815.png)
开机默认会使用上一次的hsv数值，现在需要重新取色，然后按下键盘上的r/R键进入选色模式，用鼠标框出一片区域（该区域只能有一种颜色），

![](/media/202511/f5652762879a4e52ba61c8beb094625e4928.png)
选完后，效果如下图，
然后，按下空格键，开始计算速度，小车巡线自动驾驶。巡线过程中遇到障碍物会停下来嘀嘀嘀叫。
3.2、动态参数调节
终端输入，

`ros2 run rqt_reconfigure rqt_reconfigure`

![](/media/202511/7f5c578ac3e24c81b6626835cc15d12e9516.png)

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

参数解析：

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

【Hmax】、【Smax】、【Vmax】：表示HSV的最大值

【Kp】、【KI】、【KD】：表示调节转弯角度的pid的三个参数

【scale】：PID调节比例系数

【LaserAngle】：雷达检测的角度

【linear】：线速度大小

【ResponseDist】：避障检测距离

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

计算巡线的中心坐标与图像中心的偏移量，

根据坐标偏移量计算出角速度的值，

发布速度驱动小车。

订阅雷达话题数据，计算障碍物距离

这个程序主要有以下几个功能：

计算中心坐标，

```
#计算hsv值 #Calculate HSV value
rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)
#计算self.circle，计算出X的坐标、半径值。半径值为0说明没有检测到线，则发布停车信息
#Calculate self.circle and determine the coordinates and radius value of X. A radius value of 0 indicates that no line was detected, and parking information will be published
rgb_img, binary, self.circle = self.color.line_follow(rgb_img, self.hsv_range)
```
计算出角速度的值，

```
#320是中心点的X坐标的值，通过得到的图像的X值与320的偏差，可以计算出“我现在距离中心有多远”，然后计算角速度的值
#320 is the value of the X coordinate of the center point. By calculating the deviation between the X value of the obtained image and 320, we can determine "how far am I from the center now" and then calculate the value of angular velocity
[z_Pid, _] = self.PID_controller.update([(point_x - 320)*1.0/16, 0])
```

18、深度相机视觉循迹自动驾驶