## 1. 课程内容

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

启动自动驾驶程序，在沙盘地图中使用车道保持居中自动驾驶功能

## 2.运行案例程序
自动驾驶源码路径：`/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/auto_drive.py`

```启动相机节点
ros2 launch ascamera hp60c.launch.py
```
```启动小车底盘
ros2 run yahboomcar_bringup Mcnamu_driver_M1
```
```启动车道线居中程序
ros2 run auto_drive auto_drive
```
成功运行会出现一个可视化界面

![](/media/202512/c9395f567c5043cf8c722aae351f974c1394.png)
启动成功之后正常小车会直接沿着沙盘车道线居中行驶，如果运行之后小车不动，可以根据下面的动态参数调节，看小车运行的参数有没有打开或者关闭（START_AutoDrive =True ，conbine_nav = False）之后车机会沿外圈的车道进行自动驾驶
* 在直线路段，会自动执行直线行驶和左右车道居中保持
* 在弯道路段会自动转向，出弯道后恢复车道保持模式

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

*   如果需要停车，取消勾选`START_AutoDrive`即可
*   启动动态参数配置节点
*   
```
ros2 run rqt_reconfigure rqt_reconfigure
```

可以通过实时调节上面参数来优化小车车道居中的效果

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

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

🛠 参数解析：

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

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

【angular_speed】：转弯角速度大小

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

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

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

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

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

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

**上面参数是主要调节优化参数，其他参数保持默认值**

中轴线调整
如果车道线保持过程中出现了小车提前转弯或者转弯过晚了，可以去调试中轴线来调试小车转弯的时机
参数路径：

```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/config/midline_points.yaml
```

```
midline_points:
  start_point:
    x: 360
    y: 477
  end_point:
    x: 355
    y: 424
  midline_length: 120
  mid_distance: [200,40]
```
通过修改midline_length参数来调整小车的转弯时机，这里以120长度为例，如果转弯过早了，我们可以尝试将 120修改成115这样去调试，也可以通过相机的角度来调试轻轻往下掰一点点也会让转弯晚一点，具体根据实际环境去调试。修改完之后需要编译修改的内再去运行程序

```
cd yahboomcar_ros2_ws/yahboomcar_ws/ 
```

```
colcon build --packages-select auto_drive
```

## 3.源码解析

### 3.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
```

上面是关键车道线居中程序，根据左右黄色车道线和中轴线的斜率夹角来判断转弯实际以及居中行走的时机。

无人驾驶的车道保持