## 1、开启自动驾驶程序

>i **信息提示**
>
> 注意：由于图像显示需要用到桌面显示，所以需要连接显示屏或者使用VNC远程登录桌面。

#### 我们需要先在终端进入自动驾驶程序所在文件夹再运行python程序：打开终端运行下面两条命令
`cd ~/Rosmaster/auto_drive`
`python3 yolov5_auto.py`

![](/media/202511/dacf58fc78584dd69c365d04e38624f09062.png)

>i **信息提示**
>运行成功后，大概等待3分钟左右程序就会弹出摄像头，小车会一直前进并且前轮舵机会摆动一下：若舵机没有摆动，等待一段时间，若长时间没有摆动，可能是程序初始化失败，可以重新运行程序或者重新插拔PWM舵机接口运行程序。

### 自动驾驶功能
自动驾驶使用Yolov5进行物体识别，根据识别的路标和赛道黄线控制小车运动：

所以摄像头采集的画面会影响R2L小车自动驾驶效果。

程序运行后，小车会向前运动，我们可以将小车放到地图其中一个拐角处，看一下拐弯的效果：我们可以使用自动驾驶配套的STOP路标控制小车停止，小车停止后我们调整摄像头的俯仰角，使用前进路标控制小车前进，然后观察小车拐弯的效果：小车若单个拐弯处没有问题，我们可以让小车跑整个自动驾驶地图，这个过程我们可以微调摄像头高度，确保效果是最好的。

<span style="color: red;">小车拐弯压赛道内侧黄线，我们需要将摄像头调低；小车拐弯压赛道外侧黄线，我们需要将摄像头调高。</span>
================分割线===============================
记录2025年11月29日，四个直角转弯比较完美的数值：
`self.FollowLinePID = (60, 25, 20)第61行 ` 
```
[(point_x - 30)*1.2/32,0]  # 第183行
[(610-point_x)*1.2/32, 0]  # 第18行
```
以下代码在：/Rosmaster/auto_drive/yolov5_auto.py
全局速度：
`self.car_ speed=0.26`
=============分割线===================================
###  PID简单解析（右转压内弯）：修改：line_detect.py
`self.FollowLinePID = (60, 0, 20)  # P:60, I:0, D:20`原来的 这行代码在第61行
`self.FollowLinePID = (40, 5, 15)  # P:40, I:5, D:15` 修改后
**减小 P 值：从 60→40，降低比例增益，减少转向灵敏度
增加 I 值：从 0→5，添加积分项消除静态误差，让机器人更稳定
减小 D 值：从 20→15，降低微分项，减少高频抖动**

修改第169行和174行

```
[(point_x - 30)*1.8/32,0]  # 第183行
[(610-point_x)*1.8/32, 0]  # 第18行
这是原代码
```
```
把参数修改为这样
[(point_x - 30)*1.2/32,0]  # 减小系数到1.2
[(610-point_x)*1.2/32, 0]  # 减小系数到1.2
```
**修改原因：
减小归一化系数，降低 PID 输入的幅度
进一步降低转向灵敏度，防止转弯过猛**

#### 角速度阈值调整
`if abs(self.go_angular_z)<0.1:  # 第194行`
改为：`if abs(self.go_angular_z)<0.05:  # 降低阈值到0.05`

**修改原因：
降低最小角速度阈值
确保机器人在小偏移时也能有足够的转向响应**

#### 线速度调整
原代码：
`self.linear = 0.2  # 第63行`
修改为（如果上述调整后仍有问题）：
`self.linear = 0.15  # 降低线速度`
**修改原因：
降低机器人行驶速度
给 PID 控制器更多时间进行转向调整**

| 参数    | 原值     | 修改后值     | 作用          |
|-------|--------|----------|-------------|
| P 值   | 60     | 40       | 降低转向灵敏度     |
| I 值   | 0      | 5        | 消除稳态误差      |
| D 值   | 20     | 25       | 增加阻尼，抑制振荡   |
| 归一化系数 | 1.8/32 | 1.2/32   | 降低 PID 输入幅度 |
| 角速度阈值 | 0.1    | 0.05     | 提高小偏移响应     |
| 线速度   | 0.2    | 0.15（可选） | 降低行驶速度      |

