# 无人驾驶之转向决策

## 1.课程内容

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

1.学习小车识别到右转之后做的动作
2.学习新出现的关键源码
### 2.路标检测
路标检测源码路径：
`/home/jetson/yahboomcar_auto/src/auto_driver/scripts/auto_drive.py`

### 3.程序启动
* 参数配置
程序路径：
/home/jetson/yahboomcar_auto/src/auto_driver/config/auto_drive_node.yaml
```
turn_radius: 0.4
linear_speed: 0.13
angular_speed: -0.55
duration: 2.0
response_distance_1: 2900
response_distance_2: 132
cooldown_time: 0.2
DEBUG_MODE: False
DEBUG_MODE_2: True
START_AutoDrive: True
Kp: -0.62
Ki: 0.0
Kd: -0.1
max_integral: 1000.0
image_size: [640, 480]
```

```启动相机+底盘+自动驾驶节点
roslaunch auto_driver auto_drive.launch
```

![](/media/202512/6dfd9c0d490c4f639229a493c0a0f0787989.png)
启动成功之后会显示画面不会运动，为了测试路标识别情况。（如果想要运动，将auto_drive_node.yaml的参数START_AutoDrive改成 True）

![](/media/202512/04de8e0a568a43d1ba91f6e74577ead52805.png)

程序运行之后会显示yolo识别画面，小车会车道线居中运行，同时会根据行驶过程中识别到的路标做对应的动作。

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

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

![](/media/202512/76197200fb3348e2a1de5ee97ec9fc0c7570.png)

程序识别到右转程序会打印

![](/media/202512/52dcc5a9968d436f869d5f05627b86d54324.png)

小车会运行固定动作进行转向决策，看到右转会停下来一秒然后前进5秒右转。

![](/media/202512/8ebbec05a32f4ee28be16e6d3314b6049290.png)

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

```
def turn(self, direction):
        rospy.loginfo(f'Turn {direction}')
        if self.R == 0:
            angular_z = self.angular_speed if direction == 'left' else -self.angular_speed
            self._set_current_speed(angular_z=angular_z)
        else:
            self.flag = True
            # rospy.loginfo(f"angular_z : {angular_z}")
            self._set_current_speed(linear_x=0.0, angular_z=0.0)
            time.sleep(1)
            self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.05)
            time.sleep(5)
            self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.55)
            time.sleep(2.3)   #可自行修改转弯时间
            self.flag = False

time.sleep(self.duration)
        self._set_current_speed(linear_x=self.drive_speed, angular_z=0.0)
```

## 4.源码解析

### 4.1、auto_drive.py

yolo路标消息接收：

```
results = self.sign_detect.get_infer_result(frame, True)
annotated_frame = frame.copy()
```
多重过滤机制：主要为了防止一直识别路标并触发，识别到不同路标才会解锁动作。

```
 # 如果检测到需要冷却的标志，启动冷却
if detected_sign_requires_cooldown:
    self.sign_cooldown = True
    self.last_sign_time = current_time
    rospy.loginfo(f"Sign detected, starting {self.cooldown_duration}s cooldown")
 # 检查是否应该忽略重复路标
should_ignore = False
if obj.class_name == self.last_sign:
        should_ignore = True
```
事件队列处理

```
def _event_handling_worker(self):
        '''事件处理工作线程'''
        # 标志处理映射表
        sign_handlers = {
            "straight": self.go_straight,
            "turnright": lambda: self.turn('right'),
            "turnright_ground": self.do_nothing,
            "honking": self.car_horn,
            "sidewalk": self.slow_down,
            "sidewalk_ground": self.do_nothing,
            "speedlimit": self.slow_down,
            "stop": self.stop,
            "school": self.school,
            "parkA": self.parking_A,
            "parkB": self.parking_A,
            "greenlight": self.go_straight,
            "redlight": self.red_stop,
            "yellowlight": self.stop,
            "out_park": self.out_parking
        }
        
        # 标志启用索引映射
        sign_enable_map = {
            "straight": 10, "turnright": 11, "honking": 1, "sidewalk": 6,
            "sidewalk_ground": 7, "speedlimit": 8, "stop": 9, "school": 5,
            "parkA": 2, "parkB": 3, "greenlight": 0, "redlight": 4, "yellowlight": 13
        }
        
```
具体动作实现

```
def turn(self, direction):
        rospy.loginfo(f'Turn {direction}')
        if self.R == 0:
            angular_z = self.angular_speed if direction == 'left' else -self.angular_speed
            self._set_current_speed(angular_z=angular_z)
        else:
            self.flag = True
            # rospy.loginfo(f"angular_z : {angular_z}")
            self._set_current_speed(linear_x=0.0, angular_z=0.0)
            time.sleep(1)
            self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.05)
            time.sleep(5)
            self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.55)
            time.sleep(2.3)   #可自己修改转弯时间
            self.flag = False

time.sleep(self.duration)
        self._set_current_speed(linear_x=self.drive_speed, angular_z=0.0)
```

无人驾驶转向决策