## 1.课程内容

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

1.学习小车识别到红绿灯之后做的动作

2.学习新出现的关键源码

### 2.自助泊车源码

自助泊车源码路径：

```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/yolo_detect.py
```
```
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/auto_drive.py
```

### 3.程序启动

```
启动相机+底盘+路标车道线
ros2 launch auto_drive auto_drive_bringup.launch.py
```

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

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

```
ros2 run rqt_reconfigure rqt_reconfigure
```

![](/media/202512/768b7ca2e61147a79b800b7f418e7d5e8607.png)

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

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

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

**注意：因为自助泊车使用的固定动作，所以右转和停车A的路标是需要放在固定位置的**

![](/media/202512/17b1a158e8bf49c5811de554a19420979659.png)

程序识别到停车A程序会打印（停车A停车B是做一样的动作）

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

小车会运行固定动作入库，先前进3秒之后横向入库

![](/media/202512/5aceee12e80c47a294ef8230e60ddda59156.png)

自助泊车处理函数在auto_drive.py文件下，下面是识别到停车之后进入的函数，用户可以自行修改识别到路标之后小车运动

```
def parking_A(self):
        ''''停车场A、B'''
        self.get_logger().info('parking A')
        time.sleep(3.5)
        self.turn_action_active = True
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
        time.sleep(1.0)
        self.__set_current_speed(linear_y=-0.3)
        time.sleep(1.7)
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
```

## 4.源码解析

### 4.1、auto_drive.py

yolo路标消息接收：

```
def sign_callback(self, msg:DetectionObject):
    '''订阅yolo识别结果'''
    class_name = msg.class_name
    object_distance = self._get_distance(msg.ymin)  # 计算距离
    object = Object(msg.class_name, msg.confidence, object_distance)
```

多重过滤机制：主要为了防止一直识别路标并触发，识别到不同路标才会解锁动作。

```
# 1. 检查是否正在执行其他动作
if self.action_runing:
    return

# 2. 检查冷却时间（防重复触发）
current_time = time.time()
if class_name in self.last_processed_time:
    time_diff = current_time - self.last_processed_time[class_name]
    if time_diff < self.cooldown_time:  # 默认3秒冷却
        return

# 3. 检查是否是重复路标
if class_name == self.last_sign:
    return

# 4. 检查该路标是否被使能
if not self.sign_enable_list[对应的索引]:
    return
```
事件队列处理

```
def handle_events(self):
    '''处理目标识别事件'''
    while True:
        if not self.event_queue.empty(): 
            object = self.event_queue.get()
            self.action_runing = True
            current_sign = object.class_name
            
            try:
                # 根据路标类型执行相应动作
                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')
                # ... 其他路标处理
                
            finally:
                # 更新状态记录
                self.last_processed_time[current_sign] = time.time()
                self.last_sign = current_sign
                self.action_runing = False
```
具体动作实现

```
def parking_A(self):
        ''''停车场A、B'''
        self.get_logger().info('parking A')
        time.sleep(3.5)
        self.turn_action_active = True
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
        time.sleep(1.0)
        self.__set_current_speed(linear_y=-0.3)
        time.sleep(1.7)
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
```

无人驾驶自主泊车