**调整原理说明
PID 参数是核心：P 值过大是导致转向过度压内线的主要原因
归一化系数辅助：配合 PID 参数调整，进一步控制转向幅度
阈值调整补充：确保系统在各种情况下都能稳定工作
调试建议
先调整 PID 参数：这是最关键的一步
再调整归一化系数：如果转向仍然过度
最后调整线速度：如果需要更稳定的行驶**

**这些参数调整应该能够有效解决机器人转弯压内线的问题，同时保持代码结构不变。如果调整后仍有问题，可以尝试进一步减小 P 值或归一化系数。**

======================分隔符===========================
**文件路径：/Rosmaster/auto_drive/yolov5_auto.py**
## 自动驾驶数据调试
self.auto_control变量是控制是否由小车自动驾驶，如果设置为True表示小车会根据摄像头读取的画面分析并控制小车前轮方向，如果设置为False表示小车需要执行特定的功能。

```
def car_run(self):
        self.car.set_beep(500)
        self.car.set_car_motion(self.car_speed , 0, 0)
        self.auto_control = True  //这里设置为True就是开启自动驾驶
        self.car_state = 1
```
## **单个路标调试**
程序识别路标后的动作是固定的，所以不同的环境可能效果不同，我们需要掌握微调的方法。

接下来，我们分析几种可能需要微调代码的路标：右转、倒车A库，倒车B库

其它路标主要取决识别的准确率，我们只需要调整路标的摆放角度提高识别的准确率，一般不需要微调代码。
### 右转
程序识别到右转路标，蜂鸣器会发出滴的一声，我们可以根据这个提示判断小车一般在什么位置识别右转路标，以及识别后的延时和转速是否合适，若发现程序不适合自己的环境，我们可以调整这个延时函数和小车旋转的速度和弧度，控制小车以最佳的位置转入倒车入库的赛道；

```
def car_turn_right(self):
        self.car.set_beep(500)  // 识别路标后，蜂鸣器滴一声
        time.sleep(5)   // 保持上一个状态5s
        self.auto_control = False   //关闭 自动驾驶
        time.sleep(.1)
        for i in range(20):
            self.car.set_car_motion(self.car_speed , 0, -2.5)   // 小车右转
            time.sleep(.1)
        self.auto_control = True    // 开启自动驾驶
```
### 倒车A库
程序识别到PA路标后，蜂鸣器会发出滴的一声，我们可以根据这个提示判断小车一般在什么位置识别PA路标，以及识别后的前进、右后方后退、后退、右前方前进、前进的时间是否合理，我们可以微调这些代码达到最好的效果。

```
def car_parking_A(self):#倒车A库
    self.car.set_beep(500)              # 识别路标后，蜂鸣器滴一声
    self.car.set_car_motion(0, 0, 0)    # 小车停止
    time.sleep(1)                       # 等待1秒
    self.car.set_car_motion(0.25, 0, 0)
# 0.25代表车的前进速度，第一个0控制小车的横向移动，第二个0控制小车的转向角度
    time.sleep(4)                       # 保持前进4秒
    self.auto_control = False           # 关闭自动控制
    self.car.set_car_motion(0, 0, 0)    # 小车停止
    time.sleep(.1)                      # 等待0.1秒
    self.car.set_car_motion(-0.25, 0, 1.5)  # 小车向右后方后退
#-0.25代表车的后退速度，第一个0控制小车的横向移动，第二个1.5控制小车的转向角度
    time.sleep(1)                       # 保持右后倒车1秒
    self.car.set_car_motion(-0.25, 0, 0)    # 后退
#-0.25代表车的后退速度，第一个0控制小车的横向移动，第二个0控制小车的转向角度
    time.sleep(2.5)                     # 保持后退2.5秒
    self.car.set_car_motion(0.25, 0, -1.5)  # 小车向右前方前进
#0.25代表车的后退速度，第一个0控制小车的横向移动，第二个-1.5控制小车的转向角度
    time.sleep(1.25)                    # 保持右前前进1.25秒
    self.car.set_car_motion(-0.25, 0, 0)    # 后退
# 0.25代表车的前进速度，第一个0控制小车的横向移动，第二个0控制小车的转向角度
    time.sleep(1)                       # 保持后退1秒
    self.car.set_car_motion(0, 0, 0)   # 小车停止
    self.car.set_beep(500)              # 蜂鸣器滴一声提示完成

```
### 倒车B库
同样的，程序识别到PB路标后，蜂鸣器会发出滴的一声，我们可以根据这个提示判断小车一般在什么位置识别PB路标，以及识别后的前进、右后方旋转、前进的时间是否合理，我们可以微调这些代码达到最好的效果。

