# 自动驾驶综合应用

## 1.课程内容

**注意：这个程序是在沙盘地图上运行的，个人地图需要自行调整程序**

该教程使用的是出厂的路标模型和车道线识别模型，在沙盘地图上进行居中运动，同时识别到路标会做对应的路标函数动作。

## 2. 程序路径

自动驾驶源码路径： 车道线保
`/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/auto_drive.py`

路标识别：`/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/yolo_detect.py`

## 3.运行案例程序

### 3.1 运行程序

*   启动相机+底盘+路标车道线

```
ros2 launch auto_drive auto_drive_bringup.launch.py
```
![](/media/202512/80f0f141bf4f444a85eef3e9b51a48e51824.png)

程序运行之后会显示yolo识别画面，小车会车道线居中运行，同时会根据行驶过程中识别到的路标做对应的动作。（如果开启程序之后终端没有报错，小车不动，可以运行下面程序来检查参数）

![](/media/202512/c56b43aa9ab64420a262f14e1716ad4b2090.png)

*   启动动态参数配置节点
```
ros2 run rqt_reconfigure rqt_reconfigure
```

![](/media/202512/9c0375768d534bf78cb21f61384f45f16168.png)

**如果小车不动检查两个参数，START_AutoDrive 是否勾上了，conbine_nav是否为flase**

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

🛠 参数解析：

【turn_radius】：转弯半径。正常不需要更改这个参数

【drive_speed】：线速度大小。小车的行驶速度，出厂默认是0.15，如果修改这个参数需要自行同步调试其他的参数

【angular_speed】：转弯角速度大小

【duration】：识别右转标志的右转时间，

【cooldown_time】：路标识别冷却时间

【DEBUG_MODE】：可视化debug参数，打开可能会影响居中效果

【START_AutoDrive】：控制小车运动参数，true为小车可运动

【kp、ki、kd】：小车居中pid参数

【turn_radius】：转弯半径。正常不需要更改这个参数

【conbine_nav】：路网导航+开启yolo才需要打开，打开之后小车会停止不动

### 3.2 识别到路标之后小车的运动

*   直行
    
    *   按照设定的速度进行行驶，也可以在被停车标识之后停止后恢复行驶
        
*   右转
    
    *   触发右转的固定动作
        
*   鸣笛
    
    *   触发汽车鸣笛音效
        
*   人行道
    
    *   触发汽车鸣笛音效、减速1秒
        
*   限速
    
    *   汽车减速行驶3秒，然后恢复正常速度
        
*   学校
    
    *   触发汽车鸣笛音效、减速行驶1秒
        
*   停车场A、B
    
    *   触发预设的固定泊车动作
        
*   红绿灯
    
    *   红灯
        
        *   不结合路网规划时，触发解除车道保持自动驾驶状态
            
        *   结合路网规划时，取消正在执行的导航任务
            
    *   绿灯
        
        *   不结合路网规划时，触发激活车道保持自动驾驶状态
            
        *   结合路网规划时，恢复到上一个导航任务

下图是右转和停车位置固定位置，因为程序采用了固定动作所以要效果好建议是放到这个位置，如果需要放到其他的位置需要重新去调试固定动作时间 。

![image-20251201191324580](/media/202512/2f96dcda266e4b3aa046683a3187fea77233.png)

### 3.3 调试参数

这里分为两种调试参数的方案，一种是临时实时调试，一种是永久修改参数调试

*   临时实时调试

运行程序之后运行动态调参指令去调试：

```
ros2 run rqt_reconfigure rqt_reconfigure
```

![](/media/202512/025dae25462b42cca501a21d224902869048.png)

**调试经验：**

1.  如果车道居中保持不稳定可以去看《4.无人驾驶之车道保持》章节
    
2.  识别右转，转弯不够或者转弯过头可以调节duration这个参数来控制右转弯时间
    
3.  防止频繁识别路标，修改cooldown_time路标冷却，表示识别路标之间的冷却时长
    
4.  车道居中调整过慢，调大kp。

*   永久修改参数调试

通过修改参数文件来达到永久修改参数，参数路径

```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/config/auto_drive_node.yaml
```
```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/config/yolo_param.yaml
```
```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/config/midline_points.yaml
```
三个参数分别表示：自动驾驶参数、路标识别参数、车道线居中参数

**自动驾驶参数：**

