## 自动驾驶综合应用
### 1.课程内容
注意：这个程序是在沙盘地图上运行的，如果在其他地图需要自行调整程序
该教程使用的是出厂的路标模型和车道线识别模型，在沙盘地图上进行居中运动，同时识别到路标会做对应的路标函数动作。
命令组合：（使用不同的终端打开）

```第一步打开语音环境，输入cd / 进入根目录
ros2 launch largemodel largemodel_control.launch.py
```
```第二步打开自动驾驶，输入cd / 进入根目录
ros2 launch auto_drive auto_drive_bringup.launch.py
```

```第三步打开终端返回根目录
ros2 run rqt_reconfigure rqt_reconfigure
```

**第四步：START_AutoDrive 打勾就会自动驾驶**

*   小车左右震荡：减小`Kp`，适度增大`Kd`；
*   小车回正缓慢：适度增大`Kp`，减小`Kd`；
*   右转过冲 / 不足：调整`duration`或`angular_speed`；
*   路标识别误触发：增大`cooldown_time`，调整`response_distance_1/2`。
####   安全调节提示：
*   先调`drive_speed`（基础速度）→ 再调`angular_speed/duration`（转弯匹配）→ 最后调 PID（Kp/Kd/Ki）；
*   禁止一次性修改多个核心参数（如同时改`drive_speed`+`Kp`），难以定位问题。
*   
  **安全调试流程**：
    *   先设`START_AutoDrive: False`，验证路标识别 / 偏差计算是否正常；
    *   再设`drive_speed`为低速（如 0.1m/s），开启`DEBUG_MODE: True`，观察居中效果；
    *   最后逐步提速，微调 PID 参数。

![](/media/202512/3e3269e117c1436ebb528fbf67d3a1b37089.png)

### 2.程序路径
自动驾驶源码路径：
#### 2.1车道线保持： 
```
/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 运行程序
启动相机+底盘+路标车道线程序运行之后会显示yolo识别画面，小车会车道线居中运行，同时会根据行驶过程中识别到的路标做对应的动作。（如果开启程序之后终端没有报错，小车不动，可以运行下面程序来检查参数）
```这是启动自动驾驶的命令
ros2 launch auto_drive auto_drive_bringup.launch.py
```
![](/media/202512/2aa13750ec2a4ee0bc4e2c58309a10dc1031.png)

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

![](/media/202512/19a804a4cc81434d82034a984696d3c31781.png)

* 启动动态参数配置节点
## **  这个命令必须先用cd / 回车才可以使用，就是返回到home的根目录**
```必须启动了自动驾驶才可以运行这个命令，否则打开后窗口为空
ros2 run rqt_reconfigure rqt_reconfigure
```

![](/media/202512/e924e41bf8d3412894abaae8048685ac2005.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 识别到路标之后小车的运动
【straight】：小车根据默认线速度直行。
【turnright】：小车会停下来一秒，根据固定动作进行转弯。注意摆放右转标志的路标这里是固定放在停车场附近的蓝框内
【honking】 ： 鸣笛一秒
【speedlimit】：减速三秒之后恢复正常行驶
【stop】：停车三秒之后恢复正常行驶
【school】：减速三秒之后鸣笛一秒
【parkA、parkB】：停车固定动作，之后小车停止，需要【straight】【speedlimit】解锁才会正常行驶
【redlight】：停车不动
【greenlight】：小车继续行驶
【yellowlight】：停车三秒之后恢复正常行驶

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

![](/media/202512/1f6b28cb3ae74eeabbb1e305829118f61722.png)

### 3.3 调试参数
这里分为两种调试参数的方案，一种是临时实时调试，一种是永久修改参数调试临时实时调试运行程序之后运行动态调参指令去调试：
```必须启动了自动驾驶才可以运行这个命令，否则打开后窗口为空
ros2 run rqt_reconfigure rqt_reconfigure
```

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

调试经验：
1. 如果车道居中保持不稳定可以去看《车道保持》章节
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  //线速度大小。小车的行驶速度，默认是0.15，如果修改这个参数需要自行同步调试其他的参数
		angular_speed: -0.72 //转弯角速度大小
		duration: 1.3 //识别右转标志的右转时间
		response_distance_1: 2900 
		response_distance_2: 132
		cooldown_time: 3.0  //路标识别冷却时间
		DEBUG_MODE: False //可视化debug参数，打开可能会影响居中效果，False关闭，True：打开
		START_AutoDrive: True //控制小车运动参数，true为小车可运动

conbine_nav: False
		Kp: -0.62
		Ki: 0.0
		Kd: -0.1 //小车居中pid参数
		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.en
gine'
	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.engin
e'
	conf: 0.5
half: True
#如果启用，则使用半精度推理，可以加快在支持的 GPU 上的模型推理速度，同时对准确性的影响极
小。
device: "cuda:0"
#推理设备
batch : 50
#批处理大小,更大的批处理大小可以提供更高的吞吐量，从而缩短推理所需的总时
间。
max_det : 4
#最多检测的物体数
augment : False
#启用测试时增强 (TTA) 进行预测，可能会提高检测的鲁棒性，但会降低推理速度。
verbose : False
#是否在终端输出推理的详细信息。
```

#设置检测的最小置信度阈值。 将忽略置信度低于此阈值的检测到的对象。 调整此值有助于减少误报。
iou: 0.6
#用于非极大值抑制 (NMS) 的重叠阈值。较低的值会通过消除重叠的框来减少检测结果
rect: True
#如果启用，则对图像较短的一边进行最小填充，直到可以被步长整除，以提高推理速度。
**上面参数使用我设置的即可，一般不需要修改**
**车道线居中参数**

```
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)分为
1. 接收并处理图像数据
2. 检测车道线和交通标志
3. 根据检测结果决定控制策略
4. 发布控制指令（速度、方向）
5. 循环执行以上步骤

```
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
```
### 4.2、yolo_detect.py

程序源码位于 /home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/yolo_detect.py

控制流程 (Control Flow)

1.图像回调 (image_callback):将ROS图像消息转换为OpenCV格式将图像放入处理队列
2.标志检测处理 (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)
```
主线程负责接收图像，工作线程负责处理图像和检测任务，通过队列进行线程间通信。
### 4.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)
...
```

新机器人自动驾驶