```
def car_parking_B(self): #倒车B库
        self.car.set_beep(500) # 识别路标后，蜂鸣器滴一声
        self.car.set_car_motion(0, 0, 0) # 小车停止
        time.sleep(1)# 等待1秒
        self.car.set_car_motion(0.25, 0, 0)
# 0.25代表车的前进速度，第一个0控制小车的横向移动，第二个0控制小车的转向角度
        time.sleep(3)
        self.auto_control = False
        self.car.set_car_motion(0, 0, 0)# 小车停止
        time.sleep(.1)
        self.car.set_car_motion(-0.25, 0, 1.5)  // 小车向右后方旋转
        time.sleep(3.8) 保持3.8秒
        self.car.set_car_motion(0.25, 0, 0)
        time.sleep(.5)
        self.car.set_car_motion(0, 0, 0)
        self.car.set_beep(500)
```
### 其它路标现象
下面路标一般不需要调试，如果有需要，可以自己修改代码。

#### 前进路标
蜂鸣器滴一声，开启小车自动驾驶：小车在无其它路标沿跑道行驶。

```
def car_run(self):
        self.car.set_beep(500)
        self.car.set_car_motion(self.car_speed , 0, 0) # 设置小车以前进速度直线行驶
        self.auto_control = True # 开启自动控制模式
        self.car_state = 1 # 设置小车状态为运行中（状态码1）
```
#### 鸣笛路标
蜂鸣器滴一声，并设置500毫秒延迟时间

```
def car_whistle(self):
        self.car.set_beep(500)# 设置蜂鸣器发出500毫秒的鸣笛声
        time.sleep(.5)# 等待0.5秒，确保鸣笛完成
```
#### 人行道路标
蜂鸣器滴一声，小车先停止2s时间，然后再继续前进。

```
def car_sidewalk(self):
    self.car.set_beep(500)      # 发出蜂鸣声提示开始避让操作
    self.auto_control = False   # 关闭自动控制模式，转为手动控制
    time.sleep(.1)              # 短暂等待0.1秒确保模式切换完成
    self.car_stop()             # 调用小车停止方法，确保完全停止
    time.sleep(2)               # 等待2秒，观察路况或等待行人通过
    self.car_run()              # 重新启动小车运行
    self.auto_control = True    # 恢复自动控制模式
    time.sleep(3)               # 保持运行3秒，完成避让后的正常行驶

```
#### 速度限制路标
蜂鸣器滴一声，设置自动驾驶速度0.4m/s行驶3s，之后恢复原速度。

```
def car_limiting_velocity(self):
    self.car.set_beep(500)                  # 发出蜂鸣声提示开始限速操作
    self.auto_control = True                # 开启自动控制模式
    self.car.set_car_motion(0.4, 0, 0)     # 设置小车以0.4速度直线行驶（限速状态）
    time.sleep(3)                           # 保持限速状态3秒
    self.car.set_car_motion(self.car_speed, 0, 0)  # 恢复到正常预设速度行驶

```
#### 停车路标
蜂鸣器滴一声，关闭自动驾驶功能。

```
def car_shutdown(self):
    self.car.set_beep(500)      # 发出蜂鸣声提示开始关机操作
    self.auto_control = False   # 关闭自动控制模式
    time.sleep(.1)              # 短暂等待0.1秒确保模式切换完成
    self.car_stop()             # 调用小车停止方法，完全停止所有运动

```
#### 学校路段路标
蜂鸣器滴一声，降低小车自动驾驶速度5s，之后恢复原速度。

```
def car_school_decelerate(self):
    self.car.set_beep(500)                  # 发出蜂鸣声提示进入学校区域
    self.auto_control = True                # 保持自动控制模式
    self.car.set_car_motion(0.1, 0, 0)     # 设置小车以0.1的低速直线行驶
    time.sleep(5)                           # 保持低速行驶5秒（通过学校区域）
    self.car.set_car_motion(self.car_speed, 0, 0)  # 恢复到正常预设速度行驶

```
交通灯路标