```
auto_drive:                        
  ros__parameters:
    turn_radius: 0.4
    drive_speed: 0.15
    angular_speed: -0.72
    duration: 1.3
    response_distance_1: 2900
    response_distance_2: 132
    cooldown_time: 3.0
    DEBUG_MODE: False
    START_AutoDrive: True
    conbine_nav: False
    Kp: -0.62
    Ki: 0.0
    Kd: -0.1
    max_integral: 1000.0
    image_size: [640, 480]
```
上面参数和动态调节参数的意义是一样，这里修改之后下次运行默认调用这里的程序，启动需要注意的是START_AutoDrive: True和conbine_nav: False，这两个参数决定小车能不能运行。conbine_nav: False参数之后使用路网导航+自动驾驶的前提才需要打开。

**路标识别参数：**

```
#路标检测推理参数
sign_detection:
  model_path: '/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/tools/sign_model.engine'
  conf: 0.92                                                                            #设置检测的最小置信度阈值。 将忽略置信度低于此阈值的检测到的对象。 调整此值有助于减少误报。
  iou: 0.7                                                                              #用于非极大值抑制 (NMS) 的重叠阈值。较低的值会通过消除重叠的框来减少检测结果
  rect: True                                                                            #如果启用，则对图像较短的一边进行最小填充，直到可以被步长整除，以提高推理速度。
  half: True                                                                            #如果启用，则使用半精度推理，可以加快在支持的 GPU 上的模型推理速度，同时对准确性的影响极小。
  device: "cuda:0"                                                                                           #推理设备
  batch : 50                                                                                        #批处理大小,更大的批处理大小可以提供更高的吞吐量，从而缩短推理所需的总时间。
  max_det : 4                                                                                    #最多检测的物体数
  augment : False                                                                                #启用测试时增强 (TTA) 进行预测，可能会提高检测的鲁棒性，但会降低推理速度。
  verbose : False                                                                                #是否在终端输出推理的详细信息。
  classes : [0, 1, 2, 3, 4, 5, 6, 8, 9, 10, 11, 12, 13]

line_detection:
#车道线检测推理参数
  model_path: '/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/tools/lane_V6.engine'
  conf: 0.5                                                                            #设置检测的最小置信度阈值。 将忽略置信度低于此阈值的检测到的对象。 调整此值有助于减少误报。
  iou: 0.6                                                                             #用于非极大值抑制 (NMS) 的重叠阈值。较低的值会通过消除重叠的框来减少检测结果
  rect: True                                                                           #如果启用，则对图像较短的一边进行最小填充，直到可以被步长整除，以提高推理速度。
  half: True                                                                           #如果启用，则使用半精度推理，可以加快在支持的 GPU 上的模型推理速度，同时对准确性的影响极小。
  device: "cuda:0"                                                                                           #推理设备
  batch : 50                                                                                        #批处理大小,更大的批处理大小可以提供更高的吞吐量，从而缩短推理所需的总时间。
  max_det : 4                                                                                    #最多检测的物体数
  augment : False                                                                                #启用测试时增强 (TTA) 进行预测，可能会提高检测的鲁棒性，但会降低推理速度。
  verbose : False                                                                                #是否在终端输出推理的详细信息。
```

上面参数使用默认即可，一般不需要修改

**车道线居中参数**

```
midline_points:
  start_point:
    x: 360
    y: 477
  end_point:
    x: 355
    y: 424
  midline_length: 120
  mid_distance: [200,40]
```
启动start_point和end_point标定小车的中心点，具体可以看中轴线标定的课程。midline_length参数用来调整小车在沙盘地图上的转弯时机，越小转弯的越慢，反之则早转弯

## 4.源码解析

### 4.1、auto_drive.py

程序源码路径：
`/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/auto_drive.py`
控制流程 (Control Flow)分为

接收并处理图像数据

检测车道线和交通标志

根据检测结果决定控制策略

发布控制指令（速度、方向）

循环执行以上步骤

处理车道检测结果