```
交通灯需放在跑道中间效果更佳，放在道路两侧可能识别效果不佳！
```
交通灯路标只能识别红灯和无灯两种情况，即红灯停无灯行：

红灯情况：蜂鸣器滴一声，关闭自动驾驶，小车后退1s。

无灯情况：蜂鸣器滴一声，开启自动驾驶。

```
def car_no_light(self):
        self.car.set_beep(500)
        self.car_run()
        time.sleep(5)
        
def car_red_light(self):
    self.car.set_beep(500)
    if self.car_state == 1:
        self.auto_control = False
        self.car_stop()
        time.sleep(.5)
        self.car.set_car_motion(-self.car_speed , 0, 0.01)
        time.sleep(1)
        self.car_stop()
        time.sleep(.1)
        self.car_state = 0
```
### 注意事项
由于整个过程都是依靠视觉识别和程序的固定控制达到自动驾驶效果，用户需要耐心调试程序甚至自己优化程序。
**路标识别效果不好**
1.可以尝试修改小车自动驾驶的全局速度，降低小车速度

```
def __init__(self):
    self.device = "nano4G"        # 设备型号标识，使用Nano 4G开发板
    self.yolo_busy = False        # YOLO目标检测是否正在处理中
    self.yolo_start = False       # YOLO检测是否已启动
    self.car_enable = False       # 小车是否启用（使能状态）
    self.auto_control = False     # 是否处于自动控制模式
    self.road_sign_count = 1      # 路标计数器，用于记录识别的路标数量
    self.car_state = 1            # 小车状态码，1通常表示待机或准备状态
    self.car_start = False        # 小车是否已启动运行
    self.car_speed = 0.25         # 小车默认行驶速度设置

```
2.调整自动驾驶场所以及路标的摆放角度

3.用户若对模型训练比较熟悉，可以自行训练模型，增加训练集可以提供识别准确率
**单个路标效果不好**
可以修改程序中是识别到路标的单个功能的程序代码，调整延时和小车控制代码达到最佳效果！

## 文件解析：