```
def __handle_lane_detect_result(self,left_box,right_box,frame):
        '''处理车道检测结果'''
        self.error_mode = 99
        error_angular = [0.0, 0.0]  # 默认角度偏差Default angle deviation
        
        if left_box is not None and right_box is not None and left_box[0]<right_box[0] :
            #如果左右车道线都存在，则绘制左右车道线和交点
            self.error_mode=0  #标记误差模式，居中模式：0，左偏模式：1，右偏模式：2
            frame=self.lane_detect.draw_left_line(frame, left_box)#绘制左车道线
            frame=self.lane_detect.draw_right_line(frame, right_box)#绘制右车道线
            frame,x_center,y_center=self.lane_detect.draw_center_point(frame,left_box,right_box)# 绘制车道估计中心点 
            left_lane_offset,right_lane_offset=self.lane_detect.cal_lane_lateral_offset(left_box,right_box)
            # self.get_logger().info(f"left_lane_offset: {left_lane_offset:.2f} , right_lane_offset: {right_lane_offset:.2f} ")
            error_angular=self.lane_detect.cal_error_angular(x_center,y_center)#计算车身与中心点的角度误差
            
        elif left_box is not None and right_box is None and left_box[0]<320:
            self.error_mode=1
            self.lane_detect.prev_center=None
            frame=self.lane_detect.draw_left_line(frame, left_box)
        elif right_box is not None and left_box is None :
            self.error_mode=2
            self.lane_detect.prev_center=None
            frame=self.lane_detect.draw_right_line(frame, right_box)
        else:
            self.lane_detect.prev_center=None

# 转向动作激活时，暂停车道线速度控制
        if self.turn_action_active:
            return frame

if self.error_mode==0 :
            #居中
            if error_angular[1]>=0.03 or error_angular[1]<=-0.03:
                if abs(error_angular[1])<0.3:
                    angular_z=self.pid_controller.pid_control(error_angular[1])
                    angular_z=min(angular_z,0.3)
                    angular_z=max(angular_z,-0.3)
                    self.__set_current_speed(linear_x=self.drive_speed,angular_z=angular_z)

elif self.error_mode==1:
            #只有左边车道线
            if_infer=self.lane_detect.IF_lane_intersection(left_box)
            if if_infer is not None:
                self.__set_current_speed(linear_x=self.drive_speed,angular_z=self.angular_speed)
            else:
                self.__set_current_speed(linear_x=self.drive_speed,angular_z=0.0)
        else:
            self.__set_current_speed(linear_x=self.drive_speed,angular_z=0.0)

# 在图像上显示角度偏差
        cv2.putText(frame, f"Angle Dev: {error_angular[1]:.2f} rad", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 255), 2)
        frame=self.lane_detect.draw_midline(frame)
        return frame
```
auto_drive节点中sign_callback回调函数会订阅/sign_detect话题，然后在handle_events线程中执行和交通标识对应的动作

```
def handle_events(self):
        '''处理目标识别事件Handle target recognition events'''
        while True:
            try:
                object:Object=self.event_queue.get(timeout=0.2)
            except queue.Empty:
                continue 
            if self.__check_cooldown(object.class_name):
                continue
            # self.get_logger().info(f'{object.class_name} handle')
            
            if object.class_name=="straight" and self.sign_enable_list[10]:
                self.go_straight()
            elif object.class_name=="turnright" and self.sign_enable_list[11]:
                self.turn('right')
            elif object.class_name=="turnright_ground":
                pass
            elif object.class_name=="honking" and self.sign_enable_list[1]:
                self.car_horn()
            elif object.class_name== "sidewalk" and self.sign_enable_list[6]:
                self.sidewalk()
            elif object.class_name=="sidewalk_ground" and self.sign_enable_list[7]:
                pass
            elif object.class_name=="speedlimit" and self.sign_enable_list[8]:
                self.speedlimit()
            elif object.class_name=="stop" and self.sign_enable_list[9]:
                self.stop()
            elif object.class_name=="school" and self.sign_enable_list[5]:
                self.school()
            elif object.class_name=="parkA" and self.sign_enable_list[2]:
                    self.parking_A()
            elif object.class_name=="parkB" and self.sign_enable_list[3]:
                self.parking_A()
            elif object.class_name=="greenlight" and self.sign_enable_list[0]:
                self.greenlight()
            elif object.class_name =="redlight" and self.sign_enable_list[4]:
                self.redlight()
            elif object.class_name=="yellowlight" and self.sign_enable_list[13]:
                pass
            elif object.class_name=="out_park" :
                self.out_parking()
```

### 4.2、yolo_detect.py

程序源码位于

```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/yolo_detect.py
```
控制流程 (Control Flow)
图像回调 (image_callback):
将ROS图像消息转换为OpenCV格式
将图像放入处理队列
标志检测处理 (handle_sign):
从队列中获取图像帧
使用YOLO模型进行目标检测
处理检测结果：
过滤忽略的标志（如"turnright_ground", "sidewalk_ground"）
发布检测到的标志信息
在调试模式下显示FPS和推理时间
实时显示检测结果

```
def handle_sign(self):
        '''处理图像中的道路标识检测结果'''
        while True:
            try:
                frame = self.sign_detect.image_queue.get(timeout=0.01)
            except queue.Empty:
                continue  
            infer_start = time.time()
            results=self.sign_detect.get_infer_result(frame,True)
            for res in results:
                if res.boxes is not None:
                    annotated_frame = res.plot()
                    boxes = res.boxes
                    for i in range(len(boxes)):
                        box_coords = boxes.xyxy[i].tolist()# 获取边界框坐标 (xyxy格式: xmin, ymin, xmax, ymax)
                        class_name = res.names[int(boxes.cls[i].item())]# 获取物体名称
                        confidence = boxes.conf[i].item()# 获取置信度

# 检查是否为需要忽略的标志
                        if class_name in self.ignored_signs:
                            # rospy.loginfo(f"Ignoring sign: {class_name}")
                            continue  # 跳过忽略的标志

# 发布检测结果
                        if self.sign_pub_counter % 5 == 0:
                            self.sign_publisher.publish(DetectionObject(
                                class_name=class_name,
                                confidence=confidence,
                                xmin=float(box_coords[0]),
                                ymin=float(box_coords[1]),
                                xmax=float(box_coords[2]),
                                ymax=float(box_coords[3])
                            ))
                        self.sign_pub_counter += 1
        
            if self.DEBUG_MODE:
                infer_end = time.time()
                # 更新FPS
                self.frame_count += 1
                curr_time = time.time()
                time_diff = curr_time - self.prev_time
                if time_diff >= 1.0:
                    self.fps = self.frame_count / time_diff
                    self.frame_count = 0
                    self.prev_time = curr_time
                # 在图像上显示FPS
                cv2.putText(annotated_frame,f"FPS: {self.fps:.2f}",(10, 25),cv2.FONT_HERSHEY_SIMPLEX,0.6,(0, 255, 0),1,)
                # 显示推理时间
                infer_time = (infer_end - infer_start) * 1000
                cv2.putText(annotated_frame, f"Infer: {infer_time:.1f} ms", (10, 45),cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 1)

cv2.imshow("Sign-Detect Panel", annotated_frame)
            cv2.waitKey(1)
```

主线程负责接收图像，工作线程负责处理图像和检测任务，通过队列进行线程间通信。

### .2、yolov11_infer.py

程序源码位于`/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/utils/yolov11_infer.py`

**控制流程**

**Sign_Detect类**：

*   创建 Sign_Detect 实例时，加载并解析配置文件
    
*   调用 init_yolo_engine() 方法：
    
    *   初始化图像队列（最大容量为10）
        
    *   设置模型推理参数（置信度、IOU 阈值等）
        
    *   加载 YOLO 模型
        
*   通过 put\_image 方法将图像放入处理队列
    
*   通过 get\_infer\_result 方法获取推理结果
    
*   使用 YOLO 模型进行目标检测

**Lane_Detect类：**

*   **车道线检测**：使用 YOLO 模型检测图像中的车道线
    
*   **车道中心点计算**：计算左右车道线的中心点
    
*   **车道偏离检测**：计算车辆相对于车道的偏移角度和距离
    
*   **可视化**：提供多种绘图方法，用于显示检测结果

```
class Sign_Detect(YOLO_MODEL):
    """道路标志检测"""
    def __init__(self,config_file_path):
        with open(config_file_path, "r") as file:
            full_config = yaml.safe_load(file)
            self.config_param = full_config.get("sign_detection", {})
...

class Lane_Detect(YOLO_MODEL):
    """车道线检测模型"""
    def __init__(self,config_file_path:str,
                 midline_file_path:str):
        with open(config_file_path, "r") as file:
            full_config = yaml.safe_load(file)
            self.config_param = full_config.get("line_detection", {})
        with open(midline_file_path, "r") as file:
            self.point_config = yaml.safe_load(file)
...
```

无人驾驶综合应用