```
#!/usr/bin/env python3              # 指定使用Python 3解释器执行此脚本
# encoding: utf-8                   # 指定文件编码为UTF-8，确保中文等字符正常显示
import base64                       # 导入base64模块，用于图像数据的编码和解码
import sys                          # 导入系统模块，提供对解释器使用或维护的变量和函数
import time                         # 导入时间模块，提供时间相关功能如延时
import cv2 as cv                    # 导入OpenCV计算机视觉库并重命名为cv
from yolov5_trt import YoLov5TRT  # 从yolov5_trt模块导入YoLov5TRT类，用于TensorRT加速的YOLOv5推理
import numpy as np                  # 导入NumPy数值计算库并重命名为np
import threading                    # 导入线程模块，用于多线程编程

import torch                        # 导入PyTorch深度学习框架
from torch2trt import TRTModule    # 从torch2trt模块导入TRTModule类，用于PyTorch到TensorRT的转换
from utils import preprocess        # 从utils模块导入preprocess函数，用于图像预处理

from Rosmaster_Lib import Rosmaster # 从Rosmaster_Lib模块导入Rosmaster类，用于控制智能小车硬件
import time                         # 重复导入时间模块（可优化删除）

class YoloDetect:                   # 定义YoloDetect类，用于封装YOLO检测和小车控制功能
    def __init__(self):             # 类初始化方法
        self.device = "nano4G"      # 设置设备型号为NVIDIA Jetson Nano 4G
        self.yolo_busy = False      # 初始化YOLO忙碌状态为False，表示当前未进行检测
        self.yolo_start = False      # 初始化YOLO启动状态为False，表示检测功能未激活
        self.car_enable = False      # 初始化小车使能状态为False，表示小车未启用
        self.auto_control = False    # 初始化自动控制状态为False，表示当前为手动控制模式
        self.road_sign_count = 1     # 初始化路标计数器为1，用于统计识别的路标数量
        self.car_state = 1           # 初始化小车状态为1，可能表示待机或准备状态
        self.car_start = False       # 初始化小车启动状态为False，表示小车未开始运行
        self.car_speed = 0.2         # 设置小车默认行驶速度为0.2
   def cancel(self):
        if self.car_enable:                    # 检查小车是否处于启用状态
            self.car_stop()                    # 调用小车停止方法，停止所有运动
            self.car_enable = False            # 将小车启用状态设置为False，禁用小车
        if self.yolo_start:                    # 检查YOLO目标检测是否已启动
            while self.yolo_busy:              # 循环等待，直到YOLO不再忙碌
                pass                            # 空操作，用于等待YOLO完成当前检测任务
            self.yolov5_wrapper.destroy()      # 销毁YOLO包装器实例，释放相关资源
            self.yolo_start = False            # 将YOLO启动状态设置为False
def load_yolo(self):
        param_ = "/home/jetson/ROS/R2L/yahboomcar_ws/src/yahboomcar_yolov5/param/"  # 定义YOLO参数文件的基础路径
        # param_ = "/home/jetson/Rosmaster/auto_drive/yolov5/"  # 注释掉的备用参数路径
        file_yaml = param_ + 'traffic.yaml'  # 拼接完整的交通标志配置文件路径
        PLUGIN_LIBRARY = param_ + self.device + "/libmyplugins.so"  # 根据设备类型构建插件库路径
        engine_file_path = param_ + self.device + "/yolov5s.engine"  # 构建TensorRT引擎文件路径
        self.yolov5_wrapper = YoLov5TRT(file_yaml, PLUGIN_LIBRARY, engine_file_path)  # 创建YOLOv5 TensorRT包装器实例
        self.yolo_start = True  # 将YOLO启动状态设置为True，表示检测功能已就绪

def detect(self, frame):
        try:
            score = -0.1  # 初始化最高置信度为-0.1（低于有效阈值）
            classid = -1  # 初始化类别ID为-1（无效值）
            index = 0  # 初始化最佳检测结果的索引为0
            frame, result_boxes, result_scores, result_classid = self.yolov5_wrapper.infer(frame)  # 调用YOLO推理方法处理输入帧
            for i in range(len(result_scores)):  # 遍历所有检测结果的置信度分数
                if result_scores[i] > score:  # 如果当前分数高于已知最高分
                    index = i  # 更新最佳检测结果的索引
                    score = float(result_scores[i])  # 更新最高置信度分数
                    classid = int(result_classid[i])  # 更新对应的类别ID
            if len(result_boxes) > 0:  # 检查是否存在有效的检测框
                box = result_boxes[index]  # 获取置信度最高的检测框坐标
                text ="{}:{:.2f}".format(self.yolov5_wrapper.categories[classid], score)  # 格式化显示文本：类别名+置信度
                self.yolov5_wrapper.plot_one_box(box, frame, label=text)  # 在图像上绘制检测框和标签
                return frame, classid, score, box  # 返回处理后的图像、类别ID、置信度和检测框
        except:  # 捕获所有可能的异常
            pass  # 异常发生时静默处理，不中断程序
        return frame, 0, 0, [0, 0, 0, 0]  # 返回默认值，表示检测失败或无有效结果
    
    def car_stop(self):
        self.auto_control = False  # 关闭自动控制模式
        self.car.set_car_motion(0, 0, 0)  # 设置小车运动参数为全零，实现完全停止
        
    def car_run(self):
        self.car.set_car_motion(self.car_speed , 0, 0)  # 设置小车以预设速度直线前进
        self.auto_control = True  # 开启自动控制模式
        self.car_state = 1  # 设置小车状态为1（运行状态）
    
    def car_turn_right(self):
        time.sleep(5)  # 等待5秒，保持当前状态
        self.auto_control = False  # 关闭自动控制模式，准备执行转向操作
        time.sleep(.1)  # 短暂等待0.1秒，确保模式切换完成
        for i in range(20):  # 循环执行20次转向调整
            self.car.set_car_motion(self.car_speed , 0, -3.0)  # 设置小车以预设速度向右转向（负转向值）
            time.sleep(.1)  # 每次转向后等待0.1秒，实现平滑转向
        self.auto_control = True  # 转向完成后重新开启自动控制模式

def car_whistle(self):
        self.car.set_beep(500)  # 设置蜂鸣器发出500毫秒的鸣笛声
        time.sleep(.5)  # 等待0.5秒，确保鸣笛完成
    def car_sidewalk(self):
        self.auto_control = False  # 关闭自动控制模式
        time.sleep(.1)  # 等待0.1秒，确保模式切换稳定
        self.car_stop()  # 调用小车停止方法，确保完全停止
        time.sleep(2)  # 等待2秒，观察路况或等待行人通过
        self.car_run()  # 调用小车运行方法，重新启动
        self.auto_control = True  # 重新开启自动控制模式
        time.sleep(3)  # 保持运行状态3秒，完成避让后的正常行驶
def car_limiting_velocity(self):
        self.auto_control = True  # 开启自动控制模式
        self.car.set_car_motion(0.4 , 0, 0)  # 设置小车以0.4速度直线行驶（限速状态）
        time.sleep(3)  # 保持限速状态3秒
        self.car.set_car_motion(self.car_speed , 0, 0)  # 恢复到正常预设速度行驶
        
    def car_shutdown(self):
        self.auto_control = False  # 关闭自动控制模式
        time.sleep(.1)  # 等待0.1秒确保模式切换完成
        self.car_stop()  # 调用小车停止方法，完全停止所有运动
        
    def car_school_decelerate(self):
        self.auto_control = True  # 保持自动控制模式
        self.car.set_car_motion(0.1 , 0, 0)  # 设置小车以0.1的低速直线行驶（学校区域减速）
        time.sleep(5)  # 保持低速行驶5秒（通过学校区域）
        self.car.set_car_motion(self.car_speed , 0, 0)  # 恢复到正常预设速度行驶
    
    def car_parking(self):
        self.car.set_car_motion(0, 0, 0)  # 停止小车所有运动
        time.sleep(1)  # 等待1秒确保完全停止
        self.car.set_car_motion(0.25, 0, 0)  # 设置小车以0.25速度直线前进
        time.sleep(5)  # 保持前进状态5秒
        self.auto_control = False  # 关闭自动控制模式
        self.car.set_car_motion(0, 0, 0)  # 再次停止小车运动
        time.sleep(.1)  # 等待0.1秒
        self.car.set_car_motion(-0.25, 0, 1.5)  # 设置小车以-0.25速度向右后方倒车（转向角1.5）
        time.sleep(4)  # 保持右后方倒车4秒
        self.car.set_car_motion(-0.25, 0, 0)  # 设置小车以-0.25速度直线后退
        time.sleep(1)  # 保持后退1秒
        self.car.set_car_motion(0, 0, 0)  # 完全停止小车运动，完成停车
        
    def car_no_light(self):
        self.car_run()  # 调用小车运行方法，正常行驶
        time.sleep(5)  # 保持运行5秒
        
    
    def car_red_light(self):
        if self.car_state == 1:  # 检查小车是否处于运行状态（状态码1）
            self.auto_control = False  # 关闭自动控制模式
            self.car_stop()  # 调用小车停止方法
            time.sleep(.5)  # 等待0.5秒
            self.car.set_car_motion(-self.car_speed , 0, 0.01)  # 设置小车以负速度轻微左转（模拟等待红灯时的微调）
            time.sleep(2)  # 保持调整状态2秒
            self.car_stop()  # 再次停止小车运动
            time.sleep(.1)  # 等待0.1秒
            self.car_state = 0  # 将小车状态设置为0（停止状态）
def cal_box_area(self, box):
        area = 0  # 初始化面积变量为0
        width = int(box[2] - box[0])  # 计算检测框的宽度（x2-x1）并转换为整数
        height = int(box[3] - box[1])  # 计算检测框的高度（y2-y1）并转换为整数
        area = width * height  # 计算检测框的面积（宽度×高度）
        return area  # 返回计算得到的面积值
    
    # categories: ["Go_straight","Turn_right","whistle","Sidewalk","Limiting_velocity","Shutdown","School_decelerate","Parking_lotB","Parking_lotA","No_light","Red_light"]  # 类别名称列表，对应不同的交通标志
    def control_car(self, classid, score, box):
        if classid >= 0:  # 检查类别ID是否有效（大于等于0表示有检测结果）
            # print("classid:", classid)  # 注释掉的调试信息输出
            detect_classid = int(classid)  # 将类别ID转换为整数
            detect_score = float(score)  # 将置信度分数转换为浮点数
            area = self.cal_box_area(box)  # 调用方法计算检测框的面积
            # if detect_score < 0.7 or area < 2000:  # 注释掉的原始过滤条件
            if (detect_score < 0.7 or area < 2000) and detect_classid < 9:  # 如果置信度低于0.7或面积小于2000，并且类别ID小于9
                # print("detect_classid:", detect_classid)  # 注释掉的调试信息输出
                return  # 满足过滤条件时直接返回，不执行后续控制
            print("detect_classid:", detect_classid, self.yolov5_wrapper.categories[detect_classid], detect_score, area)  # 输出检测到的类别信息
            # print("box:", area, box[0], box[1], box[2], box[3])  # 注释掉的检测框详细信息输出
            
            if detect_classid == 0: # Go_straight  # 如果检测到直行标志
                self.car_run()  # 调用小车直行方法
            elif detect_classid == 1: # Turn_right  # 如果检测到右转标志
                self.car_turn_right()  # 调用小车右转方法
            elif detect_classid == 2: # whistle  # 如果检测到鸣笛标志
                self.car_whistle()  # 调用小车鸣笛方法（注意：方法名拼写错误）
            elif detect_classid == 3: # Sidewalk  # 如果检测到人行道标志
                self.car_sidewalk()  # 调用小车人行道避让方法
            elif detect_classid == 4: # Limiting_velocity  # 如果检测到限速标志
                self.car_limiting_velocity()  # 调用小车限速方法
            elif detect_classid == 5: # Shutdown  # 如果检测到关机标志
                self.car_shutdown()  # 调用小车关机方法
            elif detect_classid == 6: # School_decelerate  # 如果检测到学校减速标志
                self.car_school_decelerate()  # 调用小车学校减速方法（注意：方法名拼写错误）
            elif detect_classid == 8 or detect_classid == 7: # Parking_lotA  # 如果检测到停车位A或B标志
                self.car_parking()  # 调用小车停车方法
            elif detect_classid == 9: # No_light  # 如果检测到无信号灯标志
                self.car_no_light()  # 调用小车无信号灯通行方法（注意：方法名拼写错误）
            elif detect_classid == 10: # Red_light  # 如果检测到红灯标志
                self.car_red_light()  # 调用小车红灯停车方法
    
    def thread_detect(self, frame):
        image, classid, score, box = self.detect(frame)  # 调用检测方法处理输入帧，返回图像、类别ID、分数和检测框
        # print("detect_classid:", classid, self.yolov5_wrapper.categories[classid], score)  # 注释掉的调试信息输出
        self.control_car(classid, score, box)  # 调用控制方法根据检测结果控制小车
        self.yolo_busy = False  # 将YOLO忙碌状态设置为False，表示检测任务已完成
def yolo_detect(self, image):
        if not self.yolo_busy:  # 检查YOLO是否空闲（不在忙碌状态）
            self.yolo_busy = True  # 将YOLO状态设置为忙碌
            task_yolo = threading.Thread(target=self.thread_detect, args=(image,), name="task_yolo")  # 创建检测线程，传入图像参数
            task_yolo.setDaemon(True)  # 将线程设置为守护线程，主程序退出时自动结束
            task_yolo.start()  # 启动检测线程
        return image  # 返回原始图像（无论是否启动检测线程）
        
    def load_model_trt(self):
        print("load model trt")  # 输出加载模型提示信息
        self.model_trt = TRTModule()  # 创建TensorRT模块实例
        self.model_trt.load_state_dict(torch.load('road_following_model_trt.pth'))  # 加载预训练的TensorRT模型权重
        
    def cal_road_following(self, image):
        STEERING_GAIN = 60  # 定义转向增益系数为60
        STEERING_BIAS = 0.00  # 定义转向偏置为0.00
        image = cv.resize(image, (224, 224))  # 将图像调整为224x224像素
        image = preprocess(image).half()  # 对图像进行预处理并转换为半精度浮点数
        output = self.model_trt(image).detach().cpu().numpy().flatten()  # 模型推理，获取输出并转换为numpy数组
        x = float(output[0])  # 提取输出数组的第一个元素并转换为浮点数
        fvalue = x * STEERING_GAIN + STEERING_BIAS  # 计算最终的转向控制值（注意：变量名拼写错误）
        return fvalue  # 返回计算得到的转向控制值
    
    def load_car(self):
        self.car = Rosmaster()  # 创建Rosmaster小车控制实例
        self.car.set_beep(100)  # 设置蜂鸣器发出100毫秒的提示音
        self.car_enable = True  # 将小车使能状态设置为True（注意：变量名拼写错误）
        
    def car_control(self, value):
        if self.auto_control:  # 检查是否处于自动控制模式
            self.car.set_akm_steering_angle(value, True)  # 设置小车的阿克曼转向角度
if __name__ == "__main__":  # 判断是否为程序主入口
    print("Python version: ", sys.version)  # 输出当前Python版本信息
    # capture = cv.VideoCapture(0)  # 注释掉的默认摄像头初始化代码
    capture = cv.VideoCapture("/dev/camera_wide_angle")  # 初始化广角摄像头视频捕获
    capture.set(6, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))  # 设置视频编码格式为MJPEG
    capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)  # 设置视频帧宽度为640像素
    capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)  # 设置视频帧高度为480像素（注意：属性名拼写错误）
    # capture.set(cv.CAP_PROP_FRAME_WIDTH, 320)  # 注释掉的备用宽度设置
    # capture.set(cv.CAP_PROP_FRAME_HEIGHT, 240)  # 注释掉的备用高度设置
    print("capture get FPS : ", capture.get(cv.CAP_PROP_FPS))  # 输出摄像头帧率信息
    detect = YoloDetect()  # 创建YoloDetect检测器实例
    detect.load_yolo()  # 加载YOLO目标检测模型
    detect.load_model_trt()  # 加载TensorRT道路跟随模型
    detect.load_car()  # 加载小车硬件控制（注意：方法名拼写错误）
    cTime = 0  # 初始化当前时间变量为0
    pTime = 0  # 初始化上一帧时间变量为0
    t_start = time.time()  # 记录程序开始运行的时间戳
    m_fps = 0  # 初始化平均帧率变量为0
    load_opencv = 0  # 初始化OpenCV加载状态为0
    # detect.car_run()  # 注释掉的小车启动方法调用
try:  # 开始异常处理块，捕获可能出现的运行时错误
        while capture.isOpened():  # 循环检查摄像头是否已成功打开并处于可用状态
            ret, frame = capture.read()  # 从摄像头读取一帧图像，ret表示读取是否成功，frame是图像数据
            action = cv.waitKey(10) & 0xFF  # 等待键盘输入10毫秒，获取按键的ASCII码
            if action == ord('q'): break  # 如果按下'q'键，则跳出循环
            # frame, classid = detect.detect(frame)  # 注释掉的原始检测方法调用
            # print("classid:", classid)  # 注释掉的调试信息输出
            detect.yolo_detect(frame)  # 调用YOLO目标检测方法处理当前帧
            car_value = detect.cal_road_following(frame)  # 调用道路跟随计算方法（注意：方法名拼写错误）
            # print("car_value:", car_value)  # 注释掉的转向值输出
            detect.car_control(car_value)  # 调用小车控制方法，传入转向值
            m_fps = m_fps + 1  # 帧数计数器加1
            fps = m_fps / (time.time() - t_start)  # 计算平均帧率（总帧数/总时间）
            if(time.time() - t_start >= 2000):  # 检查是否已经运行超过2000秒（注意：条件判断有误）
                t_start = time.time()  # 重置开始时间戳为当前时间
                m_fps = fps  # 重置帧数计数器为当前帧率值
            # cTime = time.time()  # 注释掉的当前时间获取
            # fps = 1 / (cTime - pTime)  # 注释掉的瞬时帧率计算方法
            # pTime = cTime  # 注释掉的上次时间更新
            text = "FPS: " + str(int(fps))  # 格式化帧率显示文本
            cv.putText(frame, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)  # 在图像上绘制帧率文本
            cv.imshow('frame', frame)  # 在名为'frame'的窗口中显示处理后的图像
            # print(text)  # 注释掉的帧率信息输出
            if load_opencv == 0:  # 检查OpenCV是否已初始化完成
                detect.car_run()  # 调用小车运行方法
                load_opencv = 1  # 将OpenCV加载状态设置为1，避免重复初始化
    except:  # 捕获所有异常
        pass  # 异常发生时静默处理，不中断程序
    capture.release()  # 释放摄像头资源
    detect.cancel()  # 调用检测器取消方法，停止所有检测任务
    cv.destroyAllWindows()  # 销毁所有OpenCV创建的窗口

```

5、自动驾驶基础调试(代码解析)