huangfu/MAGAIL4AutoDrive
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: huangfu:train_not_ok
Branches Tags
huangfu:main huangfu:BC_SA huangfu:BC_II huangfu:BC_dev huangfu:BC huangfu:dev huangfu:train_not_ok
...
compare: huangfu:4dbea5f0a6c33229f44a4b4c2b7a80639206062c
Branches Tags
huangfu:BC_SA huangfu:BC_II huangfu:BC_dev huangfu:BC huangfu:main huangfu:dev huangfu:train_not_ok

3 Commits
train_not_ ... 4dbea5f0a6

Author SHA1 Message Date
huangfu
4dbea5f0a6 mixed_training 2026-01-17 14:50:20 +08:00
huangfu
c94571ddaa 新增scripts工具 2025-10-25 21:44:11 +08:00
huangfu
62e638c4d2 回退了一些内容,增加了对于环境方法的描述 2025-10-24 21:39:31 +08:00
51 changed files with 2914 additions and 2578 deletions
Expand all files Collapse all files

0
Algorithm/__init__.py Normal file
View File

339
Env/DEBUG_GUIDE.md
View File

@@ -1,339 +0,0 @@
# 调试功能使用指南
## 📋 概述
已为车道过滤和红绿灯检测功能添加了详细的调试输出，帮助您诊断和理解代码行为。
---
## 🎛️ 调试开关
### 1. 配置参数
在创建环境时，可以通过 `config` 参数启用调试模式：
```python
env = MultiAgentScenarioEnv(
config={
# ... 其他配置 ...
# 🔥 调试开关
"debug_lane_filter": True, # 启用车道过滤调试
"debug_traffic_light": True, # 启用红绿灯检测调试
},
agent2policy=your_policy
)
```
### 2. 默认值
两个调试开关默认都是 `False`（关闭），避免正常运行时产生大量日志。
---
## 📊 车道过滤调试 (`debug_lane_filter=True`)
### 输出内容
```
📍 场景信息统计:
- 总车道数: 123
🔍 开始车道过滤: 共 51 辆车待检测
车辆 1/51: ID=128
🔍 检测位置 (-4.11, 46.76), 容差=3.0m
✅ 在车道上 (车道184, 检查了32条)
✅ 保留
车辆 7/51: ID=134
🔍 检测位置 (-51.34, -3.77), 容差=3.0m
❌ 不在任何车道上 (检查了123条车道)
❌ 过滤 (原因: 不在车道上)
... (所有车辆)
📊 过滤结果: 保留 45 辆, 过滤 6 辆
```
### 调试信息说明
| 信息 | 含义 |
|------|------|
| 📍 场景信息统计 | 场景的基本信息（车道数、红绿灯数） |
| 🔍 开始车道过滤 | 开始过滤，显示待检测车辆总数 |
| 🔍 检测位置 | 车辆的坐标和使用的容差值 |
| ✅ 在车道上 | 找到了车辆所在的车道，显示车道ID和检查次数 |
| ❌ 不在任何车道上 | 所有车道都检查完了，未找到匹配的车道 |
| 📊 过滤结果 | 最终统计：保留多少辆，过滤多少辆 |
### 典型输出案例
**情况1：车辆在正常车道上**
```
车辆 1/51: ID=128
🔍 检测位置 (-4.11, 46.76), 容差=3.0m
✅ 在车道上 (车道184, 检查了32条)
✅ 保留
```
→ 检查了32条车道后找到匹配的车道184
**情况2：车辆在草坪/停车场**
```
车辆 7/51: ID=134
🔍 检测位置 (-51.34, -3.77), 容差=3.0m
❌ 不在任何车道上 (检查了123条车道)
❌ 过滤 (原因: 不在车道上)
```
→ 检查了所有123条车道都不匹配，该车辆被过滤
---
## 🚦 红绿灯检测调试 (`debug_traffic_light=True`)
### 输出内容
```
📍 场景信息统计:
- 总车道数: 123
- 有红绿灯的车道数: 0
⚠️ 场景中没有红绿灯！
🚦 检测车辆红绿灯 - 位置: (-4.1, 46.8)
方法1-导航模块:
current_lane = <metadrive.component.lane.straight_lane.StraightLane object>
lane_index = 184
has_traffic_light = False
该车道没有红绿灯
方法2-遍历车道: 开始遍历 123 条车道
✓ 找到车辆所在车道: 184 (检查了32条)
has_traffic_light = False
该车道没有红绿灯
结果: 返回 0 (无红绿灯/未知)
```
### 调试信息说明
| 信息 | 含义 |
|------|------|
| 有红绿灯的车道数 | 统计场景中有多少个红绿灯 |
| ⚠️ 场景中没有红绿灯 | 如果数量为0，会特别提示 |
| 方法1-导航模块 | 尝试从导航系统获取 |
| current_lane | 导航系统返回的当前车道对象 |
| lane_index | 车道的唯一标识符 |
| has_traffic_light | 该车道是否有红绿灯 |
| status | 红绿灯的状态（GREEN/YELLOW/RED/None） |
| 方法2-遍历车道 | 兜底方案，遍历所有车道查找 |
| ✓ 找到车辆所在车道 | 遍历找到了匹配的车道 |
### 典型输出案例
**情况1：场景没有红绿灯**
```
📍 场景信息统计:
- 有红绿灯的车道数: 0
⚠️ 场景中没有红绿灯！
🚦 检测车辆红绿灯 - 位置: (-4.1, 46.8)
方法1-导航模块:
...
has_traffic_light = False
该车道没有红绿灯
结果: 返回 0 (无红绿灯/未知)
```
→ 所有车辆都会返回0，这是正常的
**情况2：有红绿灯且状态正常**
```
🚦 检测车辆红绿灯 - 位置: (10.5, 20.3)
方法1-导航模块:
current_lane = <...>
lane_index = 205
has_traffic_light = True
status = TRAFFIC_LIGHT_GREEN
✅ 方法1成功: 绿灯
```
→ 方法1直接成功，返回1（绿灯）
**情况3：红绿灯状态为None**
```
🚦 检测车辆红绿灯 - 位置: (10.5, 20.3)
方法1-导航模块:
current_lane = <...>
lane_index = 205
has_traffic_light = True
status = None
⚠️ 方法1: 红绿灯状态为None
```
→ 有红绿灯，但状态异常，返回0
**情况4：导航失败，方法2兜底**
```
🚦 检测车辆红绿灯 - 位置: (15.2, 30.5)
方法1-导航模块: 不可用 (hasattr=True, not_none=False)
方法2-遍历车道: 开始遍历 123 条车道
✓ 找到车辆所在车道: 210 (检查了45条)
has_traffic_light = True
status = TRAFFIC_LIGHT_RED
✅ 方法2成功: 红灯
```
→ 方法1失败，方法2兜底成功，返回3（红灯）
---
## 🧪 测试方法
### 方式1：使用测试脚本
```bash
# 标准测试（无详细调试）
python Env/test_lane_filter.py
# 调试模式（详细输出）
python Env/test_lane_filter.py --debug
```
### 方式2：在代码中直接启用
```python
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
env = MultiAgentScenarioEnv(
config={
"data_directory": "...",
"use_render": False,
# 启用调试
"debug_lane_filter": True,
"debug_traffic_light": True,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
obs = env.reset(0)
# 调试信息会自动输出
```
---
## 📝 调试输出控制
### 场景1：只想看车道过滤
```python
config = {
"debug_lane_filter": True,
"debug_traffic_light": False, # 关闭红绿灯调试
}
```
### 场景2：只想看红绿灯检测
```python
config = {
"debug_lane_filter": False,
"debug_traffic_light": True, # 只看红绿灯
}
```
### 场景3：生产环境（关闭所有调试）
```python
config = {
"debug_lane_filter": False,
"debug_traffic_light": False,
}
# 或者直接不设置这两个参数，默认就是False
```
---
## 💡 常见问题诊断
### 问题1：所有红绿灯状态都是0
**检查调试输出：**
```
📍 场景信息统计:
- 有红绿灯的车道数: 0
⚠️ 场景中没有红绿灯！
```
**结论：** 场景本身没有红绿灯，返回0是正常的
---
### 问题2：车辆被过滤但不应该过滤
**检查调试输出：**
```
车辆 X: ID=XXX
🔍 检测位置 (x, y), 容差=3.0m
❌ 不在任何车道上 (检查了123条车道)
❌ 过滤 (原因: 不在车道上)
```
**可能原因：**
1. 车辆确实在非车道区域（草坪/停车场）
2. 容差值太小，可以尝试增大 `lane_tolerance`
3. 车道数据有问题
**解决方案：**
```python
config = {
"lane_tolerance": 5.0, # 增大容差到5米
}
```
---
### 问题3：性能下降
启用调试模式会有大量输出，影响性能：
**解决方案：**
- 只在开发/调试时启用
- 生产环境关闭所有调试开关
- 或者只测试少量车辆：
```python
config = {
"max_controlled_vehicles": 5, # 只测试5辆车
"debug_traffic_light": True,
}
```
---
## 📌 最佳实践
1. **开发阶段**：启用调试，理解代码行为
2. **调试问题**：根据需要选择性启用调试
3. **性能测试**：关闭所有调试
4. **生产运行**：永久关闭调试
---
## 🔧 调试输出示例
完整的调试运行示例：
```bash
cd /home/huangfukk/MAGAIL4AutoDrive
python Env/test_lane_filter.py --debug
```
输出会包含：
- 场景统计信息
- 每辆车的详细检测过程
- 最终的过滤/检测结果
- 性能统计
---
## 📖 相关文档
- `README.md` - 项目总览和问题解决
- `CHANGELOG.md` - 更新日志
- `PERFORMANCE_OPTIMIZATION.md` - 性能优化指南

221
Env/GPU_ACCELERATION.md
View File

@@ -1,221 +0,0 @@
# GPU加速指南
## 当前性能瓶颈分析
从测试结果看，即使关闭渲染，FPS仍然只有15-20左右，主要瓶颈是：
### 计算量分析（51辆车）
```
激光雷达计算：
- 前向雷达：80束 × 51车 = 4,080次射线检测
- 侧向雷达：10束 × 51车 = 510次射线检测
- 车道线雷达：10束 × 51车 = 510次射线检测
合计：5,100次射线检测/帧
红绿灯检测：
- 遍历所有车道 × 51车 = 数千次几何计算
```
**关键问题**：这些计算都是CPU单线程串行的，无法利用多核和GPU！
---
## GPU加速方案
### 方案1：优化激光雷达计算（已实现）✅
**优化内容：**
1. 减少激光束数量：100束 → 52束（减少48%）
2. 优化红绿灯检测：避免遍历所有车道
3. 激光雷达缓存：每N帧才重新计算一次
**预期提升：** 2-4倍（30-60 FPS）
**使用方法：**
```bash
python Env/run_multiagent_env_fast.py
```
---
### 方案2：MetaDrive GPU渲染（有限支持）
**说明：**
MetaDrive基于Panda3D引擎，理论上支持GPU渲染，但：
- GPU主要用于**图形渲染**，不是物理计算
- 激光雷达的射线检测仍在CPU上
- GPU渲染主要加速可视化，不加速训练
**启用方法：**
```python
config = {
"use_render": True,
"render_mode": "onscreen", # 或 "offscreen"
# Panda3D会自动尝试使用GPU
}
```
**限制：**
- 需要显示器或虚拟显示（Xvfb）
- WSL2环境需要配置X11转发
- 对无渲染训练无帮助
---
### 方案3：使用GPU加速的物理引擎（推荐但需要迁移）
**选项A：Isaac Gym (NVIDIA)**
- 完全在GPU上运行物理模拟和渲染
- 可同时模拟数千个环境
- **缺点**：需要完全重写环境代码，迁移成本高
**选项B：IsaacSim/Omniverse**
- NVIDIA的高级仿真平台
- 支持GPU加速的激光雷达
- **缺点**：学习曲线陡峭，环境配置复杂
**选项C：Brax (Google)**
- JAX驱动，完全在GPU/TPU上运行
- **缺点**：功能有限，不支持复杂场景
---
### 方案4：策略网络GPU加速（推荐）✅
虽然环境仿真在CPU，但可以让**策略网络在GPU上运行**：
```python
import torch
# 创建GPU上的策略模型
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
policy = PolicyNetwork().to(device)
# 批量处理观测
obs_batch = torch.tensor(obs_list).to(device)
with torch.no_grad():
actions = policy(obs_batch)
actions = actions.cpu().numpy()
```
**优势：**
- 51辆车的推理可以并行
- 如果使用RL训练，GPU加速训练过程
- 不需要修改环境代码
---
### 方案5：多进程并行（最实用）✅
既然单个环境受限于CPU单线程，可以**并行运行多个环境**：
```python
from multiprocessing import Pool
import os
def run_single_env(seed):
"""运行单个环境实例"""
env = MultiAgentScenarioEnv(config=...)
obs = env.reset(seed)
for step in range(1000):
actions = {...}
obs, rewards, dones, infos = env.step(actions)
if dones["__all__"]:
break
env.close()
return results
# 使用进程池并行运行
if __name__ == "__main__":
num_processes = os.cpu_count() # 12600KF有10核20线程
seeds = list(range(num_processes))
with Pool(processes=num_processes) as pool:
results = pool.map(run_single_env, seeds)
```
**预期提升：** 接近线性（10核 ≈ 10倍吞吐量）
**CPU利用率：** 可达80-100%
---
## 推荐的完整优化方案
### 1. 立即可用（已实现）
```bash
# 使用优化版本，激光束减少+缓存
python Env/run_multiagent_env_fast.py
```
**预期：** 30-60 FPS（2-4倍提升）
### 2. 短期优化（1-2小时）
- 实现多进程并行
- 策略网络迁移到GPU
**预期：** 300-600 FPS（总吞吐量）
### 3. 中期优化（1-2天）
- 使用NumPy矢量化批量处理观测
- 优化Python代码热点（用Cython/Numba）
**预期：** 额外20-30%提升
### 4. 长期方案（1-2周）
- 迁移到Isaac Gym等GPU加速仿真器
- 或使用分布式训练框架（Ray/RLlib）
**预期：** 10-100倍提升
---
## 为什么MetaDrive无法直接使用GPU？
### 架构限制：
1. **物理引擎**：使用Bullet/Panda3D的CPU物理引擎
2. **射线检测**：串行CPU计算，无法并行
3. **Python GIL**：全局解释器锁限制多线程
4. **设计目标**：MetaDrive设计时主要考虑灵活性而非极致性能
### GPU在仿真中的作用：
- ✅ **图形渲染**：绘制画面（但我们训练时不需要）
- ✅ **神经网络推理/训练**：策略模型计算
- ❌ **物理计算**：MetaDrive的物理引擎在CPU
- ❌ **传感器模拟**：激光雷达等在CPU
---
## 检查GPU是否可用
```bash
# 检查NVIDIA GPU
nvidia-smi
# 检查PyTorch GPU支持
python -c "import torch; print(f'CUDA available: {torch.cuda.is_available()}')"
# 检查MetaDrive渲染设备
python -c "from panda3d.core import GraphicsPipeSelection; print(GraphicsPipeSelection.get_global_ptr().get_default_pipe())"
```
---
## 总结
| 方案 | 实现难度 | 性能提升 | GPU使用 | 推荐度 |
|------|----------|----------|---------|--------|
| 减少激光束 | ⭐ | 2-4x | ❌ | ⭐⭐⭐⭐⭐ |
| 激光雷达缓存 | ⭐ | 1.5-3x | ❌ | ⭐⭐⭐⭐⭐ |
| 多进程并行 | ⭐⭐ | 5-10x | ❌ | ⭐⭐⭐⭐⭐ |
| 策略GPU加速 | ⭐⭐ | 2-5x | ✅ | ⭐⭐⭐⭐ |
| GPU渲染 | ⭐⭐⭐ | 1.2x | ✅ | ⭐⭐ |
| 迁移Isaac Gym | ⭐⭐⭐⭐⭐ | 10-100x | ✅ | ⭐⭐⭐ |
**结论：**
1. 先用已实现的优化（减少激光束+缓存）
2. 再实现多进程并行
3. 策略网络用GPU训练
4. 如果还不够，考虑迁移到GPU仿真器

413
Env/LOGGING_GUIDE.md
View File

@@ -1,413 +0,0 @@
# 日志记录功能使用指南
## 📋 概述
为所有运行脚本添加了日志记录功能，可以将终端输出同时保存到文本文件，方便后续分析和问题排查。
---
## 🎯 功能特点
1. **双向输出**：同时输出到终端和文件，不影响实时查看
2. **自动管理**：使用上下文管理器，自动处理文件开启/关闭
3. **灵活配置**：支持自定义文件名和日志目录
4. **时间戳命名**：默认使用时间戳生成唯一文件名
5. **无缝集成**：只需添加命令行参数，无需修改代码
---
## 🚀 快速使用
### 1. 基础用法
```bash
# 不启用日志（默认）
python Env/run_multiagent_env.py
# 启用日志记录
python Env/run_multiagent_env.py --log
# 或使用短选项
python Env/run_multiagent_env.py -l
```
### 2. 自定义文件名
```bash
# 使用自定义日志文件名
python Env/run_multiagent_env.py --log --log-file=my_test.log
# 测试脚本也支持
python Env/test_lane_filter.py --log --log-file=test_results.log
```
### 3. 组合使用调试和日志
```bash
# 测试脚本：调试模式 + 日志记录
python Env/test_lane_filter.py --debug --log
# 会生成类似：test_debug_20251021_123456.log
```
---
## 📁 日志文件位置
默认日志目录：`Env/logs/`
### 文件命名规则
| 脚本 | 默认文件名格式 | 示例 |
|------|---------------|------|
| `run_multiagent_env.py` | `run_YYYYMMDD_HHMMSS.log` | `run_20251021_143022.log` |
| `run_multiagent_env_fast.py` | `run_fast.log` | `run_fast.log` |
| `test_lane_filter.py` | `test_{mode}_YYYYMMDD_HHMMSS.log` | `test_debug_20251021_143500.log` |
**说明**：
- `YYYYMMDD_HHMMSS` 是时间戳（年月日_时分秒）
- `{mode}` 是测试模式（`standard` 或 `debug`）
---
## 📝 所有支持的脚本
### 1. run_multiagent_env.py（标准运行脚本）
```bash
# 不启用日志
python Env/run_multiagent_env.py
# 启用日志（自动生成时间戳文件名）
python Env/run_multiagent_env.py --log
# 自定义文件名
python Env/run_multiagent_env.py --log --log-file=run_test1.log
```
**日志位置**：`Env/logs/run_YYYYMMDD_HHMMSS.log`
---
### 2. run_multiagent_env_fast.py（高性能版本）
```bash
# 启用日志
python Env/run_multiagent_env_fast.py --log
# 自定义文件名
python Env/run_multiagent_env_fast.py --log --log-file=fast_test.log
```
**日志位置**：`Env/logs/run_fast.log`（默认）
---
### 3. test_lane_filter.py（测试脚本）
```bash
# 标准测试 + 日志
python Env/test_lane_filter.py --log
# 调试测试 + 日志
python Env/test_lane_filter.py --debug --log
# 自定义文件名
python Env/test_lane_filter.py --log --log-file=my_test.log
# 组合使用
python Env/test_lane_filter.py --debug --log --log-file=debug_run.log
```
**日志位置**：
- 标准模式：`Env/logs/test_standard_YYYYMMDD_HHMMSS.log`
- 调试模式：`Env/logs/test_debug_YYYYMMDD_HHMMSS.log`
---
## 💻 编程接口
如果您想在代码中直接使用日志功能：
```python
from logger_utils import setup_logger
# 方式1：使用上下文管理器（推荐）
with setup_logger(log_file="my_log.log", log_dir="logs"):
print("这条消息会同时输出到终端和文件")
# 运行您的代码
# ...
# 方式2：手动管理
from logger_utils import LoggerContext
logger = LoggerContext(log_file="custom.log", log_dir="output")
logger.__enter__() # 开启日志
print("输出消息")
logger.__exit__(None, None, None) # 关闭日志
```
---
## 📊 日志内容示例
### 标准运行
```
📝 日志记录已启用
📁 日志文件: Env/logs/run_20251021_143022.log
------------------------------------------------------------
💡 提示: 使用 --log 或 -l 参数启用日志记录
示例: python run_multiagent_env.py --log
自定义文件名: python run_multiagent_env.py --log --log-file=my_run.log
------------------------------------------------------------
[INFO] Environment: MultiAgentScenarioEnv
[INFO] MetaDrive version: 0.4.3
...
------------------------------------------------------------
✅ 日志已保存到: Env/logs/run_20251021_143022.log
```
### 调试模式
```
📝 日志记录已启用
📁 日志文件: Env/logs/test_debug_20251021_143500.log
------------------------------------------------------------
🐛 调试模式启用
============================================================
📍 场景信息统计:
- 总车道数: 123
- 有红绿灯的车道数: 0
⚠️ 场景中没有红绿灯！
🔍 开始车道过滤: 共 51 辆车待检测
...
------------------------------------------------------------
✅ 日志已保存到: Env/logs/test_debug_20251021_143500.log
```
---
## 🔧 高级配置
### 自定义日志目录
```python
from logger_utils import setup_logger
# 指定不同的日志目录
with setup_logger(log_file="test.log", log_dir="my_logs"):
print("日志会保存到 my_logs/test.log")
```
### 追加模式
```python
from logger_utils import setup_logger
# 追加到现有文件（而不是覆盖）
with setup_logger(log_file="test.log", mode='a'): # mode='a' 表示追加
print("这条消息会追加到文件末尾")
```
### 只重定向特定输出
```python
from logger_utils import LoggerContext
# 只重定向stdout，不重定向stderr
logger = LoggerContext(
log_file="test.log",
redirect_stdout=True, # 重定向标准输出
redirect_stderr=False # 不重定向错误输出
)
```
---
## 📋 命令行参数总结
| 参数 | 短选项 | 说明 | 示例 |
|------|--------|------|------|
| `--log` | `-l` | 启用日志记录 | `--log` |
| `--log-file=NAME` | 无 | 指定日志文件名 | `--log-file=test.log` |
| `--debug` | `-d` | 启用调试模式（test_lane_filter.py） | `--debug` |
### 参数组合
```bash
# 示例1：标准模式 + 日志
python Env/test_lane_filter.py --log
# 示例2：调试模式 + 日志
python Env/test_lane_filter.py --debug --log
# 示例3：调试 + 自定义文件名
python Env/test_lane_filter.py -d --log --log-file=my_debug.log
# 示例4：所有参数
python Env/test_lane_filter.py --debug --log --log-file=full_test.log
```
---
## 🛠️ 常见问题
### Q1: 日志文件在哪里？
**A**: 默认在 `Env/logs/` 目录下。如果目录不存在，会自动创建。
```bash
# 查看所有日志文件
ls -lh Env/logs/
# 查看最新的日志
ls -lt Env/logs/ | head -5
```
---
### Q2: 如何查看日志内容？
**A**: 使用任何文本编辑器或命令行工具：
```bash
# 方式1：使用cat
cat Env/logs/run_20251021_143022.log
# 方式2：使用less（可翻页）
less Env/logs/run_20251021_143022.log
# 方式3：查看末尾内容
tail -n 50 Env/logs/run_20251021_143022.log
# 方式4：实时监控（适合长时间运行）
tail -f Env/logs/run_20251021_143022.log
```
---
### Q3: 日志文件太多怎么办？
**A**: 可以定期清理旧日志：
```bash
# 删除7天前的日志
find Env/logs/ -name "*.log" -mtime +7 -delete
# 只保留最新的10个日志
cd Env/logs && ls -t *.log | tail -n +11 | xargs rm -f
```
---
### Q4: 日志会影响性能吗？
**A**: 影响很小，因为：
1. 文件I/O是异步的
2. 使用了缓冲区
3. 立即刷新确保数据不丢失
如果追求极致性能，建议训练时不启用日志，只在需要分析时启用。
---
### Q5: 可以同时记录多个脚本的日志吗？
**A**: 可以，每个脚本使用不同的日志文件：
```bash
# 终端1
python Env/run_multiagent_env.py --log --log-file=script1.log
# 终端2（同时运行）
python Env/test_lane_filter.py --log --log-file=script2.log
```
---
## 💡 最佳实践
### 1. 开发阶段
```bash
# 使用调试模式 + 日志，方便排查问题
python Env/test_lane_filter.py --debug --log
```
### 2. 长时间运行
```bash
# 启用日志，避免输出丢失
nohup python Env/run_multiagent_env.py --log > /dev/null 2>&1 &
# 查看实时输出
tail -f Env/logs/run_*.log
```
### 3. 批量实验
```bash
# 为每次实验使用不同的日志文件
for i in {1..5}; do
python Env/run_multiagent_env.py --log --log-file=exp_${i}.log
done
```
### 4. 性能测试
```bash
# 不启用日志，获得最佳性能
python Env/run_multiagent_env_fast.py
```
---
## 📖 相关文档
- `README.md` - 项目总览
- `DEBUG_GUIDE.md` - 调试功能使用指南
- `CHANGELOG.md` - 更新日志
---
## 🔍 技术细节
### 实现原理
1. **TeeLogger类**：实现同时写入终端和文件
2. **上下文管理器**：自动管理资源（文件打开/关闭）
3. **sys.stdout重定向**：拦截所有print输出
4. **即时刷新**：每次写入后立即刷新，确保数据不丢失
### 源代码
详见 `Env/logger_utils.py`
```python
# 简化示例
class TeeLogger:
def write(self, message):
self.terminal.write(message) # 输出到终端
self.log_file.write(message) # 写入文件
self.log_file.flush() # 立即刷新
```
---
## ✅ 总结
- ✅ 简单易用：只需添加 `--log` 参数
- ✅ 不影响输出：终端仍可实时查看
- ✅ 自动管理：文件自动开启/关闭
- ✅ 灵活配置：支持自定义文件名和目录
- ✅ 完整记录：包含所有调试信息
立即开始使用：
```bash
python Env/test_lane_filter.py --debug --log
```

131
Env/PERFORMANCE_OPTIMIZATION.md
View File

@@ -1,131 +0,0 @@
# MetaDrive 性能优化指南
## 为什么帧率只有15FPS且CPU利用率不高？
### 主要原因：
1. **渲染瓶颈（最主要）**
- `use_render: True` + 每帧调用 `env.render()` 会严重限制帧率
- MetaDrive 使用 Panda3D 渲染引擎，渲染是**同步阻塞**的
- 即使CPU有余力，也要等待渲染完成才能继续下一步
- 这就是为什么CPU利用率低但帧率也低的原因
2. **激光雷达计算开销**
- 每帧对每辆车进行3次激光雷达扫描（100个激光束）
- 需要进行物理射线检测，计算量较大
3. **物理引擎同步**
- 默认物理步长很小（0.02s），需要频繁计算
4. **Python GIL限制**
- Python全局解释器锁限制了多核并行
- 即使是多核CPU，Python单线程性能才是瓶颈
## 性能优化方案
### 方案1：关闭渲染（推荐用于训练）
**预期提升：10-20倍（150-300+ FPS）**
```python
config = {
"use_render": False, # 关闭渲染
"render_pipeline": False,
"image_observation": False,
"interface_panel": [],
"manual_control": False,
}
```
### 方案2：降低物理计算频率
**预期提升：2-3倍**
```python
config = {
"physics_world_step_size": 0.05, # 默认0.02，增大步长
"decision_repeat": 5, # 每5个物理步执行一次决策
}
```
### 方案3：优化激光雷达
**预期提升：1.5-2倍**
修改 `scenario_env.py` 中的 `_get_all_obs()` 函数：
```python
# 减少激光束数量
lidar = self.engine.get_sensor("lidar").perceive(
num_lasers=40, # 从80减到40
distance=30,
base_vehicle=vehicle,
physics_world=self.engine.physics_world.dynamic_world
)
# 或者降低扫描频率（每N步才扫描一次）
if self.round % 5 == 0:
lidar = self.engine.get_sensor("lidar").perceive(...)
else:
lidar = self.last_lidar[agent_id] # 使用缓存
```
### 方案4：间歇性渲染
**适用场景：既需要可视化又想提升性能**
```python
# 每10步渲染一次，而不是每步都渲染
if step % 10 == 0:
env.render(mode="topdown")
```
### 方案5：使用多进程并行（高级）
**预期提升：接近线性（取决于进程数）**
```python
from multiprocessing import Pool
def run_env(seed):
env = MultiAgentScenarioEnv(config=...)
# 运行仿真
return results
# 使用进程池并行运行多个环境
with Pool(processes=8) as pool:
results = pool.map(run_env, range(8))
```
## 文件说明
- `run_multiagent_env.py` - **标准版本**（无渲染，基础优化）
- `run_multiagent_env_fast.py` - **极速版本**（激光雷达优化+缓存）⭐推荐
- `run_multiagent_env_parallel.py` - **并行版本**（多进程，最高吞吐量）⭐⭐推荐
- `run_multiagent_env_visual.py` - **可视化版本**（有渲染，适合调试）
## 性能对比
| 配置 | 单环境FPS | 总吞吐量 | CPU利用率 | 文件 | 适用场景 |
|------|-----------|----------|-----------|------|----------|
| 原始配置（有渲染） | 15-20 | 15-20 | 15-20% | visual | 实时可视化调试 |
| 关闭渲染 | 20-25 | 20-25 | 20-30% | 标准版 | 基础训练 |
| 激光雷达优化+缓存 | 30-60 | 30-60 | 30-50% | fast | 快速训练⭐ |
| 多进程并行（10核） | 30-60 | 300-600 | 90-100% | parallel | 大规模训练⭐⭐ |
**说明：**
- **单环境FPS**：单个环境实例的帧率
- **总吞吐量**：所有进程合计的 steps/second
- 12600KF（10核20线程）推荐使用并行版本
## 建议
1. **训练时**：使用高性能版本（关闭渲染）
2. **调试时**：使用可视化版本，或间歇性渲染
3. **大规模实验**：使用多进程并行
4. **如果需要GPU加速**：考虑使用GPU渲染或将策略网络部署到GPU上
## 为什么CPU利用率低？
- **渲染阻塞**：CPU在等待渲染完成
- **Python GIL**：限制了多核利用
- **I/O等待**：可能在等待磁盘读取数据
- **单线程瓶颈**：MetaDrive主循环是单线程的
解决方法：关闭渲染 + 多进程并行

241
Env/QUICK_START.md
View File

@@ -1,241 +0,0 @@
# 快速使用指南
## 🚀 已实现的性能优化
根据您的测试结果，原始版本FPS只有15左右，现已进行了全面优化。
---
## 📊 性能瓶颈分析
您的CPU是12600KF（10核20线程），但利用率不到20%，原因是：
1. **激光雷达计算瓶颈**：51辆车 × 100个激光束 = 每帧5100次射线检测
2. **红绿灯检测低效**：遍历所有车道进行几何计算
3. **Python GIL限制**：单线程执行，无法利用多核
4. **计算串行化**：所有车辆依次处理，没有并行
---
## 🎯 推荐使用方案
### 方案1：极速单环境（推荐新手）⭐
```bash
python Env/run_multiagent_env_fast.py
```
**优化内容：**
- ✅ 激光束：100束 → 52束（减少48%计算量）
- ✅ 激光雷达缓存：每3帧才重新计算
- ✅ 红绿灯检测优化：避免遍历所有车道
- ✅ 关闭所有渲染和调试
**预期性能：** 30-60 FPS（2-4倍提升）
---
### 方案2：多进程并行（推荐训练）⭐⭐
```bash
python Env/run_multiagent_env_parallel.py
```
**优化内容：**
- ✅ 同时运行10个独立环境（充分利用10核CPU）
- ✅ 每个环境应用所有单环境优化
- ✅ CPU利用率可达90-100%
**预期性能：** 300-600 steps/s（20-40倍总吞吐量）
---
### 方案3：可视化调试
```bash
python Env/run_multiagent_env_visual.py
```
**说明：** 保留渲染功能，FPS约15，仅用于调试
---
## 🔧 关于GPU加速
### GPU能否加速MetaDrive？
**简短回答：有限支持，主要瓶颈不在GPU**
**详细说明：**
1. **物理计算（主要瓶颈）** ❌ 不支持GPU
- MetaDrive使用Bullet物理引擎，只在CPU运行
- 激光雷达射线检测也在CPU
- 这是FPS低的主要原因
2. **图形渲染** ✅ 支持GPU
- Panda3D会自动使用GPU渲染
- 但我们训练时关闭了渲染，所以GPU无用武之地
3. **策略网络** ✅ 支持GPU
- 可以把Policy模型放到GPU上
- 但环境本身仍在CPU
### GPU渲染配置（可选）
```python
config = {
"use_render": True,
# GPU会自动用于渲染
}
```
### 策略网络GPU加速（推荐）
```python
import torch
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
policy_model = PolicyNet().to(device)
# 批量推理
obs_tensor = torch.tensor(obs_list).to(device)
actions = policy_model(obs_tensor)
```
**详细说明请看：** `GPU_ACCELERATION.md`
---
## 📈 性能对比
| 版本 | FPS | CPU利用率 | 改进 |
|------|-----|-----------|------|
| 原始版本 | 15 | 20% | - |
| 极速版本 | 30-60 | 30-50% | 2-4x |
| 并行版本 | 30-60/env | 90-100% | 总吞吐20-40x |
---
## 💡 使用建议
### 场景1：快速测试环境
```bash
python Env/run_multiagent_env_fast.py
```
单环境，快速验证功能
### 场景2：大规模数据收集
```bash
python Env/run_multiagent_env_parallel.py
```
多进程，最大化数据收集速度
### 场景3：RL训练
```bash
# 推荐使用Ray RLlib等框架，它们内置了并行环境管理
# 或者修改parallel版本，保存经验到replay buffer
```
### 场景4：调试/可视化
```bash
python Env/run_multiagent_env_visual.py
```
带渲染，可以看到车辆运行
---
## 🔍 性能监控
所有版本都内置了性能统计，运行时会显示：
```
Step 100: FPS = 45.23, 车辆数 = 51, 平均步时间 = 22.10ms
```
---
## ⚙️ 高级优化选项
### 调整激光雷达缓存频率
编辑 `run_multiagent_env_fast.py`：
```python
env.lidar_cache_interval = 3 # 改为5可进一步提速（但观测会更旧）
```
### 调整并行进程数
编辑 `run_multiagent_env_parallel.py`：
```python
num_workers = 10 # 改为更少的进程数（如果内存不足）
```
### 进一步减少激光束
编辑 `scenario_env.py` 的 `_get_all_obs()` 函数：
```python
lidar = self.engine.get_sensor("lidar").perceive(
num_lasers=20, # 从40进一步减少到20
distance=20, # 从30减少到20米
...
)
```
---
## 🎓 为什么CPU利用率低？
### 原因分析：
1. **单线程瓶颈**
- Python GIL限制
- MetaDrive主循环是单线程的
- 即使有10个核心，也只用1个
2. **I/O等待**
- 等待渲染完成（如果开启）
- 等待磁盘读取数据
3. **计算不均衡**
- 某些计算很重（激光雷达），某些很轻
- CPU在重计算之间有空闲
### 解决方案：
✅ **已实现：** 多进程并行（`run_multiagent_env_parallel.py`）
- 每个进程占用1个核心
- 10个进程可充分利用10核CPU
- CPU利用率可达90-100%
---
## 📚 相关文档
- `PERFORMANCE_OPTIMIZATION.md` - 详细的性能优化指南
- `GPU_ACCELERATION.md` - GPU加速的完整说明
---
## ❓ 常见问题
### Q: 为什么关闭渲染后FPS还是只有20？
A: 主要瓶颈是激光雷达计算，不是渲染。请使用 `run_multiagent_env_fast.py`。
### Q: GPU能加速训练吗？
A: 环境模拟在CPU，但策略网络可以在GPU上训练。
### Q: 如何最大化CPU利用率？
A: 使用 `run_multiagent_env_parallel.py` 多进程版本。
### Q: 会影响观测精度吗？
A: 激光束减少会略微降低精度，但实践中影响很小。缓存会让观测滞后1-2帧。
### Q: 如何恢复原始配置？
A: 使用 `run_multiagent_env_visual.py` 或修改配置文件中的参数。
---
## 🚦 下一步
1. 先测试 `run_multiagent_env_fast.py`，验证性能提升
2. 如果满意，用于日常训练
3. 需要大规模训练时，使用 `run_multiagent_env_parallel.py`
4. 考虑将策略网络迁移到GPU
祝训练顺利！🎉

BIN
Env/__pycache__/expert_replay_env.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/expert_replay_env.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/expert_replay_policy.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/inverse_dynamics.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/inverse_dynamics.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/logger_utils.cpython-310.pyc
View File

Binary file not shown.

BIN
Env/__pycache__/logger_utils.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/replay_policy.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/scenario_env.cpython-310.pyc
View File

Binary file not shown.

BIN
Env/__pycache__/scenario_env.cpython-313.pyc
View File

Binary file not shown.

BIN
Env/__pycache__/scenario_env.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
Env/__pycache__/simple_idm_policy.cpython-310.pyc
View File

Binary file not shown.

BIN
Env/__pycache__/simple_idm_policy.cpython-313.pyc
View File

Binary file not shown.

BIN
Env/__pycache__/simple_idm_policy.cpython-39.pyc Normal file
View File

Binary file not shown.

116
Env/example_with_logging.py
View File

@@ -1,116 +0,0 @@
"""
日志记录功能示例
演示如何在自定义脚本中使用日志功能
"""
from logger_utils import setup_logger
from datetime import datetime
import time
def example_without_logging():
"""示例1：不使用日志"""
print("=" * 60)
print("示例1：普通输出（不记录日志）")
print("=" * 60)
print("这是普通的print输出")
print("只会显示在终端")
print("不会保存到文件")
print()
def example_with_logging():
"""示例2：使用日志记录"""
print("=" * 60)
print("示例2：使用日志记录")
print("=" * 60)
# 使用with语句，自动管理日志文件
with setup_logger(log_file="example_demo.log", log_dir="logs"):
print("✅ 这条消息会同时输出到终端和文件")
print("✅ 运行一些计算...")
for i in range(5):
print(f" 步骤 {i+1}/5: 处理中...")
time.sleep(0.1)
print("✅ 计算完成！")
print("日志文件已关闭")
print()
def example_custom_filename():
"""示例3：使用时间戳命名"""
print("=" * 60)
print("示例3：自动生成时间戳文件名")
print("=" * 60)
# log_file=None 会自动生成时间戳文件名
with setup_logger(log_file=None, log_dir="logs"):
print("文件名会自动包含时间戳")
print("适合批量实验，避免覆盖")
print()
def example_append_mode():
"""示例4：追加模式"""
print("=" * 60)
print("示例4：追加到现有文件")
print("=" * 60)
# 第一次写入
with setup_logger(log_file="append_test.log", log_dir="logs", mode='w'):
print("第一次写入：这会覆盖文件")
# 第二次写入（追加）
with setup_logger(log_file="append_test.log", log_dir="logs", mode='a'):
print("第二次写入：这会追加到文件末尾")
print()
def example_complex_output():
"""示例5：复杂输出（包含颜色、格式）"""
print("=" * 60)
print("示例5：复杂输出格式")
print("=" * 60)
with setup_logger(log_file="complex_output.log", log_dir="logs"):
# 模拟多种输出格式
print("\n📊 实验统计：")
print(" - 实验名称：车道过滤测试")
print(" - 开始时间：", datetime.now().strftime("%Y-%m-%d %H:%M:%S"))
print(" - 车辆总数：51")
print(" - 过滤后：45")
print("\n🚦 红绿灯检测：")
print(" ✅ 方法1成功：3辆")
print(" ✅ 方法2成功：2辆")
print(" ⚠️ 未检测到：40辆")
print("\n" + "="*50)
print("实验完成！")
print()
def main():
"""运行所有示例"""
print("\n" + "🎯 " + "="*56)
print("日志记录功能完整示例")
print("="*60 + "\n")
example_without_logging()
example_with_logging()
example_custom_filename()
example_append_mode()
example_complex_output()
print("="*60)
print("✅ 所有示例运行完成！")
print("📁 查看日志文件：ls -lh logs/")
print("="*60)
if __name__ == "__main__":
main()

527
Env/expert_replay_env.py Normal file
View File

@@ -0,0 +1,527 @@
import logging
import numpy as np
from collections import defaultdict
from metadrive.component.vehicle.vehicle_type import DefaultVehicle
from metadrive.type import MetaDriveType
from Env.scenario_env import MultiAgentScenarioEnv, PolicyVehicle
from Env.inverse_dynamics import InverseDynamics
class ExpertReplayEnv(MultiAgentScenarioEnv):
def __init__(self, config=None):
# Allow passing config without agent2policy since we don't use policies for replay
if config is None:
config = {}
# Ensure we don't simulate physics for the controlled agents in the traditional sense
# but we still need the engine to run
super().__init__(config, agent2policy={})
self.inverse_dynamics = InverseDynamics()
self.expert_tracks = {}
# Replay SDC/ego ("default_agent" in MetaDrive) as well; otherwise it will keep default action=0 and look stuck.
self.replay_sdc = self.config.get("replay_sdc", True)
self.sdc_track = None
self.sdc_vehicle = None
self.sdc_agent_id = "default_agent"
def reset(self, seed=None):
self.round = 0
if self.logger is None:
from metadrive.engine.logger import get_logger, set_log_level
self.logger = get_logger()
log_level = self.config.get("log_level", logging.INFO)
set_log_level(log_level)
self.lazy_init()
self._reset_global_seed(seed)
if self.engine is None:
raise ValueError("Broken MetaDrive instance.")
self.background_vehicles = {} # Vehicles that exist but are static/background
# Helper function to check if a position is on a valid lane
def is_on_lane(pos, map_manager, threshold=2.0):
# Check if point is close to any lane in the road network
# This can be expensive if checked for every point, so we check sample points
# or rely on lane index if available.
# Waymo tracks don't have lane index, just positions.
# We can use map.road_network.get_closest_lane_index(pos)
if map_manager is None or map_manager.current_map is None:
return True # If no map, assume valid
try:
# Use a larger search radius to catch slightly offset lanes
lane, lane_index = map_manager.current_map.road_network.get_closest_lane_index(pos, return_lane=True)
if lane is None:
return False
# Check lateral distance
long, lat = lane.local_coordinates(pos)
width = lane.width
# Allow being slightly off-lane (e.g. changing lanes)
# But parking lots are usually far from defined lanes in Waymo converted maps
if abs(lat) <= (width / 2 + threshold):
return True
return False
except:
return False
# --- MODIFIED SECTION START ---
# Capture expert tracks before they are cleaned
self.expert_tracks = {}
# Capture SDC track for ego replay (MetaDrive default agent)
self.sdc_track = None
self.sdc_vehicle = None
if self.replay_sdc and hasattr(self.engine, "traffic_manager"):
sdc_sid = self.engine.traffic_manager.sdc_scenario_id
self.sdc_track = self.engine.traffic_manager.current_traffic_data.get(sdc_sid, None)
_obj_to_clean_this_frame = []
self.car_birth_info_list = []
# Pre-filter: Check tracks against map AND check for static vehicles
for scenario_id, track in self.engine.traffic_manager.current_traffic_data.items():
if scenario_id == self.engine.traffic_manager.sdc_scenario_id:
continue
else:
if track["type"] == MetaDriveType.VEHICLE:
_obj_to_clean_this_frame.append(scenario_id)
valid = track['state']['valid']
if not valid.any():
continue
first_show = np.argmax(valid)
last_show = len(valid) - 1 - np.argmax(valid[::-1])
mid_show = (first_show + last_show) // 2
# 1. Lane check (existing logic)
points_to_check = [first_show, mid_show, last_show]
on_road_count = 0
is_valid_track = True
start_pos = track['state']['position'][first_show]
if not is_on_lane(start_pos, self.engine.map_manager, threshold=5.0): # 5m tolerance
mid_pos = track['state']['position'][mid_show]
if not is_on_lane(mid_pos, self.engine.map_manager, threshold=5.0):
is_valid_track = False
# 2. Static check
# Calculate total displacement and max speed
positions = track['state']['position'][valid.astype(bool)]
velocities = track['state']['velocity'][valid.astype(bool)]
total_displacement = 0
max_speed = 0
if len(positions) > 1:
total_displacement = np.linalg.norm(positions[-1] - positions[0])
max_speed = np.max(np.linalg.norm(velocities, axis=1))
is_static = False
if total_displacement < 5.0 and max_speed < 1.0: # Relaxed threshold: <5m move and <1m/s
is_static = True
# Decision logic:
# - If off-road AND static: Skip completely (don't even spawn as background)
# - If off-road but moving: Maybe keep? Or skip? Usually off-road moving is weird, skip.
# - If on-road but static: Spawn as BACKGROUND (visible but not controlled agent)
# - If on-road and moving: Spawn as CONTROLLED agent
if not is_valid_track:
# Skip off-road vehicles entirely (both static and moving off-road)
continue
if is_static:
# Add to background list, but NOT to car_birth_info_list (which is for controlled agents)
# We need a way to spawn them. Let's add a separate list.
self.background_vehicles[scenario_id] = {
'id': track['metadata']['object_id'],
'show_time': first_show,
'begin': (track['state']['position'][first_show, 0], track['state']['position'][first_show, 1]),
'heading': track['state']['heading'][first_show],
'end': (track['state']['position'][last_show, 0], track['state']['position'][last_show, 1]),
'scenario_id': scenario_id,
'length': track['state']['length'][first_show],
'width': track['state']['width'][first_show],
'valid': valid # Need validity to know when to show/hide
}
continue # Do not add to controlled list
# Store the full track for replay (only for controlled agents)
self.expert_tracks[scenario_id] = track
self.car_birth_info_list.append({
'id': track['metadata']['object_id'],
'show_time': first_show,
'begin': (track['state']['position'][first_show, 0], track['state']['position'][first_show, 1]),
'heading': track['state']['heading'][first_show],
'end': (track['state']['position'][last_show, 0], track['state']['position'][last_show, 1]),
'scenario_id': scenario_id, # Keep track of original ID to lookup tracks
'length': track['state']['length'][first_show],
'width': track['state']['width'][first_show]
})
for scenario_id in _obj_to_clean_this_frame:
self.engine.traffic_manager.current_traffic_data.pop(scenario_id)
# --- MODIFIED SECTION END ---
self.engine.reset()
self.reset_sensors()
self.engine.taskMgr.step()
self.lanes = self.engine.map_manager.current_map.road_network.graph
if self.top_down_renderer is not None:
self.top_down_renderer.clear()
self.engine.top_down_renderer = None
self.dones = {}
self.episode_rewards = defaultdict(float)
self.episode_lengths = defaultdict(int)
self.controlled_agents.clear()
self.controlled_agent_ids.clear()
# We skip calling super().reset() to avoid double reset
# But we need to ensure ScenarioEnv-specific setup is done if any.
# ScenarioEnv.reset() basically does engine.reset() and some cleanup.
# We covered most of it.
self._spawn_controlled_agents()
self._spawn_background_vehicles() # Initial spawn for background
# Ensure SDC/ego is moved to the correct initial expert state.
if self.replay_sdc:
self.sdc_vehicle = self.engine.agent_manager.active_agents.get(self.sdc_agent_id, None)
if self.sdc_vehicle is not None and self.sdc_track is not None:
valid = self.sdc_track["state"]["valid"]
t0 = int(np.argmax(valid)) if valid.any() else 0
pos0 = self.sdc_track["state"]["position"][t0]
heading0 = self.sdc_track["state"]["heading"][t0]
vel0 = self.sdc_track["state"]["velocity"][t0]
self.sdc_vehicle.set_position(pos0)
self.sdc_vehicle.set_heading_theta(heading0)
self.sdc_vehicle.set_velocity(vel0)
return self._get_all_obs()
def _spawn_background_vehicles(self):
# Spawn static/background vehicles
# Since they are static, we might just spawn them once if their show_time is 0
# But Waymo tracks have valid bits, they might appear/disappear.
# For optimization, if they are truly static (never move), we just spawn them when show_time matches.
# We need to track spawned background vehicles to remove them if they become invalid?
# Since we defined them as "static", they probably stay put.
# But validity might change (e.g. late spawn).
# For simplicity in this step, let's just iterate and spawn if time matches
for sid, car in self.background_vehicles.items():
if car['show_time'] == self.round:
# Spawn as a Traffic Vehicle (not PolicyVehicle), or just a static object?
# Using DefaultVehicle is fine, but don't add to controlled_agents
# Check duplication
bg_id = f"bg_{car['id']}"
# if bg_id in self.engine.obj_to_id: # obj_to_id might not be available in all versions
if bg_id in self.engine.agent_manager.active_agents:
continue
vehicle_config = {}
if 'length' in car and 'width' in car:
vehicle_config = {
"length": car['length'],
"width": car['width']
}
v = self.engine.spawn_object(
DefaultVehicle,
name=bg_id,
vehicle_config=vehicle_config,
position=car['begin'],
heading=car['heading']
)
# Set color to grey/dark to indicate background
v.set_velocity([0, 0])
# Maybe set color? MetaDrive vehicles random color.
# v.set_color(...) if supported
# Register as an active object but NOT controlled agent
# The engine manages it.
# CRITICAL: We need it in self.engine.agent_manager.active_agents for Observation?
# If we want it to be seen by Lidar/Observation, it needs to be an "agent" or "traffic".
# DefaultVehicle spawned this way is just an object.
# We should add it to traffic manager? Or just leave it as object?
# MultiAgentScenarioEnv._get_all_obs iterates self.engine.agent_manager.active_agents
# If we want it in observation, we must add it to active_agents OR iterate over all objects.
# Adding to active_agents is easier for compatibility.
self.engine.agent_manager.active_agents[bg_id] = v
# Store valid mask to remove it later if needed?
v.valid_mask = car['valid']
v.start_t = car['show_time']
def _update_background_vehicles(self):
# Remove background vehicles if they become invalid
# Or spawn new ones
self._spawn_background_vehicles()
# Check validity for existing
to_remove = []
for aid, v in self.engine.agent_manager.active_agents.items():
if aid.startswith("bg_"):
# Check validity
if hasattr(v, 'valid_mask'):
curr_step = self.round
if curr_step >= len(v.valid_mask) or not v.valid_mask[curr_step]:
to_remove.append(aid)
for aid in to_remove:
self.engine.agent_manager.active_agents.pop(aid, None)
# if aid in self.engine.obj_to_id:
# self.engine.clear_objects([self.engine.obj_to_id[aid]])
# Instead, we should find the object by ID and clear it.
# Since we don't track obj directly, we can't easily clear it without obj ref.
# Wait, active_agents stores the vehicle object.
# So we can just clear that object.
pass
# Re-iterate to clear objects properly
for aid in to_remove:
# We need to find the vehicle object to clear it.
# But we popped it from active_agents.
# Wait, we should get it before pop.
pass
def _update_background_vehicles(self):
# Remove background vehicles if they become invalid
# Or spawn new ones
self._spawn_background_vehicles()
# Check validity for existing
to_remove = []
objects_to_clear = []
for aid, v in self.engine.agent_manager.active_agents.items():
if aid.startswith("bg_"):
# Check validity
if hasattr(v, 'valid_mask'):
curr_step = self.round
if curr_step >= len(v.valid_mask) or not v.valid_mask[curr_step]:
to_remove.append(aid)
objects_to_clear.append(v)
for aid in to_remove:
self.engine.agent_manager.active_agents.pop(aid, None)
if objects_to_clear:
self.engine.clear_objects(objects_to_clear)
def _spawn_controlled_agents(self):
for car in self.car_birth_info_list:
if car['show_time'] == self.round:
agent_id = f"controlled_{car['id']}"
# Check if we already have this agent (shouldn't happen with unique IDs but safety check)
if agent_id in self.controlled_agents:
continue
# Handling ID flickering / merging
# If this ID is new, check if there's an existing agent very close to its start position
# that just disappeared? (Not implemented here, complex logic)
# But we can check if there's an overlap with existing agents?
# For now, just spawn.
# Read vehicle type/size if available
vehicle_config = {}
if 'length' in car and 'width' in car:
vehicle_config = {
"length": car['length'],
"width": car['width']
}
vehicle = self.engine.spawn_object(
PolicyVehicle,
name=agent_id,
vehicle_config=vehicle_config,
position=car['begin'],
heading=car['heading']
)
vehicle.reset(position=car['begin'], heading=car['heading'])
# We don't set policy or destination in the same way, or maybe we do for compatibility
vehicle.set_destination(car['end'])
# Store extra info for replay
vehicle.expert_track = self.expert_tracks[car['scenario_id']]
vehicle.original_id = car['id']
self.controlled_agents[agent_id] = vehicle
self.controlled_agent_ids.append(agent_id)
self.engine.agent_manager.active_agents[agent_id] = vehicle
def step(self, action_dict=None):
# We ignore input action_dict for the purpose of controlling agents
# Instead, we calculate what the action *should* be
self.round += 1
expert_actions = {}
# 1. Update state of all controlled agents to the current timestep (self.round)
# and compute action from (self.round-1) to (self.round).
# Wait, usually step() moves T -> T+1.
# Current state is T. We want to move to T+1.
# So we need state at T and T+1.
# Identify agents that are done (valid=0 at T+1 or T+1 >= length)
agents_to_remove = []
# Update SDC/ego first (otherwise it will stay still with default action=0)
if self.replay_sdc and self.sdc_vehicle is not None and self.sdc_track is not None:
next_step = self.round
curr_step = self.round - 1
if next_step < len(self.sdc_track["state"]["position"]) and self.sdc_track["state"]["valid"][next_step]:
curr_state = {
"position": self.sdc_track["state"]["position"][curr_step],
"heading": self.sdc_track["state"]["heading"][curr_step],
"velocity": self.sdc_track["state"]["velocity"][curr_step],
}
next_state = {
"position": self.sdc_track["state"]["position"][next_step],
"heading": self.sdc_track["state"]["heading"][next_step],
"velocity": self.sdc_track["state"]["velocity"][next_step],
}
action, _ = self.inverse_dynamics.compute_action(curr_state, next_state, dt=0.1)
expert_actions[self.sdc_agent_id] = action
self.sdc_vehicle.set_position(next_state["position"])
self.sdc_vehicle.set_heading_theta(next_state["heading"])
self.sdc_vehicle.set_velocity(next_state["velocity"])
self.sdc_vehicle.last_expert_action = action
for agent_id, vehicle in self.controlled_agents.items():
track = vehicle.expert_track
# current_step = self.round - 1 # Since we incremented at start
# But vehicle is currently at state corresponding to self.round - 1.
# We want to move it to self.round.
# Check bounds
next_step = self.round
curr_step = self.round - 1
if next_step >= len(track['state']['position']):
agents_to_remove.append(agent_id)
continue
valid = track['state']['valid'][next_step]
if not valid:
agents_to_remove.append(agent_id)
continue
# Get states
curr_pos = track['state']['position'][curr_step]
next_pos = track['state']['position'][next_step]
curr_heading = track['state']['heading'][curr_step]
next_heading = track['state']['heading'][next_step]
curr_vel = track['state']['velocity'][curr_step]
next_vel = track['state']['velocity'][next_step]
# Prepare state dicts for Inverse Dynamics
curr_state = {
'position': curr_pos,
'heading': curr_heading,
'velocity': curr_vel
}
next_state = {
'position': next_pos,
'heading': next_heading,
'velocity': next_vel
}
# Calculate action
action, raw_info = self.inverse_dynamics.compute_action(curr_state, next_state, dt=0.1) # Waymo is 10Hz?
expert_actions[agent_id] = action
# Force update vehicle state
vehicle.set_position(next_pos)
vehicle.set_heading_theta(next_heading)
vehicle.set_velocity(next_vel)
# Also record this action in the vehicle for later retrieval if needed
vehicle.last_expert_action = action
# Remove finished agents
for agent_id in agents_to_remove:
vehicle = self.controlled_agents[agent_id]
self.controlled_agents.pop(agent_id)
self.controlled_agent_ids.remove(agent_id)
self.engine.agent_manager.active_agents.pop(agent_id, None)
self.engine.clear_objects([vehicle.id])
# Step physics world to update sensors/collision detection
# We don't need full integration, but we need to update the physics world state
self.engine.taskMgr.step()
# Spawn new agents for this turn
self._spawn_controlled_agents()
self._update_background_vehicles()
# Get observations
obs = self._get_all_obs()
rewards = {aid: 0.0 for aid in self.controlled_agents}
dones = {aid: False for aid in self.controlled_agents}
dones["__all__"] = (self.round >= self.config["horizon"]) or (len(self.controlled_agents) == 0 and self.round > 190) # Waymo scenarios are usually ~198 steps (20s @ 10Hz) or 90 steps (9s)
infos = {aid: {"expert_action": expert_actions.get(aid, np.zeros(2))} for aid in self.controlled_agents}
return obs, rewards, dones, infos
def _get_all_obs(self):
# Implement custom observation: 30m range, 10 nearest vehicles
obs_dict = {}
for agent_id, vehicle in self.controlled_agents.items():
# 1. Ego State
ego_state = [
vehicle.position[0], vehicle.position[1],
vehicle.velocity[0], vehicle.velocity[1],
vehicle.heading_theta
]
# 2. Neighbors
neighbors = []
# Iterate through all vehicles in the engine
candidates = []
for other_id, other_vehicle in self.engine.agent_manager.active_agents.items():
if other_id == agent_id:
continue
dist = np.linalg.norm(vehicle.position - other_vehicle.position)
if dist < 30.0:
candidates.append((dist, other_vehicle))
# Sort by distance
candidates.sort(key=lambda x: x[0])
# Take top 10
top_10 = candidates[:10]
neighbor_feats = []
for _, neighbor in top_10:
neighbor_feats.extend([
neighbor.position[0] - vehicle.position[0], # Relative pos
neighbor.position[1] - vehicle.position[1],
neighbor.velocity[0], # Absolute vel? or Relative? Usually relative in MultiAgent
neighbor.velocity[1]
])
# Pad if < 10
missing = 10 - len(top_10)
if missing > 0:
neighbor_feats.extend([0.0] * (4 * missing))
# Flatten
obs = np.array(ego_state + neighbor_feats, dtype=np.float32)
obs_dict[agent_id] = obs
return obs_dict

65
Env/inverse_dynamics.py Normal file
View File

@@ -0,0 +1,65 @@
import numpy as np
import math
class InverseDynamics:
def __init__(self, max_steering=0.7, max_acc=15.0, length=4.5):
"""
:param max_steering: Max steering angle in radians (approx 40 degrees)
:param max_acc: Max acceleration in m/s^2
:param length: Vehicle length in meters (Waymo default approx 4.5m)
"""
self.max_steering = max_steering
self.max_acc = max_acc
self.wheelbase = 0.7 * length # Approximation as per request
def compute_action(self, current_state, next_state, dt=0.1):
"""
Compute action [steering, acceleration] from current and next state.
State format: dictionary or object with keys/attrs: position (x, y), heading, velocity (v_x, v_y)
or numpy array [x, y, vx, vy, heading]
Using Bicycle Model:
delta = arctan(L * theta_dot / v)
acc = (v_next - v_curr) / dt
"""
# Extract state
# Assume state is dict-like for now, can adapt if needed
# We need: velocity (scalar), heading
# Helper to get speed
def get_speed(vel):
return np.linalg.norm(vel)
v_curr = get_speed(current_state['velocity'])
v_next = get_speed(next_state['velocity'])
# 1. Acceleration (longitudinal)
acc = (v_next - v_curr) / dt
# 2. Steering (lateral)
# theta_dot = (theta_next - theta_curr) / dt
theta_curr = current_state['heading']
theta_next = next_state['heading']
# Handle angle wrapping [-pi, pi]
diff_theta = theta_next - theta_curr
if diff_theta > np.pi:
diff_theta -= 2 * np.pi
elif diff_theta < -np.pi:
diff_theta += 2 * np.pi
theta_dot = diff_theta / dt
# Avoid division by zero for stationary vehicles
if v_curr < 0.1:
steering = 0.0
else:
# delta = arctan(L * theta_dot / v)
steering = np.arctan(self.wheelbase * theta_dot / v_curr)
# Normalize actions to [-1, 1]
norm_acc = np.clip(acc / self.max_acc, -1.0, 1.0)
norm_steering = np.clip(steering / self.max_steering, -1.0, 1.0)
return np.array([norm_steering, norm_acc]), {'raw_acc': acc, 'raw_steering': steering}

170
Env/logger_utils.py
View File

@@ -1,170 +0,0 @@
"""
日志工具模块
提供将终端输出同时保存到文件的功能
"""
import sys
import os
from datetime import datetime
class TeeLogger:
"""
双向输出类：同时输出到终端和文件
"""
def __init__(self, filename, mode='w', terminal=None):
"""
Args:
filename: 日志文件路径
mode: 文件打开模式 ('w'=覆盖, 'a'=追加)
terminal: 原始输出流（通常是sys.stdout或sys.stderr）
"""
self.terminal = terminal or sys.stdout
self.log_file = open(filename, mode, encoding='utf-8')
def write(self, message):
"""写入消息到终端和文件"""
self.terminal.write(message)
self.log_file.write(message)
self.log_file.flush() # 立即写入磁盘
def flush(self):
"""刷新缓冲区"""
self.terminal.flush()
self.log_file.flush()
def close(self):
"""关闭日志文件"""
if self.log_file:
self.log_file.close()
class LoggerContext:
"""
日志上下文管理器
使用with语句自动管理日志的开启和关闭
"""
def __init__(self, log_file=None, log_dir="logs", mode='w',
redirect_stdout=True, redirect_stderr=True):
"""
Args:
log_file: 日志文件名（None则自动生成时间戳文件名）
log_dir: 日志目录
mode: 文件打开模式 ('w'=覆盖, 'a'=追加)
redirect_stdout: 是否重定向标准输出
redirect_stderr: 是否重定向标准错误
"""
self.log_dir = log_dir
self.mode = mode
self.redirect_stdout = redirect_stdout
self.redirect_stderr = redirect_stderr
# 创建日志目录
os.makedirs(log_dir, exist_ok=True)
# 生成日志文件名
if log_file is None:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
log_file = f"run_{timestamp}.log"
self.log_path = os.path.join(log_dir, log_file)
# 保存原始的stdout和stderr
self.original_stdout = sys.stdout
self.original_stderr = sys.stderr
# 日志对象
self.stdout_logger = None
self.stderr_logger = None
def __enter__(self):
"""进入上下文：开启日志"""
print(f"📝 日志记录已启用")
print(f"📁 日志文件: {self.log_path}")
print("-" * 60)
# 创建TeeLogger对象
if self.redirect_stdout:
self.stdout_logger = TeeLogger(
self.log_path,
mode=self.mode,
terminal=self.original_stdout
)
sys.stdout = self.stdout_logger
if self.redirect_stderr:
self.stderr_logger = TeeLogger(
self.log_path,
mode='a', # stderr总是追加模式
terminal=self.original_stderr
)
sys.stderr = self.stderr_logger
return self
def __exit__(self, exc_type, exc_val, exc_tb):
"""退出上下文：关闭日志"""
# 恢复原始输出
sys.stdout = self.original_stdout
sys.stderr = self.original_stderr
# 关闭日志文件
if self.stdout_logger:
self.stdout_logger.close()
if self.stderr_logger:
self.stderr_logger.close()
print("-" * 60)
print(f"✅ 日志已保存到: {self.log_path}")
# 返回False表示不抑制异常
return False
def setup_logger(log_file=None, log_dir="logs", mode='w'):
"""
快速设置日志记录
Args:
log_file: 日志文件名（None则自动生成）
log_dir: 日志目录
mode: 文件模式 ('w'=覆盖, 'a'=追加)
Returns:
LoggerContext对象
Example:
with setup_logger("my_test.log"):
print("这条消息会同时输出到终端和文件")
"""
return LoggerContext(log_file=log_file, log_dir=log_dir, mode=mode)
def get_default_log_filename(prefix="run"):
"""
生成默认的日志文件名（带时间戳）
Args:
prefix: 文件名前缀
Returns:
str: 格式为 "prefix_YYYYMMDD_HHMMSS.log"
"""
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
return f"{prefix}_{timestamp}.log"
if __name__ == "__main__":
# 测试代码
print("测试1: 使用默认配置")
with setup_logger():
print("这是测试消息1")
print("这是测试消息2")
print("日志记录已结束\n")
print("测试2: 使用自定义文件名")
with setup_logger(log_file="test_custom.log"):
print("自定义文件名测试")
for i in range(3):
print(f" 消息 {i+1}")
print("完成")

49
Env/run_multiagent_env.py
View File

@@ -1,20 +1,10 @@
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
from Env.simple_idm_policy import ConstantVelocityPolicy
from metadrive.engine.asset_loader import AssetLoader
from logger_utils import setup_logger
import sys
import os
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/Env"
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/data"
def main(enable_logging=False, log_file=None):
"""
主函数
Args:
enable_logging: 是否启用日志记录到文件
log_file: 日志文件名（None则自动生成时间戳文件名）
"""
def main():
env = MultiAgentScenarioEnv(
config={
# "data_directory": AssetLoader.file_path(AssetLoader.asset_path, "waymo", unix_style=False),
@@ -26,18 +16,12 @@ def main(enable_logging=False, log_file=None):
"sequential_seed": True,
"reactive_traffic": True,
"manual_control": True,
# 车道检测与过滤配置
"filter_offroad_vehicles": True, # 启用车道区域过滤，过滤草坪等非车道区域的车辆
"lane_tolerance": 3.0, # 车道检测容差（米），可根据需要调整
"max_controlled_vehicles": 2, # 限制最大车辆数（可选，None表示不限制）
"debug_lane_filter": True,
"debug_traffic_light": True,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
obs = env.reset(0)
obs = env.reset(0
)
for step in range(10000):
actions = {
aid: env.controlled_agents[aid].policy.act()
@@ -54,25 +38,4 @@ def main(enable_logging=False, log_file=None):
if __name__ == "__main__":
# 解析命令行参数
enable_logging = "--log" in sys.argv or "-l" in sys.argv
# 提取自定义日志文件名
log_file = None
for arg in sys.argv:
if arg.startswith("--log-file="):
log_file = arg.split("=")[1]
break
if enable_logging:
# 使用日志记录
log_dir = os.path.join(os.path.dirname(__file__), "logs")
with setup_logger(log_file=log_file, log_dir=log_dir):
main(enable_logging=True, log_file=log_file)
else:
# 普通运行（只输出到终端）
print("💡 提示: 使用 --log 或 -l 参数启用日志记录")
print(" 示例: python run_multiagent_env.py --log")
print(" 自定义文件名: python run_multiagent_env.py --log --log-file=my_run.log")
print("-" * 60)
main(enable_logging=False)
main()

115
Env/run_multiagent_env_fast.py
View File

@@ -1,115 +0,0 @@
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
from metadrive.engine.asset_loader import AssetLoader
from logger_utils import setup_logger
import time
import sys
import os
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/Env"
def main(enable_logging=False):
"""极致性能优化版本 - 启用所有优化选项"""
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 300,
# 关闭所有渲染
"use_render": False,
"render_pipeline": False,
"image_observation": False,
"interface_panel": [],
"manual_control": False,
"show_fps": False,
"debug": False,
# 物理引擎优化
"physics_world_step_size": 0.02,
"decision_repeat": 5,
"sequential_seed": True,
"reactive_traffic": True,
# 车道检测与过滤配置
"filter_offroad_vehicles": True, # 过滤非车道区域的车辆
"lane_tolerance": 3.0,
"max_controlled_vehicles": 15, # 限制车辆数以提升性能
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
# 【关键优化】启用激光雷达缓存
# 每3帧才重新计算激光雷达，其余帧使用缓存
# 可将激光雷达计算量减少到原来的1/3
env.lidar_cache_interval = 3
obs = env.reset(0)
# 性能统计
start_time = time.time()
total_steps = 0
print("=" * 60)
print("极致性能模式")
print("激光雷达优化：80→40束 (前向), 10→6束 (侧向+车道线)")
print("激光雷达缓存：每3帧计算一次，中间帧使用缓存")
print("预期性能提升：3-5倍")
print("=" * 60)
for step in range(10000):
actions = {
aid: env.controlled_agents[aid].policy.act()
for aid in env.controlled_agents
}
obs, rewards, dones, infos = env.step(actions)
total_steps += 1
# 每100步输出一次性能统计
if step % 100 == 0 and step > 0:
elapsed = time.time() - start_time
fps = total_steps / elapsed
print(f"Step {step:4d}: FPS = {fps:6.2f}, 车辆数 = {len(env.controlled_agents):3d}, "
f"平均步时间 = {1000/fps:.2f}ms")
if dones["__all__"]:
break
# 最终统计
elapsed = time.time() - start_time
fps = total_steps / elapsed
print("\n" + "=" * 60)
print(f"总计: {total_steps} 步")
print(f"耗时: {elapsed:.2f}s")
print(f"平均FPS: {fps:.2f}")
print(f"单步平均耗时: {1000/fps:.2f}ms")
print("=" * 60)
env.close()
if __name__ == "__main__":
# 解析命令行参数
enable_logging = "--log" in sys.argv or "-l" in sys.argv
# 提取自定义日志文件名
log_file = None
for arg in sys.argv:
if arg.startswith("--log-file="):
log_file = arg.split("=")[1]
break
if enable_logging:
# 使用日志记录
log_dir = os.path.join(os.path.dirname(__file__), "logs")
with setup_logger(log_file=log_file or "run_fast.log", log_dir=log_dir):
main(enable_logging=True)
else:
# 普通运行（只输出到终端）
print("💡 提示: 使用 --log 或 -l 参数启用日志记录")
print("-" * 60)
main(enable_logging=False)

156
Env/run_multiagent_env_parallel.py
View File

@@ -1,156 +0,0 @@
"""
多进程并行版本 - 充分利用多核CPU
适合大规模数据收集和训练
"""
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
from metadrive.engine.asset_loader import AssetLoader
import time
import os
from multiprocessing import Pool, cpu_count
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/Env"
def run_single_env(args):
"""在单个进程中运行一个环境实例"""
seed, num_steps, worker_id = args
# 创建环境（每个进程独立）
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 300,
# 性能优化
"use_render": False,
"render_pipeline": False,
"image_observation": False,
"interface_panel": [],
"manual_control": False,
"show_fps": False,
"debug": False,
"physics_world_step_size": 0.02,
"decision_repeat": 5,
"sequential_seed": True,
"reactive_traffic": True,
# 车道检测与过滤配置
"filter_offroad_vehicles": True,
"lane_tolerance": 3.0,
"max_controlled_vehicles": 15,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
# 启用激光雷达缓存
env.lidar_cache_interval = 3
# 运行仿真
start_time = time.time()
obs = env.reset(seed)
total_steps = 0
total_agents = 0
for step in range(num_steps):
actions = {
aid: env.controlled_agents[aid].policy.act()
for aid in env.controlled_agents
}
obs, rewards, dones, infos = env.step(actions)
total_steps += 1
total_agents += len(env.controlled_agents)
if dones["__all__"]:
break
elapsed = time.time() - start_time
fps = total_steps / elapsed if elapsed > 0 else 0
avg_agents = total_agents / total_steps if total_steps > 0 else 0
env.close()
return {
'worker_id': worker_id,
'seed': seed,
'steps': total_steps,
'elapsed': elapsed,
'fps': fps,
'avg_agents': avg_agents,
}
def main():
"""主函数：协调多个并行环境"""
# 获取CPU核心数
num_cores = cpu_count()
# 建议使用物理核心数（12600KF是10核20线程，使用10个进程）
num_workers = min(10, num_cores)
print("=" * 80)
print(f"多进程并行模式")
print(f"CPU核心数: {num_cores}")
print(f"并行进程数: {num_workers}")
print(f"每个环境运行: 1000步")
print("=" * 80)
# 准备任务参数
num_steps_per_env = 1000
tasks = [(seed, num_steps_per_env, worker_id)
for worker_id, seed in enumerate(range(num_workers))]
# 启动多进程池
start_time = time.time()
with Pool(processes=num_workers) as pool:
results = pool.map(run_single_env, tasks)
total_elapsed = time.time() - start_time
# 统计结果
print("\n" + "=" * 80)
print("各进程执行结果：")
print("-" * 80)
print(f"{'Worker':<8} {'Seed':<6} {'Steps':<8} {'Time(s)':<10} {'FPS':<8} {'平均车辆数':<12}")
print("-" * 80)
total_steps = 0
total_fps = 0
for result in results:
print(f"{result['worker_id']:<8} "
f"{result['seed']:<6} "
f"{result['steps']:<8} "
f"{result['elapsed']:<10.2f} "
f"{result['fps']:<8.2f} "
f"{result['avg_agents']:<12.1f}")
total_steps += result['steps']
total_fps += result['fps']
print("-" * 80)
avg_fps_per_env = total_fps / len(results)
total_throughput = total_steps / total_elapsed
print(f"\n总体统计：")
print(f" 总步数: {total_steps}")
print(f" 总耗时: {total_elapsed:.2f}s")
print(f" 单环境平均FPS: {avg_fps_per_env:.2f}")
print(f" 总吞吐量: {total_throughput:.2f} steps/s")
print(f" 并行效率: {total_throughput / avg_fps_per_env:.1f}x")
print("=" * 80)
# 与单进程对比
print(f"\n性能对比：")
print(f" 单进程FPS (预估): ~30 FPS")
print(f" 多进程吞吐量: {total_throughput:.2f} steps/s")
print(f" 性能提升: {total_throughput / 30:.1f}x")
print("=" * 80)
if __name__ == "__main__":
main()

60
Env/run_multiagent_env_visual.py
View File

@@ -1,60 +0,0 @@
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
from metadrive.engine.asset_loader import AssetLoader
import time
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/Env"
def main():
"""带可视化的版本（低FPS，约15帧）"""
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 300,
# 可视化设置（牺牲性能）
"use_render": True,
"manual_control": False,
"sequential_seed": True,
"reactive_traffic": True,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
obs = env.reset(0)
start_time = time.time()
total_steps = 0
for step in range(10000):
actions = {
aid: env.controlled_agents[aid].policy.act()
for aid in env.controlled_agents
}
obs, rewards, dones, infos = env.step(actions)
env.render(mode="topdown") # 实时渲染
total_steps += 1
if step % 100 == 0 and step > 0:
elapsed = time.time() - start_time
fps = total_steps / elapsed
print(f"Step {step}: FPS = {fps:.2f}, 车辆数 = {len(env.controlled_agents)}")
if dones["__all__"]:
break
elapsed = time.time() - start_time
fps = total_steps / elapsed
print(f"\n总计: {total_steps} 步，耗时 {elapsed:.2f}s，平均FPS = {fps:.2f}")
env.close()
if __name__ == "__main__":
main()

303
Env/scenario_env.py
View File

@@ -53,13 +53,6 @@ class MultiAgentScenarioEnv(ScenarioEnv):
data_directory=None,
num_controlled_agents=3,
horizon=1000,
# 车道检测与过滤配置
filter_offroad_vehicles=True, # 是否过滤非车道区域的车辆
lane_tolerance=3.0, # 车道检测容差（米），用于放宽边界条件
max_controlled_vehicles=None, # 最大可控车辆数限制（None表示不限制）
# 调试模式配置
debug_traffic_light=False, # 是否启用红绿灯检测调试输出
debug_lane_filter=False, # 是否启用车道过滤调试输出
))
return config
@@ -69,9 +62,6 @@ class MultiAgentScenarioEnv(ScenarioEnv):
self.controlled_agent_ids = []
self.obs_list = []
self.round = 0
# 调试模式配置
self.debug_traffic_light = config.get("debug_traffic_light", False)
self.debug_lane_filter = config.get("debug_lane_filter", False)
super().__init__(config)
def reset(self, seed: Union[None, int] = None):
@@ -116,47 +106,6 @@ class MultiAgentScenarioEnv(ScenarioEnv):
self.lanes = self.engine.map_manager.current_map.road_network.graph
# 调试：场景信息统计
if self.debug_lane_filter or self.debug_traffic_light:
print(f"\n📍 场景信息统计:")
print(f" - 总车道数: {len(self.lanes)}")
# 统计红绿灯数量
if self.debug_traffic_light:
traffic_light_lanes = []
for lane in self.lanes.values():
if self.engine.light_manager.has_traffic_light(lane.lane.index):
traffic_light_lanes.append(lane.lane.index)
print(f" - 有红绿灯的车道数: {len(traffic_light_lanes)}")
if len(traffic_light_lanes) > 0:
print(f" 车道索引: {traffic_light_lanes[:5]}" +
(f" ... 共{len(traffic_light_lanes)}个" if len(traffic_light_lanes) > 5 else ""))
else:
print(f" ⚠️ 场景中没有红绿灯！")
# 在获取车道信息后，进行车道区域过滤
total_cars_before = len(self.car_birth_info_list)
valid_count, filtered_count, filtered_list = self._filter_valid_spawn_positions()
# 输出过滤信息
if filtered_count > 0:
self.logger.warning(f"车辆生成位置过滤: 原始 {total_cars_before} 辆, "
f"有效 {valid_count} 辆, 过滤 {filtered_count} 辆")
for filtered_car in filtered_list[:5]: # 只显示前5个
self.logger.debug(f" - 过滤车辆 ID={filtered_car['id']}, "
f"位置={filtered_car['position']}, "
f"原因={filtered_car['reason']}")
if filtered_count > 5:
self.logger.debug(f" - ... 还有 {filtered_count - 5} 辆车被过滤")
# 限制最大车辆数（在过滤后应用）
max_vehicles = self.config.get("max_controlled_vehicles", None)
if max_vehicles is not None and len(self.car_birth_info_list) > max_vehicles:
self.car_birth_info_list = self.car_birth_info_list[:max_vehicles]
self.logger.info(f"限制最大车辆数为 {max_vehicles} 辆")
self.logger.info(f"最终生成 {len(self.car_birth_info_list)} 辆可控车辆")
if self.top_down_renderer is not None:
self.top_down_renderer.clear()
self.engine.top_down_renderer = None
@@ -173,108 +122,6 @@ class MultiAgentScenarioEnv(ScenarioEnv):
return self._get_all_obs()
def _is_position_on_lane(self, position, tolerance=None):
"""
检测给定位置是否在有效车道范围内
Args:
position: (x, y) 车辆位置坐标
tolerance: 容差范围（米），用于放宽检测条件。None时使用配置中的默认值
Returns:
bool: True表示在车道上，False表示在非车道区域（如草坪、停车场等）
"""
if not hasattr(self, 'lanes') or self.lanes is None:
if self.debug_lane_filter:
print(f" ⚠️ 车道信息未初始化，默认允许")
return True # 如果车道信息未初始化，默认允许生成
if tolerance is None:
tolerance = self.config.get("lane_tolerance", 3.0)
position_2d = (position[0], position[1])
if self.debug_lane_filter:
print(f" 🔍 检测位置 ({position_2d[0]:.2f}, {position_2d[1]:.2f}), 容差={tolerance}m")
# 方法1：直接检测是否在任一车道上
checked_lanes = 0
for lane in self.lanes.values():
try:
checked_lanes += 1
if lane.lane.point_on_lane(position_2d):
if self.debug_lane_filter:
print(f" ✅ 在车道上 (车道{lane.lane.index}, 检查了{checked_lanes}条)")
return True
except:
continue
if self.debug_lane_filter:
print(f" ❌ 不在任何车道上 (检查了{checked_lanes}条车道)")
# 方法2：如果严格检测失败，使用容差范围检测（考虑车道边缘）
# 注释：此方法已被禁用，如需启用请取消注释
# if tolerance > 0:
# for lane in self.lanes.values():
# try:
# # 计算点到车道中心线的距离
# lane_obj = lane.lane
# # 获取车道长度并检测最近点
# s, lateral = lane_obj.local_coordinates(position_2d)
# # 如果横向距离在容差范围内，认为是有效的
# if abs(lateral) <= tolerance and 0 <= s <= lane_obj.length:
# return True
# except:
# continue
return False
def _filter_valid_spawn_positions(self):
"""
过滤掉生成位置不在有效车道上的车辆信息
根据配置决定是否执行过滤
Returns:
tuple: (有效车辆数量, 被过滤车辆数量, 被过滤车辆ID列表)
"""
# 如果配置中禁用了过滤，直接返回
if not self.config.get("filter_offroad_vehicles", True):
if self.debug_lane_filter:
print(f"🚫 车道过滤已禁用")
return len(self.car_birth_info_list), 0, []
if self.debug_lane_filter:
print(f"\n🔍 开始车道过滤: 共 {len(self.car_birth_info_list)} 辆车待检测")
valid_cars = []
filtered_cars = []
tolerance = self.config.get("lane_tolerance", 3.0)
for idx, car in enumerate(self.car_birth_info_list):
if self.debug_lane_filter:
print(f"\n车辆 {idx+1}/{len(self.car_birth_info_list)}: ID={car['id']}")
if self._is_position_on_lane(car['begin'], tolerance=tolerance):
valid_cars.append(car)
if self.debug_lane_filter:
print(f" ✅ 保留")
else:
filtered_cars.append({
'id': car['id'],
'position': car['begin'],
'reason': '生成位置不在有效车道上（可能在草坪/停车场等区域）'
})
if self.debug_lane_filter:
print(f" ❌ 过滤 (原因: 不在车道上)")
self.car_birth_info_list = valid_cars
if self.debug_lane_filter:
print(f"\n📊 过滤结果: 保留 {len(valid_cars)} 辆, 过滤 {len(filtered_cars)} 辆")
return len(valid_cars), len(filtered_cars), filtered_cars
def _spawn_controlled_agents(self):
# ego_vehicle = self.engine.agent_manager.active_agents.get("default_agent")
# ego_position = ego_vehicle.position if ego_vehicle else np.array([0, 0])
@@ -299,148 +146,26 @@ class MultiAgentScenarioEnv(ScenarioEnv):
# ✅ 关键：注册到引擎的 active_agents，才能参与物理更新
self.engine.agent_manager.active_agents[agent_id] = vehicle
def _get_traffic_light_state(self, vehicle):
"""
获取车辆当前位置的红绿灯状态（优化版）
解决问题：
1. 部分红绿灯状态为None的问题 - 添加异常处理和默认值
2. 车道分段导致无法获取红绿灯的问题 - 优先使用导航模块，失败时回退到遍历
Returns:
int: 0=无红绿灯, 1=绿灯, 2=黄灯, 3=红灯
"""
traffic_light = 0
state = vehicle.get_state()
position_2d = state['position'][:2]
if self.debug_traffic_light:
print(f"\n🚦 检测车辆红绿灯 - 位置: ({position_2d[0]:.1f}, {position_2d[1]:.1f})")
try:
# 方法1：优先尝试从车辆导航模块获取当前车道（更高效）
if hasattr(vehicle, 'navigation') and vehicle.navigation is not None:
current_lane = vehicle.navigation.current_lane
if self.debug_traffic_light:
print(f" 方法1-导航模块:")
print(f" current_lane = {current_lane}")
print(f" lane_index = {current_lane.index if current_lane else 'None'}")
if current_lane:
has_light = self.engine.light_manager.has_traffic_light(current_lane.index)
if self.debug_traffic_light:
print(f" has_traffic_light = {has_light}")
if has_light:
status = self.engine.light_manager._lane_index_to_obj[current_lane.index].status
if self.debug_traffic_light:
print(f" status = {status}")
if status == 'TRAFFIC_LIGHT_GREEN':
if self.debug_traffic_light:
print(f" ✅ 方法1成功: 绿灯")
return 1
elif status == 'TRAFFIC_LIGHT_YELLOW':
if self.debug_traffic_light:
print(f" ✅ 方法1成功: 黄灯")
return 2
elif status == 'TRAFFIC_LIGHT_RED':
if self.debug_traffic_light:
print(f" ✅ 方法1成功: 红灯")
return 3
elif status is None:
if self.debug_traffic_light:
print(f" ⚠️ 方法1: 红绿灯状态为None")
return 0
else:
if self.debug_traffic_light:
print(f" 该车道没有红绿灯")
else:
if self.debug_traffic_light:
print(f" 导航模块current_lane为None")
else:
if self.debug_traffic_light:
has_nav = hasattr(vehicle, 'navigation')
nav_not_none = vehicle.navigation is not None if has_nav else False
print(f" 方法1-导航模块: 不可用 (hasattr={has_nav}, not_none={nav_not_none})")
except Exception as e:
if self.debug_traffic_light:
print(f" ❌ 方法1异常: {type(e).__name__}: {e}")
pass
try:
# 方法2：遍历所有车道查找（兜底方案，处理车道分段问题）
if self.debug_traffic_light:
print(f" 方法2-遍历车道: 开始遍历 {len(self.lanes)} 条车道")
found_lane = False
checked_lanes = 0
for lane in self.lanes.values():
try:
checked_lanes += 1
if lane.lane.point_on_lane(position_2d):
found_lane = True
if self.debug_traffic_light:
print(f" ✓ 找到车辆所在车道: {lane.lane.index} (检查了{checked_lanes}条)")
has_light = self.engine.light_manager.has_traffic_light(lane.lane.index)
if self.debug_traffic_light:
print(f" has_traffic_light = {has_light}")
if has_light:
status = self.engine.light_manager._lane_index_to_obj[lane.lane.index].status
if self.debug_traffic_light:
print(f" status = {status}")
if status == 'TRAFFIC_LIGHT_GREEN':
if self.debug_traffic_light:
print(f" ✅ 方法2成功: 绿灯")
return 1
elif status == 'TRAFFIC_LIGHT_YELLOW':
if self.debug_traffic_light:
print(f" ✅ 方法2成功: 黄灯")
return 2
elif status == 'TRAFFIC_LIGHT_RED':
if self.debug_traffic_light:
print(f" ✅ 方法2成功: 红灯")
return 3
elif status is None:
if self.debug_traffic_light:
print(f" ⚠️ 方法2: 红绿灯状态为None")
return 0
else:
if self.debug_traffic_light:
print(f" 该车道没有红绿灯")
break
except:
continue
if self.debug_traffic_light and not found_lane:
print(f" ⚠️ 未找到车辆所在车道 (检查了{checked_lanes}条)")
except Exception as e:
if self.debug_traffic_light:
print(f" ❌ 方法2异常: {type(e).__name__}: {e}")
pass
if self.debug_traffic_light:
print(f" 结果: 返回 {traffic_light} (无红绿灯/未知)")
return traffic_light
def _get_all_obs(self):
# position, velocity, heading, lidar, navigation, TODO: trafficlight -> list
self.obs_list = []
for agent_id, vehicle in self.controlled_agents.items():
state = vehicle.get_state()
# 使用优化后的红绿灯检测方法
traffic_light = self._get_traffic_light_state(vehicle)
traffic_light = 0
for lane in self.lanes.values():
if lane.lane.point_on_lane(state['position'][:2]):
if self.engine.light_manager.has_traffic_light(lane.lane.index):
traffic_light = self.engine.light_manager._lane_index_to_obj[lane.lane.index].status
if traffic_light == 'TRAFFIC_LIGHT_GREEN':
traffic_light = 1
elif traffic_light == 'TRAFFIC_LIGHT_YELLOW':
traffic_light = 2
elif traffic_light == 'TRAFFIC_LIGHT_RED':
traffic_light = 3
else:
traffic_light = 0
break
lidar = self.engine.get_sensor("lidar").perceive(num_lasers=80, distance=30, base_vehicle=vehicle,
physics_world=self.engine.physics_world.dynamic_world)

219
Env/test_lane_filter.py
View File

@@ -1,219 +0,0 @@
"""
测试车道过滤和红绿灯检测功能
"""
from scenario_env import MultiAgentScenarioEnv
from simple_idm_policy import ConstantVelocityPolicy
from metadrive.engine.asset_loader import AssetLoader
from logger_utils import setup_logger
import os
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/Env"
def test_lane_filter():
"""测试车道过滤功能（基础版）"""
print("=" * 60)
print("测试1：车道过滤功能（基础）")
print("=" * 60)
# 创建启用过滤的环境
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 100,
"use_render": False,
# 车道过滤配置
"filter_offroad_vehicles": True,
"lane_tolerance": 3.0,
"max_controlled_vehicles": 10,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
print("\n启用车道过滤...")
obs = env.reset(0)
print(f"生成车辆数: {len(env.controlled_agents)}")
print(f"观测数据长度: {len(obs)}")
# 运行几步
for step in range(5):
actions = {aid: env.controlled_agents[aid].policy.act()
for aid in env.controlled_agents}
obs, rewards, dones, infos = env.step(actions)
env.close()
print("✓ 车道过滤测试通过\n")
def test_lane_filter_debug():
"""测试车道过滤功能（详细调试）"""
print("=" * 60)
print("测试1b：车道过滤功能（详细调试模式）")
print("=" * 60)
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 100,
"use_render": False,
# 车道过滤配置
"filter_offroad_vehicles": True,
"lane_tolerance": 3.0,
"max_controlled_vehicles": 5, # 只看前5辆车
# 🔥 启用调试模式
"debug_lane_filter": True, # 启用车道过滤调试
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
print("\n启用车道过滤调试...")
obs = env.reset(0)
env.close()
print("\n✓ 车道过滤调试测试完成\n")
def test_traffic_light():
"""测试红绿灯检测功能"""
print("=" * 60)
print("测试2：红绿灯检测功能（启用详细调试）")
print("=" * 60)
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 100,
"use_render": False,
"filter_offroad_vehicles": True,
"max_controlled_vehicles": 3, # 只测试3辆车
# 🔥 启用调试模式
"debug_traffic_light": True, # 启用红绿灯调试
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
obs = env.reset(0)
# 测试红绿灯检测（调试模式会自动输出详细信息）
print(f"\n" + "="*60)
print(f"开始逐车检测红绿灯状态（共 {len(env.controlled_agents)} 辆车）")
print("="*60)
for idx, (aid, vehicle) in enumerate(list(env.controlled_agents.items())[:3]): # 只测试前3辆
print(f"\n【车辆 {idx+1}/3】 ID={aid}")
traffic_light = env._get_traffic_light_state(vehicle)
state = vehicle.get_state()
status_text = {0: '无/未知', 1: '绿灯', 2: '黄灯', 3: '红灯'}[traffic_light]
print(f"最终结果: 红绿灯状态={traffic_light} ({status_text})\n")
env.close()
print("="*60)
print("✓ 红绿灯检测测试完成")
print("="*60 + "\n")
def test_without_filter():
"""测试禁用过滤的情况"""
print("=" * 60)
print("测试3：禁用过滤（对比测试）")
print("=" * 60)
env = MultiAgentScenarioEnv(
config={
"data_directory": AssetLoader.file_path(WAYMO_DATA_DIR, "exp_converted", unix_style=False),
"is_multi_agent": True,
"num_controlled_agents": 3,
"horizon": 100,
"use_render": False,
# 禁用过滤
"filter_offroad_vehicles": False,
"max_controlled_vehicles": None,
},
agent2policy=ConstantVelocityPolicy(target_speed=50)
)
print("\n禁用车道过滤...")
obs = env.reset(0)
print(f"生成车辆数（未过滤）: {len(env.controlled_agents)}")
env.close()
print("✓ 禁用过滤测试通过\n")
def run_tests(debug_mode=False):
"""运行测试的主函数"""
try:
if debug_mode:
print("🐛 调试模式启用")
print("=" * 60 + "\n")
test_lane_filter_debug()
test_traffic_light()
else:
print("⚡ 标准测试模式（使用 --debug 参数启用详细调试）")
print("=" * 60 + "\n")
test_lane_filter()
test_traffic_light()
test_without_filter()
print("\n" + "=" * 60)
print("✅ 所有测试通过！")
print("=" * 60)
print("\n功能说明：")
print("1. 车道过滤功能已启用，自动过滤非车道区域车辆")
print("2. 红绿灯检测采用双重策略，确保稳定获取状态")
print("3. 可通过配置参数灵活启用/禁用功能")
print("\n使用方法：")
print(" python Env/test_lane_filter.py # 标准测试")
print(" python Env/test_lane_filter.py --debug # 详细调试")
print(" python Env/test_lane_filter.py --log # 保存日志")
print(" python Env/test_lane_filter.py --debug --log # 调试+日志")
print("\n请运行 run_multiagent_env.py 查看完整效果")
except Exception as e:
print(f"\n❌ 测试失败: {e}")
import traceback
traceback.print_exc()
if __name__ == "__main__":
import sys
# 解析命令行参数
debug_mode = "--debug" in sys.argv or "-d" in sys.argv
enable_logging = "--log" in sys.argv or "-l" in sys.argv
# 提取自定义日志文件名
log_file = None
for arg in sys.argv:
if arg.startswith("--log-file="):
log_file = arg.split("=")[1]
break
if enable_logging:
# 启用日志记录
log_dir = os.path.join(os.path.dirname(__file__), "logs")
# 生成默认日志文件名
if log_file is None:
mode_suffix = "debug" if debug_mode else "standard"
from datetime import datetime
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
log_file = f"test_{mode_suffix}_{timestamp}.log"
with setup_logger(log_file=log_file, log_dir=log_dir):
run_tests(debug_mode=debug_mode)
else:
# 不启用日志，直接运行
run_tests(debug_mode=debug_mode)

141
README.md
View File

@@ -1,85 +1,98 @@
# MAGAIL4AutoDrive
### 1.1 环境搭建
环境核心代码封装于`Env`文件夹，通过运行`run_multiagent_env.py`即可启动多智能体交互环境，该脚本的核心功能为读取各智能体（车辆）的动作指令，并将其传入`env.step()`方法中完成仿真执行。
**性能优化版本：** 针对原始版本FPS低（15帧）和CPU利用率不足的问题，已提供多个优化版本：
- `run_multiagent_env_fast.py` - 激光雷达优化版（30-60 FPS，2-4倍提升）⭐推荐
- `run_multiagent_env_parallel.py` - 多进程并行版（300-600 steps/s总吞吐量，充分利用多核CPU）⭐⭐推荐
- 详见 `Env/QUICK_START.md` 快速使用指南
> 基于多智能体生成对抗模仿学习(MAGAIL)的自动驾驶训练系统 | MetaDrive + Waymo Open Motion Dataset
当前已初步实现`Env.senario_env.MultiAgentScenarioEnv.reset()`车辆生成函数，具体逻辑如下：首先读取专家数据集中各车辆的初始位姿信息；随后对原始数据进行清洗，剔除车辆 Agent 实例信息，记录核心参数（车辆 ID、初始生成位置、朝向角、生成时间戳、目标终点坐标）；最后调用`_spawn_controlled_agents()`函数，依据清洗后的参数在指定时间、指定位置生成搭载自动驾驶算法的可控车辆。
本项目利用 Waymo 真实驾驶数据，通过 MetaDrive 仿真环境构建专家回放系统，提取车辆状态与动作，用于训练多智能体模仿学习算法 (MAGAIL)。
**✅ 已解决：车辆生成位置偏差问题**
- **问题描述**：部分车辆生成于草坪、停车场等非车道区域，原因是专家数据记录误差或停车场特殊标注
- **解决方案**：实现了`_is_position_on_lane()`车道区域检测机制和`_filter_valid_spawn_positions()`过滤函数
- 检测逻辑：通过`point_on_lane()`判断位置是否在车道上，支持容差参数（默认3米）处理边界情况
- 双重检测：优先使用精确检测，失败时使用容差范围检测，确保车道边缘车辆不被误过滤
- 自动过滤：在`reset()`时自动过滤非车道区域车辆，并输出过滤统计信息
- **配置参数**：
- `filter_offroad_vehicles=True`：启用/禁用车道过滤功能
- `lane_tolerance=3.0`：车道检测容差（米），可根据场景调整
- `max_controlled_vehicles=10`：限制最大车辆数（可选）
- **使用示例**：在环境配置中设置上述参数即可自动启用，运行时会显示过滤信息（如"过滤5辆，保留45辆"）
## 📁 核心模块
* **`Env/expert_replay_env.py`**: 专家回放环境。核心类 `ExpertReplayEnv`，负责读取 Waymo 轨迹，计算逆动力学动作，并过滤非道路/静态车辆。
* **`Env/inverse_dynamics.py`**: 逆动力学模块。根据车辆位置和航向计算油门、刹车和转向动作。
* **`scripts/generate_expert_data.py`**: 数据收集脚本。批量运行场景并保存训练数据。
* **`scripts/visualize_replay.py`**: 可视化脚本。用于观察回放效果和数据质量。
### 1.2 观测获取
观测信息采集功能通过`Env.senario_env.MultiAgentScenarioEnv._get_all_obs()`函数实现，该函数支持遍历所有可控车辆并采集多维度观测数据，当前已实现的观测维度包括：车辆实时位置坐标、朝向角、行驶速度、雷达扫描点云（含障碍物与车道线特征）、导航信息（因场景复杂度较低，暂采用目标终点坐标直接作为导航输入）。
***
**✅ 已解决：红绿灯信息采集问题**
- **问题描述**：
- 问题1：部分红绿灯状态值为`None`，导致异常或错误判断
- 问题2：车道分段设计时，部分区域车辆无法匹配到红绿灯
- **解决方案**：实现了`_get_traffic_light_state()`优化方法，采用多级检测策略
- **方法1（优先）**：从车辆导航模块`vehicle.navigation.current_lane`获取当前车道，直接查询红绿灯状态（高效，自动处理车道分段）
- **方法2（兜底）**：遍历所有车道，通过`point_on_lane()`判断车辆位置，查找对应红绿灯（处理导航失败情况）
- **异常处理**：对状态为`None`的情况返回0（无红绿灯），所有异常均有try-except保护，确保不会中断程序
- **返回值规范**：0=无红绿灯/未知, 1=绿灯, 2=黄灯, 3=红灯
- **优势**：双重保障机制，优先用高效方法，失败时自动切换到兜底方案，确保所有场景都能正确获取红绿灯信息
## 🚀 1. 数据收集
### 生成专家数据
使用 `generate_expert_data.py` 脚本从 Waymo 数据集中批量提取 (State, Action) 对。
### 1.3 算法模块
本方案的核心创新点在于对 GAIL 算法的判别器进行改进，使其适配多智能体场景下 “输入长度动态变化”（车辆数量不固定）的特性，实现对整体交互场景的分类判断，进而满足多智能体自动驾驶环境的训练需求。算法核心代码封装于`Algorithm.bert.Bert`类，具体实现逻辑如下：
```bash
# 设置 Python 路径
export PYTHONPATH=$PYTHONPATH:.:./metadrive
1. 输入层处理：输入数据为维度`(N, input_dim)`的矩阵（其中`N`为当前场景车辆数量，`input_dim`为单车辆固定观测维度），初始化`Bert`类时需设置`input_dim`，确保输入维度匹配；
2. 嵌入层与位置编码：通过`projection`线性投影层将单车辆观测维度映射至预设的嵌入维度（`embed_dim`），随后叠加可学习的位置编码（`pos_embed`），以捕捉观测序列的时序与空间关联信息；
3. Transformer 特征提取：嵌入后的特征向量输入至多层`Transformer`网络（层数由`num_layers`参数控制），完成高阶特征交互与抽象；
4. 分类头设计：提供两种特征聚合与分类方案：若开启`CLS`模式，在嵌入层前拼接 1 个可学习的`CLS`标记，最终取`CLS`标记对应的特征向量输入全连接层完成分类；若关闭`CLS`模式，则对`Transformer`输出的所有车辆特征向量进行序列维度均值池化，再将池化后的全局特征输入全连接层。分类器支持可选的`Tanh`激活函数，以适配不同场景下的输出分布需求。
# 运行生成脚本
# --data_dir: Waymo 数据路径 (建议使用 exp_filtered)
# --output_dir: 结果保存路径
# --num_scenarios: 要处理的场景数量
python scripts/generate_expert_data.py \
--data_dir data/exp_filtered \
--output_dir data/training_data \
--num_scenarios 100 \
--start_index 0
```
**生成的 `.pkl` 文件结构**：
包含一个列表，每个元素是一条车辆轨迹（Trajectory Dictionary）：
* `obs`: `(T, 45)` - 观测矩阵。包含 Ego 状态 (5维) + 10辆邻居车相对信息 (40维)。
* `acts`: `(T, 2)` - 动作矩阵。`[Steering, Accel]`，归一化到 `[-1, 1]`。
* `agent_id`: 车辆 ID。
* `scenario_id`: 所属场景 ID。
### 1.4 动作执行
在当前环境测试阶段，暂沿用腾达的动作执行框架：为每辆可控车辆分配独立的`policy`模型，将单车辆观测数据输入对应`policy`得到动作指令后，传入`env.step()`完成仿真；同时在`before_step`阶段调用`_set_action()`函数，将动作指令绑定至车辆实例，最终由 MetaDrive 仿真系统完成物理动力学计算与场景渲染。
后续优化方向为构建 "参数共享式统一模型框架"，具体设计如下：所有车辆共用 1 个`policy`模型，通过参数共享机制实现模型的全局统一维护。该框架具备三重优势：一是避免多车辆独立模型带来的训练偏差（如不同模型训练程度不一致）；二是解决车辆数量动态变化时的模型管理问题（车辆新增无需额外初始化模型，车辆减少不丢失模型训练信息）；三是支持动作指令的并行计算，可显著提升每一步决策的迭代效率，适配大规模多智能体交互场景的训练需求。
**内置过滤器**：
脚本会自动过滤掉以下无效车辆：
1. **非道路车辆**：始终在停车场或路外行驶的车辆。
2. **静态车辆**：全称移动距离小于 5米 且速度从未超过 1m/s 的车辆（作为背景流存在，不收集数据）。
---
## 问题解决总结
## 🔍 2. 数据可视化与验证
### ✅ 已完成的优化
### 回放可视化
使用 `visualize_replay.py` 直观地观察回放效果，确认车辆行为是否自然，以及过滤逻辑是否生效。
1. **车辆生成位置偏差** - 实现车道区域检测和自动过滤，配置参数：`filter_offroad_vehicles`, `lane_tolerance`, `max_controlled_vehicles`
2. **红绿灯信息采集** - 采用双重检测策略（导航模块+遍历兜底），处理None状态和车道分段问题
3. **性能优化** - 提供多个优化版本（fast/parallel），FPS从15提升到30-60，支持多进程充分利用CPU
### 🧪 测试方法
```bash
# 测试车道过滤和红绿灯检测
python Env/test_lane_filter.py
# 运行标准版本（带过滤）
python Env/run_multiagent_env.py
# 运行高性能版本
python Env/run_multiagent_env_fast.py
# 运行可视化
# --horizon: 回放的最大步数 (Waymo 场景通常为 90 或 198 步)
python scripts/visualize_replay.py \
--data_dir data/exp_filtered \
--start_index 0 \
--num_scenarios 1 \
--horizon 200
```
### 📝 配置示例
```python
config = {
# 车道过滤
"filter_offroad_vehicles": True, # 启用车道过滤
"lane_tolerance": 3.0, # 容差范围（米）
"max_controlled_vehicles": 10, # 最大车辆数
# 其他配置...
}
**观察要点**：
* **受控车辆 (Controlled Agents)**：控制台会显示数量（如 `Controlled agents: 2`）。这些是真正产生数据的车辆。
* **背景车辆**：如果在渲染图中看到其他车（通常是路边停放的），但受控数量很少，说明静态过滤生效了。
### 数据分析
使用 `analyze_expert_data.py` 查看生成数据的统计分布。
```bash
python scripts/analyze_expert_data.py --data_path data/training_data/expert_data_0_100.pkl
```
---
## 🧠 3. 模型训练 (Next Steps)
有了 `data/training_data/` 下的专家数据后，您可以开始训练 MAGAIL 模型。
### 训练流程
1. **加载数据**：使用 `dataset/expert_dataset.py` 中的 `ExpertDataset` 类加载 `.pkl` 数据。
2. **初始化 MAGAIL**：
* **Generator (Policy)**: 接收观测 `(B, 45)`，输出动作 `(B, 2)`。
* **Discriminator**: 接收状态-动作对 `(s, a)`，判断是专家还是生成器。
3. **交互采样**：
* 在 `MultiAgentScenarioEnv`（非回放模式）中运行 Policy。
* 收集 Policy 生成的轨迹。
4. **对抗更新**：
* 利用专家数据和 Policy 数据训练 Discriminator。
* 利用 Discriminator 的输出作为 Reward (GAIL Reward) 训练 Policy (PPO/TRPO)。
### 推荐配置
* **Observation**: 45维 (Ego + 10 Neighbors)
* **Action**: 2维 Continuous (Steering, Accel)
* **Horizon**: 200 steps
* **Batch Size**: 1024+ (多智能体环境下数据量很大)

498
TRAINING_ARCHITECTURE.md Normal file
View File

@@ -0,0 +1,498 @@
# MAGAIL 训练方案架构文档
## 目录
1. [训练数据结构](#1-训练数据结构)
2. [多智能体训练机制](#2-多智能体训练机制)
3. [完整训练流程](#3-完整训练流程)
4. [当前项目问题](#4-当前项目问题)
5. [TensorBoard 日志问题](#5-tensorboard-日志问题)
---
## 1. 训练数据结构
### 1.1 数据维度
**观测空间 (Observation Space)**
- **维度**: 45维
- **组成**:
- **Ego状态** (5维): `[position_x, position_y, velocity_x, velocity_y, heading_theta]`
- **邻居信息** (40维): 最多10个邻居，每个邻居4维特征
- 每个邻居: `[relative_x, relative_y, velocity_x, velocity_y]`
- 如果邻居数量 < 10，用零填充
**动作空间 (Action Space)**
- **维度**: 2维
- **组成**: `[steering, accel]`
- **范围**: 归一化到 `[-1, 1]`
### 1.2 数据格式
**专家数据文件结构** (`.pkl` 文件):
```python
# 每个 .pkl 文件包含一个列表，每个元素是一条车辆轨迹
trajectories = [
{
'obs': np.array, # Shape: (T, 45) - T为轨迹长度（可变）
'acts': np.array, # Shape: (T, 2) - 对应的动作序列
'agent_id': str, # 车辆ID
'scenario_id': int # 场景ID
},
...
]
```
**数据特点**:
- 轨迹长度 `T` 是**可变的**，取决于车辆在场景中的存活时间
- 最小轨迹长度过滤: 只保留长度 > 10 的轨迹
- 数据已通过静态车辆过滤（移动距离 < 5m 且最大速度 < 1m/s 的车辆被过滤）
### 1.3 数据生成流程
**脚本**: `scripts/generate_expert_data.py`
**流程**:
1. 从 Waymo 数据 (`data/exp_filtered`) 加载场景
2. 使用 `ExpertReplayEnv` 回放专家轨迹
3. 通过逆动力学 (`Env/inverse_dynamics.py`) 计算动作
4. 构建45维观测（Ego + 10个最近邻居）
5. 过滤无效轨迹（长度 < 10）
6. 保存为 `.pkl` 文件到 `data/training_data/`
**关键代码位置**:
- 观测构建: `Env/expert_replay_env.py` 的 `_get_all_obs()` 方法
- 动作计算: `Env/inverse_dynamics.py` 的 `compute_action()` 方法
---
## 2. 多智能体训练机制
### 2.1 可变长度处理
**问题**: 不同场景中智能体数量不同，每个智能体的轨迹长度也不同。
**解决方案**:
1. **数据层面** (`dataset/magail_dataset.py`):
- 将轨迹**展平**为独立的 `(state, action)` 对
- 每个样本是独立的，不保留序列信息
- 这样所有轨迹可以统一处理，不受长度限制
```python
# MAGAILExpertDataset 的处理方式
for traj in self.trajectories:
obs = traj['obs'] # (T, 45)
acts = traj['acts'] # (T, 2)
# 展平为独立样本
for i in range(len(obs)):
self.flat_data.append((obs[i], acts[i])) # 每个样本: (45,), (2,)
```
2. **训练环境层面** (`train_magail.py`):
- 每个 episode 动态处理不同数量的智能体
- 在 rollout 循环中，为每个活跃智能体独立收集数据
- 所有智能体的数据合并到一个 `memory` 中
```python
# Rollout 循环
for agent_id, obs in obs_dict.items():
act, logprob = ppo_agent.select_action(obs)
actions[agent_id] = act
# 所有智能体的数据都存入同一个 memory
memory['states'].append(obs)
memory['actions'].append(actions[agent_id])
...
```
3. **观测维度固定**:
- 通过 `MAGAILScenarioEnv` 确保观测维度始终为45维
- 邻居数量不足时用零填充，保证维度一致
### 2.2 多智能体交互
**环境设置**:
- 使用 `MAGAILScenarioEnv` (继承自 `MultiAgentScenarioEnv`)
- 自定义 `_get_all_obs()` 方法，确保观测格式与专家数据一致
- 每个智能体独立选择动作，环境统一执行
**关键点**:
- 所有智能体共享同一个策略网络（参数共享）
- 每个智能体独立计算动作和奖励
- 数据收集时将所有智能体的经验合并
---
## 3. 完整训练流程
### 3.1 数据准备阶段
**步骤 1: 生成专家数据**
```bash
python scripts/generate_expert_data.py \
--data_dir data/exp_filtered \
--output_dir data/training_data \
--num_scenarios 100 \
--start_index 0
```
**输出**: `data/training_data/expert_data_*.pkl`
### 3.2 模型初始化
**网络架构**:
1. **Actor (策略网络)**:
- 输入: 45维状态
- 输出: 2维动作（连续）
- 结构: MLP (45 → 256 → 256 → 2)
- 输出分布: 高斯分布（均值 + 可学习标准差）
2. **Critic (价值网络)**:
- 输入: 45维状态
- 输出: 标量价值
- 结构: MLP (45 → 256 → 256 → 1)
3. **Discriminator (鉴别器)**:
- 输入: 45维状态 + 2维动作 = 47维
- 输出: 标量（0-1之间，表示专家概率）
- 结构: MLP (47 → 256 → 256 → 1) + Sigmoid
### 3.3 训练循环
**主循环** (`train_magail.py` 的 `train()` 函数):
```
For each episode:
1. 收集 Rollout
- 重置环境（随机选择场景）
- 运行策略收集轨迹
- 存储 (state, action, logprob, next_state, done)
2. 训练 Discriminator
- 采样专家批次
- 采样策略批次
- 更新鉴别器:
- Expert loss: BCE(D(s_e, a_e), 1)
- Policy loss: BCE(D(s_p, a_p), 0)
- Total: L_d = L_expert + L_policy
3. 计算 GAIL 奖励
- 对所有策略状态-动作对:
reward = -log(1 - D(s, a) + ε)
- 替换环境奖励
4. 更新策略 (PPO)
- 计算 GAE (Generalized Advantage Estimation)
- PPO 更新 (K epochs):
- 计算优势函数
- 计算策略损失（带clip）
- 计算价值损失
- 更新 Actor 和 Critic
```
### 3.4 训练目标
**Discriminator 目标**:
```
L_D = E_{(s,a)~π_E}[-log(D(s,a))] + E_{(s,a)~π_θ}[-log(1-D(s,a))]
```
- 最大化区分专家数据和策略数据的能力
**Policy (Generator) 目标**:
```
L_π = E_{(s,a)~π_θ}[-log(D(s,a))] - λ_H(π_θ)
```
- 通过 PPO 优化，使用 GAIL 奖励作为信号
- 最大化鉴别器给出的"专家概率"
- 同时保持策略熵（探索）
**PPO 更新**:
```python
# 优势函数 (GAE)
advantages = compute_gae(rewards, values, next_values, dones, gamma, lambda)
# 策略损失
ratios = exp(log_probs - old_log_probs)
surr1 = ratios * advantages
surr2 = clip(ratios, 1-ε, 1+ε) * advantages
policy_loss = -min(surr1, surr2) + 0.01 * entropy
# 价值损失
value_loss = MSE(critic(states), returns)
# 总损失
total_loss = policy_loss + 0.5 * value_loss
```
### 3.5 关键代码位置
- **训练主循环**: `train_magail.py:278-505`
- **PPO 更新**: `train_magail.py:90-146`
- **Discriminator 更新**: `train_magail.py:429-462`
- **GAIL 奖励计算**: `train_magail.py:472-477`
---
## 4. 当前项目问题
### 4.1 环境重置问题
**问题描述**:
- MetaDrive 环境在快速重置时可能出现对象清理不完整的问题
- 错误信息: "You should clear all generated objects..."
**当前处理**:
- 代码中已有异常处理机制（`train_magail.py:288-342`）
- 重置失败时会尝试关闭并重新创建环境
- 但可能导致训练不稳定
**建议修复**:
- 在每次重置前显式清理所有对象
- 增加重置间隔，避免过于频繁的重置
- 考虑使用环境池（Environment Pool）复用环境实例
### 4.2 观测维度对齐
**问题描述**:
- 原始 `MultiAgentScenarioEnv` 返回108维观测（包含Lidar）
- 专家数据使用45维观测
- 维度不匹配会导致训练失败
**当前解决方案**:
- 通过 `MAGAILScenarioEnv` 重写 `_get_all_obs()` 方法
- 确保训练环境与专家数据使用相同的观测格式
**代码位置**: `train_magail.py:223-262`
### 4.3 数据收集效率
**问题描述**:
- 每个 episode 都需要完整运行环境收集数据
- 可变长度轨迹导致 batch 大小不一致
- 可能影响训练稳定性
**当前处理**:
- 使用展平的数据集，每个样本独立
- 在 rollout 时收集所有智能体的数据，合并处理
**潜在改进**:
- 考虑使用经验回放缓冲区
- 实现轨迹级别的采样（保留序列信息）
### 4.4 内存管理
**问题描述**:
- 长时间训练可能导致内存泄漏
- 环境对象可能没有完全释放
**当前处理**:
- 代码中有显式的 `gc.collect()` 和 `torch.cuda.empty_cache()`
- 但可能不够彻底
**建议**:
- 定期检查内存使用
- 考虑限制 rollout 长度
- 使用更激进的清理策略
### 4.5 训练稳定性
**问题描述**:
- Discriminator 可能过早收敛，导致策略无法学习
- GAIL 奖励可能不稳定
**当前处理**:
- 使用标准的 GAIL 奖励公式: `-log(1 - D(s,a) + ε)`
- PPO 的 clip 机制提供稳定性
**潜在改进**:
- 考虑使用 WGAN-GP 或 LSGAN 损失
- 实现 Discriminator 的预训练
- 添加奖励归一化
---
## 5. TensorBoard 日志问题
### 5.1 问题分析
**现象**:
- `runs/magail_0112/` 目录下只有模型文件（`.pth`），没有 TensorBoard 事件文件（`events.out.tfevents.*`）
- 其他目录（`magail_full`, `magail_production`）有事件文件
**可能原因**:
1. **TensorBoard 未安装**:
- 代码中有 try-except 处理（`train_magail.py:269-274`）
- 如果 TensorBoard 未安装，`writer` 会被设置为 `None`
- 训练会继续，但不会写入日志
2. **日志写入失败**:
- 即使 `SummaryWriter` 创建成功，如果写入时出错，可能不会生成文件
- 需要检查是否有异常被静默捕获
3. **训练中断**:
- 如果训练在写入第一个日志前中断，可能没有事件文件
- 但模型文件已保存，说明训练至少运行了一段时间
### 5.2 检查方法
**步骤 1: 检查 TensorBoard 安装**
```bash
python -c "import tensorboard; print(tensorboard.__version__)"
```
**步骤 2: 检查训练脚本中的日志写入**
查看 `train_magail.py:493-496`:
```python
if writer:
writer.add_scalar('Loss/Discriminator', disc_loss.item(), i_episode)
writer.add_scalar('Loss/Policy', ppo_loss, i_episode)
writer.add_scalar('Reward/Mean_GAIL', np.mean(all_gail_rewards), i_episode)
```
**步骤 3: 检查日志目录权限**
```bash
ls -la runs/magail_0112/
```
### 5.3 解决方案
**方案 1: 确保 TensorBoard 已安装**
```bash
pip install tensorboard
```
**方案 2: 添加显式刷新**
在训练循环结束后，显式调用 `writer.flush()`:
```python
if writer:
writer.flush() # 确保数据写入磁盘
```
**方案 3: 添加日志验证**
在训练开始时检查日志目录:
```python
if writer:
# 测试写入
writer.add_scalar('Test/Initialization', 0.0, 0)
writer.flush()
print(f"TensorBoard logging enabled. Log dir: {args.log_dir}")
else:
print("WARNING: TensorBoard not available. Logging disabled.")
```
**方案 4: 使用文件日志作为备份**
即使 TensorBoard 不可用，也可以写入文本日志:
```python
import logging
logging.basicConfig(
filename=os.path.join(args.log_dir, 'training.log'),
level=logging.INFO
)
```
### 5.4 代码修复建议
**在 `train_magail.py` 中添加以下改进**:
1. **确保 disc_loss 在 CPU 上**:
```python
# 第425行附近
disc_loss = torch.tensor(0.0).cuda() # 改为 .cuda() 或保持 CPU
# 或者在使用时转换
if writer:
disc_loss_value = disc_loss.item() if isinstance(disc_loss, torch.Tensor) else disc_loss
writer.add_scalar('Loss/Discriminator', disc_loss_value, i_episode)
```
2. **添加显式刷新**:
```python
# 第496行后添加
if writer:
writer.flush() # 确保数据写入磁盘
```
3. **添加初始化验证**:
```python
# 第271行后添加
if writer:
# 测试写入
writer.add_scalar('Test/Initialization', 0.0, 0)
writer.flush()
print(f"✓ TensorBoard logging enabled. Log dir: {args.log_dir}")
# 检查文件是否创建
import glob
event_files = glob.glob(os.path.join(args.log_dir, "events.out.tfevents.*"))
if event_files:
print(f"✓ TensorBoard event file created: {event_files[0]}")
else:
print("⚠ WARNING: TensorBoard not available. Logging disabled.")
```
4. **在训练结束时确保关闭**:
```python
# 第505行后添加
if writer:
writer.flush() # 最后一次刷新
writer.close()
print(f"TensorBoard logs saved to {args.log_dir}")
```
### 5.5 验证修复
**重新训练测试**:
```bash
python train_magail.py \
--expert_data_dir data/training_data \
--data_dir data/exp_filtered \
--batch_size 1024 \
--max_episodes 10 \
--log_dir runs/test_tensorboard
```
**检查输出**:
```bash
# 应该看到事件文件
ls runs/test_tensorboard/events.out.tfevents.*
# 启动 TensorBoard
tensorboard --logdir runs/test_tensorboard
```
**对于 magail_0112 训练**:
由于该训练已经完成且没有日志文件，建议：
1. 检查训练时的控制台输出，确认是否有 "TensorBoard not installed" 消息
2. 如果确实没有 TensorBoard，可以重新运行少量 episode 来验证修复
3. 或者查看是否有其他日志文件（如 `training.log`）
---
## 附录: 关键文件清单
### 核心训练文件
- `train_magail.py`: 主训练脚本
- `dataset/magail_dataset.py`: 专家数据集加载
- `Env/expert_replay_env.py`: 专家回放环境
- `Env/scenario_env.py`: 多智能体场景环境
- `Env/inverse_dynamics.py`: 逆动力学计算
### 数据生成文件
- `scripts/generate_expert_data.py`: 专家数据生成
- `scripts/visualize_replay.py`: 数据可视化
- `scripts/analyze_expert_data.py`: 数据分析
### 配置文件
- `README.md`: 项目说明
- `TRAINING_ARCHITECTURE.md`: 本文档
---
## 总结
本项目的 MAGAIL 训练方案通过以下方式处理多智能体可变长度问题：
1. **数据层面**: 将轨迹展平为独立样本，统一处理
2. **环境层面**: 动态处理不同数量的智能体，合并经验
3. **网络层面**: 固定输入维度（45维），通过零填充处理邻居不足的情况
训练流程遵循标准的 GAIL 框架，使用 PPO 作为策略优化算法。当前主要问题集中在环境稳定性和日志记录方面，需要进一步优化。

BIN
analysis_results/distributions.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 316 KiB

BIN
analysis_results/statistics.pkl Normal file
View File

Binary file not shown.

0
dataset/__init__.py Normal file
View File

BIN
dataset/__pycache__/__init__.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
dataset/__pycache__/__init__.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
dataset/__pycache__/magail_dataset.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
dataset/__pycache__/magail_dataset.cpython-39.pyc Normal file
View File

Binary file not shown.

304
dataset/expert_dataset.py Normal file
View File

@@ -0,0 +1,304 @@
import sys
import os
current_dir = os.path.dirname(os.path.abspath(__file__))
project_root = os.path.dirname(current_dir)
sys.path.insert(0, os.path.join(project_root, "Env"))
import numpy as np
import torch
from torch.utils.data import Dataset
import pickle
from scenario_env import MultiAgentScenarioEnv
from metadrive.engine.asset_loader import AssetLoader
class DummyPolicy:
def act(self, *args, **kwargs):
return np.array([0.0, 0.0])
class ExpertTrajectoryDataset(Dataset):
"""
完整107维观测的专家轨迹数据集
"""
def __init__(self,
trajectory_data: dict,
observation_data: dict = None, # 可选的完整观测
sequence_length: int = 1,
extract_actions: bool = True):
"""
Args:
trajectory_data: 专家轨迹数据
observation_data: 完整107维观测数据(可选)
sequence_length: 序列长度
extract_actions: 是否提取动作
"""
self.trajectory_data = trajectory_data
self.observation_data = observation_data if observation_data else {}
self.sequence_length = sequence_length
self.extract_actions = extract_actions
# 构建索引
self.indices = []
for traj_id, traj in trajectory_data.items():
traj_len = traj["length"]
for start_idx in range(traj_len - sequence_length):
self.indices.append((traj_id, start_idx))
obs_dim = 107 if len(self.observation_data) > 0 else 5
print(f"专家数据集: {len(trajectory_data)} 条轨迹, "
f"{len(self.indices)} 个训练样本, 观测维度: {obs_dim}")
def __len__(self):
return len(self.indices)
def __getitem__(self, idx):
traj_id, start_idx = self.indices[idx]
traj = self.trajectory_data[traj_id]
end_idx = start_idx + self.sequence_length
# 如果有完整观测,使用完整观测(107维)
if traj_id in self.observation_data and len(self.observation_data[traj_id]) > 0:
obs_sequence = self.observation_data[traj_id]
states = obs_sequence[start_idx:end_idx] # (seq_len, 107)
else:
# 否则使用简化观测(5维)
positions = traj["positions"][start_idx:end_idx+1]
headings = traj["headings"][start_idx:end_idx+1]
velocities = traj["velocities"][start_idx:end_idx]
states = []
for i in range(self.sequence_length):
state = np.concatenate([
positions[i, :2], # x, y
velocities[i], # vx, vy
[headings[i]], # heading
])
states.append(state)
states = np.array(states)
if self.extract_actions:
positions = traj["positions"][start_idx:end_idx+1]
headings = traj["headings"][start_idx:end_idx+1]
velocities = traj["velocities"][start_idx:end_idx]
actions = self._extract_actions_from_states(
positions[:-1], positions[1:],
headings[:-1], headings[1:],
velocities
)
return torch.FloatTensor(states), torch.FloatTensor(actions)
else:
next_states = states[1:]
return torch.FloatTensor(states[:-1]), torch.FloatTensor(next_states)
def _extract_actions_from_states(self, pos_t, pos_t1, head_t, head_t1, vel_t):
"""从状态序列反推动作"""
actions = []
dt = 0.1
for i in range(len(pos_t)):
current_speed = np.linalg.norm(vel_t[i])
displacement = np.linalg.norm(pos_t1[i, :2] - pos_t[i, :2])
next_speed = displacement / dt
speed_change = (next_speed - current_speed) / dt
if speed_change >= 0:
throttle = np.clip(speed_change / 5.0, 0.0, 1.0)
else:
throttle = np.clip(speed_change / 8.0, -1.0, 0.0)
heading_change = head_t1[i] - head_t[i]
heading_change = np.arctan2(np.sin(heading_change), np.cos(heading_change))
steering = np.clip(heading_change / 0.2, -1.0, 1.0)
actions.append([throttle, steering])
return np.array(actions)
@staticmethod
def collect_with_full_obs(env_config, num_scenarios=10, save_path=None):
"""
✅ 使用env._get_all_obs()收集完整107维观测
这是正确的方法!直接利用环境已有的观测函数
"""
all_trajectories = {}
all_observations = {}
# 检查数据库
data_dir = env_config["config"]["data_directory"]
summary_path = os.path.join(data_dir, "dataset_summary.pkl")
with open(summary_path, 'rb') as f:
summary = pickle.load(f)
total_scenarios = len(summary)
print(f"数据库总场景数: {total_scenarios}")
if num_scenarios is None:
num_scenarios = total_scenarios
else:
num_scenarios = min(num_scenarios, total_scenarios)
print(f"计划收集(完整107维观测): {num_scenarios} 个场景")
for i in range(num_scenarios):
try:
# 创建环境
env = MultiAgentScenarioEnv(
config={
**env_config["config"],
"start_scenario_index": i,
"num_scenarios": 1,
},
agent2policy=env_config["agent2policy"]
)
# 重置环境
env.reset()
if not hasattr(env, 'expert_trajectories'):
print(f"⚠️ 场景 {i}: 缺少expert_trajectories")
env.close()
continue
expert_trajs = env.expert_trajectories
if len(expert_trajs) == 0:
print(f"⚠️ 场景 {i}: 无专家轨迹")
env.close()
continue
# 存储轨迹
scenario_id = env.engine.current_seed
for obj_id, traj in expert_trajs.items():
unique_id = f"scenario{i}_{obj_id}"
all_trajectories[unique_id] = traj
# ✅ 关键: 使用_get_all_obs()获取完整观测
# 创建agent_id到unique_id的映射
agent_to_unique = {}
for agent_id in env.controlled_agents.keys():
# 尝试匹配agent_id到expert_trajectories的obj_id
for obj_id in expert_trajs.keys():
if str(agent_id) in str(obj_id) or str(obj_id) in str(agent_id):
unique_id = f"scenario{i}_{obj_id}"
agent_to_unique[agent_id] = unique_id
all_observations[unique_id] = []
break
# 遍历场景的每一步,收集完整观测
max_steps = min([traj["length"] for traj in expert_trajs.values()])
for step in range(max_steps):
# ✅ 直接调用_get_all_obs()获取107维观测!
obs_list = env._get_all_obs()
# 存储每个agent的观测
for agent_idx, agent_id in enumerate(env.controlled_agents.keys()):
if agent_id in agent_to_unique:
unique_id = agent_to_unique[agent_id]
if agent_idx < len(obs_list):
# obs_list[agent_idx]已经是107维向量!
all_observations[unique_id].append(np.array(obs_list[agent_idx]))
# 执行零动作(保持场景状态)
actions = {aid: np.array([0.0, 0.0])
for aid in env.controlled_agents.keys()}
env.step(actions)
# 转换为numpy数组
for unique_id in list(all_observations.keys()):
if len(all_observations[unique_id]) > 0:
all_observations[unique_id] = np.array(all_observations[unique_id])
else:
del all_observations[unique_id]
env.close()
if (i + 1) % 5 == 0:
print(f"✓ 已收集 {i+1}/{num_scenarios}, "
f"轨迹: {len(all_trajectories)}, "
f"观测: {len(all_observations)}")
except Exception as e:
print(f"✗ 场景 {i} 收集失败: {e}")
import traceback
traceback.print_exc()
try:
env.close()
except:
pass
continue
print(f"\n收集完成!")
print(f" 轨迹数: {len(all_trajectories)}")
print(f" 完整观测数: {len(all_observations)}")
# 验证观测维度
if len(all_observations) > 0:
sample_obs = list(all_observations.values())[0]
if len(sample_obs) > 0:
obs_dim = len(sample_obs[0])
print(f" 观测维度: {obs_dim} (应为107)")
if save_path:
with open(save_path, "wb") as f:
pickle.dump({
"trajectories": all_trajectories,
"observations": all_observations
}, f)
print(f"数据已保存到: {save_path}")
return all_trajectories, all_observations
if __name__ == "__main__":
WAYMO_DATA_DIR = r"/home/huangfukk/mdsn"
data_dir = AssetLoader.file_path(WAYMO_DATA_DIR, "exp_filtered", unix_style=False)
env_config = {
"config": {
"data_directory": data_dir,
"is_multi_agent": True,
"num_controlled_agents": 3,
"use_render": False,
"sequential_seed": True,
},
"agent2policy": DummyPolicy()
}
print("=" * 60)
print("选择收集模式:")
print("1. 简化观测(5维) - 快速,已验证 ✅")
print("2. 完整观测(107维) - 使用_get_all_obs() ⭐")
print("=" * 60)
mode = input("请选择模式(1或2,默认1): ").strip() or "1"
if mode == "2":
print("\n开始收集完整107维观测...")
trajectories, observations = ExpertTrajectoryDataset.collect_with_full_obs(
env_config,
num_scenarios=10,
save_path="./expert_trajectories_full.pkl"
)
if len(trajectories) > 0:
dataset = ExpertTrajectoryDataset(
trajectories,
observations,
sequence_length=1
)
state, action = dataset[0]
print(f"\n数据集测试:")
print(f" 总轨迹数: {len(trajectories)}")
print(f" 总观测数: {len(observations)}")
print(f" 训练样本数: {len(dataset)}")
print(f" 状态维度: {state.shape}")
print(f" 动作维度: {action.shape}")
else:
print("\n开始收集简化5维观测...")
# 保持原有的简化版本代码...
print("(使用之前已成功的方法)")

61
dataset/magail_dataset.py Normal file
View File

@@ -0,0 +1,61 @@
import torch
from torch.utils.data import Dataset
import pickle
import numpy as np
import os
import glob
class MAGAILExpertDataset(Dataset):
def __init__(self, data_dir, transform=None):
"""
Args:
data_dir (str): Directory containing .pkl files from generate_expert_data.py
transform (callable, optional): Optional transform to be applied on a sample.
"""
self.data_dir = data_dir
self.transform = transform
self.trajectories = []
self.flat_data = [] # (obs, act) pairs
# Load all .pkl files
pkl_files = glob.glob(os.path.join(data_dir, "*.pkl"))
print(f"Loading data from {len(pkl_files)} files in {data_dir}...")
for pkl_file in pkl_files:
try:
with open(pkl_file, 'rb') as f:
data = pickle.load(f)
# data is a list of dicts: {'obs': (T, 45), 'acts': (T, 2), ...}
self.trajectories.extend(data)
except Exception as e:
print(f"Error loading {pkl_file}: {e}")
# Flatten for training Discriminator/BC
print(f"Processing {len(self.trajectories)} trajectories...")
for traj in self.trajectories:
obs = traj['obs']
acts = traj['acts']
# obs: (T, 45), acts: (T, 2)
# We pair them up
for i in range(len(obs)):
self.flat_data.append((obs[i], acts[i]))
print(f"Total samples: {len(self.flat_data)}")
def __len__(self):
return len(self.flat_data)
def __getitem__(self, idx):
obs, act = self.flat_data[idx]
# Convert to tensor
obs = torch.from_numpy(obs).float()
act = torch.from_numpy(act).float()
sample = {'state': obs, 'action': act}
if self.transform:
sample = self.transform(sample)
return sample

BIN
expert_trajectories_full.pkl Normal file
View File

Binary file not shown.

BIN
expert_trajectories_full_obs.pkl Normal file
View File

Binary file not shown.

0
scripts/__init__.py Normal file
View File

256
scripts/analyze_expert_data.py Normal file
View File

@@ -0,0 +1,256 @@
import sys
import os
# 添加路径
current_dir = os.path.dirname(os.path.abspath(__file__))
project_root = os.path.dirname(current_dir)
env_dir = os.path.join(project_root, "Env")
sys.path.insert(0, project_root)
sys.path.insert(0, env_dir)
import numpy as np
import matplotlib.pyplot as plt
from collections import defaultdict
from scenario_env import MultiAgentScenarioEnv
from metadrive.engine.asset_loader import AssetLoader
import pickle
import os
class DummyPolicy:
"""占位策略"""
def act(self, *args, **kwargs):
return np.array([0.0, 0.0])
class ExpertDataAnalyzer:
def __init__(self, data_directory):
self.data_directory = data_directory
self.env = MultiAgentScenarioEnv(
config={
"data_directory": data_directory,
"is_multi_agent": True,
"num_controlled_agents": 3,
"use_render": False,
"sequential_seed": True,
},
agent2policy=DummyPolicy() # 添加必需参数
)
self.statistics = {
"num_scenarios": 0,
"num_trajectories": 0,
"trajectory_lengths": [],
"velocities": [],
"speeds": [], # 速度大小
"accelerations": [],
"heading_changes": [],
"inter_vehicle_distances": [],
"num_vehicles_per_scenario": [],
"static_vehicles": 0, # 统计静止车辆
}
def analyze_all_scenarios(self, num_scenarios=None):
"""遍历所有场景并收集统计信息"""
scenario_count = 0
while True:
try:
obs = self.env.reset()
if not hasattr(self.env, 'expert_trajectories'):
print("⚠️ 环境缺少expert_trajectories属性")
break
expert_trajs = self.env.expert_trajectories
if len(expert_trajs) == 0:
continue
scenario_count += 1
self.statistics["num_scenarios"] += 1
self.statistics["num_vehicles_per_scenario"].append(len(expert_trajs))
# 分析每条轨迹
for obj_id, traj in expert_trajs.items():
self.analyze_single_trajectory(traj)
# 分析车辆间交互
self.analyze_vehicle_interactions(expert_trajs)
print(f"已分析场景 {scenario_count}/{num_scenarios}, 车辆数: {len(expert_trajs)}")
if num_scenarios and scenario_count >= num_scenarios:
break
except Exception as e:
print(f"场景 {scenario_count} 处理失败: {e}")
break
self.env.close()
def analyze_single_trajectory(self, traj):
"""分析单条轨迹"""
self.statistics["num_trajectories"] += 1
length = traj["length"]
self.statistics["trajectory_lengths"].append(length)
# 速度分析
velocities = traj["velocities"]
speeds = np.linalg.norm(velocities, axis=1)
self.statistics["velocities"].extend(velocities.tolist())
self.statistics["speeds"].extend(speeds.tolist())
# 检查是否为静止车辆
if np.max(speeds) < 0.5: # 最大速度小于0.5m/s视为静止
self.statistics["static_vehicles"] += 1
# 加速度分析
if length > 1:
accelerations = np.diff(speeds) * 10 # 10Hz数据
self.statistics["accelerations"].extend(accelerations.tolist())
# 航向角变化
headings = traj["headings"]
if length > 1:
heading_changes = np.diff(headings)
heading_changes = np.arctan2(np.sin(heading_changes), np.cos(heading_changes))
self.statistics["heading_changes"].extend(heading_changes.tolist())
def analyze_vehicle_interactions(self, expert_trajs):
"""分析车辆间的距离"""
if len(expert_trajs) < 2:
return
traj_list = list(expert_trajs.values())
for i in range(len(traj_list)):
for j in range(i+1, len(traj_list)):
traj_i = traj_list[i]
traj_j = traj_list[j]
start_time = max(traj_i["start_timestep"], traj_j["start_timestep"])
end_time = min(traj_i["end_timestep"], traj_j["end_timestep"])
if start_time >= end_time:
continue
idx_i_start = start_time - traj_i["start_timestep"]
idx_i_end = end_time - traj_i["start_timestep"]
idx_j_start = start_time - traj_j["start_timestep"]
idx_j_end = end_time - traj_j["start_timestep"]
pos_i = traj_i["positions"][idx_i_start:idx_i_end, :2]
pos_j = traj_j["positions"][idx_j_start:idx_j_end, :2]
distances = np.linalg.norm(pos_i - pos_j, axis=1)
self.statistics["inter_vehicle_distances"].extend(distances.tolist())
def generate_report(self, save_dir="./analysis_results"):
"""生成统计报告"""
os.makedirs(save_dir, exist_ok=True)
stats = self.statistics
print("\n" + "="*60)
print("专家数据集统计报告")
print("="*60)
print(f"总场景数: {stats['num_scenarios']}")
print(f"总轨迹数: {stats['num_trajectories']}")
print(f"静止车辆数: {stats['static_vehicles']} ({stats['static_vehicles']/stats['num_trajectories']*100:.1f}%)")
print(f"平均每场景车辆数: {np.mean(stats['num_vehicles_per_scenario']):.2f} ± {np.std(stats['num_vehicles_per_scenario']):.2f}")
print(f"\n轨迹长度统计 (帧数 @ 10Hz):")
print(f" 平均: {np.mean(stats['trajectory_lengths']):.2f} 帧 ({np.mean(stats['trajectory_lengths'])*0.1:.2f}秒)")
print(f" 中位数: {np.median(stats['trajectory_lengths']):.2f} 帧")
print(f" 最小/最大: {np.min(stats['trajectory_lengths'])} / {np.max(stats['trajectory_lengths'])} 帧")
print(f"\n速度统计 (m/s):")
speeds = np.array(stats['speeds'])
print(f" 平均: {np.mean(speeds):.2f} ± {np.std(speeds):.2f}")
print(f" 中位数: {np.median(speeds):.2f}")
print(f" 最小/最大: {np.min(speeds):.2f} / {np.max(speeds):.2f}")
print(f" 静止帧(<0.5m/s): {np.sum(speeds < 0.5)} ({np.sum(speeds < 0.5)/len(speeds)*100:.1f}%)")
print(f"\n加速度统计 (m/s²):")
accs = np.array(stats['accelerations'])
print(f" 平均: {np.mean(accs):.4f} ± {np.std(accs):.2f}")
print(f" 最小/最大: {np.min(accs):.2f} / {np.max(accs):.2f}")
if len(stats['inter_vehicle_distances']) > 0:
dists = np.array(stats['inter_vehicle_distances'])
print(f"\n车辆间距离统计 (m):")
print(f" 平均: {np.mean(dists):.2f} ± {np.std(dists):.2f}")
print(f" 最小: {np.min(dists):.2f}")
print(f" 近距离交互(<5m): {np.sum(dists < 5.0)} ({np.sum(dists < 5.0)/len(dists)*100:.2f}%)")
# 保存数据
with open(os.path.join(save_dir, "statistics.pkl"), "wb") as f:
pickle.dump(stats, f)
# 绘制可视化
self.plot_distributions(save_dir)
print(f"\n✓ 报告已保存到: {save_dir}")
def plot_distributions(self, save_dir):
"""绘制分布图"""
stats = self.statistics
fig, axes = plt.subplots(2, 3, figsize=(15, 10))
# 1. 轨迹长度分布
axes[0, 0].hist(stats['trajectory_lengths'], bins=50, edgecolor='black')
axes[0, 0].set_xlabel('Trajectory Length (frames @ 10Hz)')
axes[0, 0].set_ylabel('Frequency')
axes[0, 0].set_title('Trajectory Length Distribution')
axes[0, 0].axvline(np.mean(stats['trajectory_lengths']), color='red',
linestyle='--', label=f'Mean: {np.mean(stats["trajectory_lengths"]):.1f}')
axes[0, 0].legend()
# 2. 速度分布
axes[0, 1].hist(stats['speeds'], bins=50, edgecolor='black')
axes[0, 1].set_xlabel('Speed (m/s)')
axes[0, 1].set_ylabel('Frequency')
axes[0, 1].set_title('Speed Distribution')
axes[0, 1].axvline(np.mean(stats['speeds']), color='red',
linestyle='--', label=f'Mean: {np.mean(stats["speeds"]):.2f}')
axes[0, 1].legend()
# 3. 加速度分布
axes[0, 2].hist(stats['accelerations'], bins=50, edgecolor='black')
axes[0, 2].set_xlabel('Acceleration (m/s²)')
axes[0, 2].set_ylabel('Frequency')
axes[0, 2].set_title('Acceleration Distribution')
# 4. 每场景车辆数
axes[1, 0].hist(stats['num_vehicles_per_scenario'], bins=30, edgecolor='black')
axes[1, 0].set_xlabel('Vehicles per Scenario')
axes[1, 0].set_ylabel('Frequency')
axes[1, 0].set_title('Vehicles per Scenario')
# 5. 航向角变化
axes[1, 1].hist(stats['heading_changes'], bins=50, edgecolor='black')
axes[1, 1].set_xlabel('Heading Change (rad)')
axes[1, 1].set_ylabel('Frequency')
axes[1, 1].set_title('Heading Change Distribution')
# 6. 车辆间距离
if len(stats['inter_vehicle_distances']) > 0:
axes[1, 2].hist(stats['inter_vehicle_distances'], bins=50,
range=(0, 50), edgecolor='black')
axes[1, 2].set_xlabel('Inter-vehicle Distance (m)')
axes[1, 2].set_ylabel('Frequency')
axes[1, 2].set_title('Distance Distribution')
plt.tight_layout()
plt.savefig(os.path.join(save_dir, "distributions.png"), dpi=300)
print(f" ✓ 分布图已保存")
if __name__ == "__main__":
WAYMO_DATA_DIR = r"/home/huangfukk/MAGAIL4AutoDrive/data"
data_dir = AssetLoader.file_path(WAYMO_DATA_DIR, "exp_filtered", unix_style=False)
print("开始分析专家数据...")
analyzer = ExpertDataAnalyzer(data_dir)
analyzer.analyze_all_scenarios(num_scenarios=100) # 分析100个场景
analyzer.generate_report()

47
scripts/check_database_info.py Normal file
View File

@@ -0,0 +1,47 @@
import pickle
import os
# 检查过滤后的数据库
filtered_db = "/home/huangfukk/mdsn/exp_filtered"
print("="*60)
print("过滤后数据库信息")
print("="*60)
# 读取summary
summary_path = os.path.join(filtered_db, "dataset_summary.pkl")
with open(summary_path, 'rb') as f:
summary = pickle.load(f)
print(f"\n总场景数: {len(summary)}")
print(f"场景ID列表(前10个): {list(summary.keys())[:10]}")
# 读取mapping
mapping_path = os.path.join(filtered_db, "dataset_mapping.pkl")
with open(mapping_path, 'rb') as f:
mapping = pickle.load(f)
print(f"\n映射关系数量: {len(mapping)}")
# 检查第一个场景的详细信息
first_scenario_id = list(summary.keys())[0]
first_scenario_info = summary[first_scenario_id]
print(f"\n第一个场景详细信息:")
print(f" 场景ID: {first_scenario_id}")
print(f" 元数据: {first_scenario_info}")
# 检查映射的文件路径
first_scenario_path = mapping[first_scenario_id]
print(f" 场景文件路径(相对): {first_scenario_path}")
# 检查文件是否存在
abs_path = os.path.join(filtered_db, first_scenario_path)
print(f" 场景文件路径(绝对): {abs_path}")
print(f" 文件存在: {os.path.exists(abs_path)}")
# 统计源数据库的场景文件
converted_db = "/home/huangfukk/mdsn/exp_converted"
converted_files = [f for f in os.listdir(converted_db) if f.endswith('.pkl') and f.startswith('sd_')]
print(f"\n源数据库 exp_converted:")
print(f" 场景文件数量: {len(converted_files)}")
print(f" 示例文件: {converted_files[:5]}")

177
scripts/check_track_fields.py Normal file
View File

@@ -0,0 +1,177 @@
import sys
import os
# 添加路径
current_dir = os.path.dirname(os.path.abspath(__file__))
project_root = os.path.dirname(current_dir)
env_dir = os.path.join(project_root, "Env")
sys.path.insert(0, project_root)
sys.path.insert(0, env_dir)
from scenario_env import MultiAgentScenarioEnv
from metadrive.engine.asset_loader import AssetLoader
import numpy as np
class DummyPolicy:
"""
占位策略,用于数据检查时初始化环境
不需要实际执行动作,只是为了满足环境初始化要求
"""
def act(self, *args, **kwargs):
# 返回零动作 [throttle, steering]
return np.array([0.0, 0.0])
def check_available_fields():
"""
检查Waymo转MetaDrive数据中实际可用的字段
"""
WAYMO_DATA_DIR = r"/home/huangfukk/mdsn"
data_dir = AssetLoader.file_path(WAYMO_DATA_DIR, "exp_filtered", unix_style=False)
# 创建占位策略
dummy_policy = DummyPolicy()
# 初始化环境,传入必需的agent2policy参数
env = MultiAgentScenarioEnv(
config={
"data_directory": data_dir,
"is_multi_agent": True,
"num_controlled_agents": 3,
"use_render": False,
"sequential_seed": True,
},
agent2policy=dummy_policy # 添加这个必需参数
)
print("✓ 环境初始化成功")
# 重置环境以加载数据
print("正在加载场景数据...")
env.reset()
# 检查是否有expert_trajectories属性
if hasattr(env, 'expert_trajectories'):
print(f"✓ expert_trajectories属性存在,包含 {len(env.expert_trajectories)} 条轨迹")
else:
print("⚠️ expert_trajectories属性不存在,请先修改scenario_env.py添加轨迹存储功能")
# 获取一个track样本
sample_track = None
for scenario_id, track in env.engine.traffic_manager.current_traffic_data.items():
if track["type"] == "VEHICLE":
sample_track = track
print(f"\n找到样本车辆: scenario_id = {scenario_id}")
break
if sample_track is None:
print("未找到车辆轨迹数据")
env.close()
return
print("="*60)
print("Track数据结构分析")
print("="*60)
# 1. 顶层字段
print("\n1. Track顶层字段:")
for key in sample_track.keys():
print(f" - {key}: {type(sample_track[key])}")
# 2. metadata字段
print("\n2. track['metadata']字段:")
if "metadata" in sample_track:
for key, value in sample_track["metadata"].items():
if isinstance(value, (str, int, float, bool)):
print(f" - {key}: {type(value).__name__} = {value}")
else:
print(f" - {key}: {type(value).__name__}")
# 3. state字段
print("\n3. track['state']字段:")
if "state" in sample_track:
for key, value in sample_track["state"].items():
if isinstance(value, np.ndarray):
print(f" - {key}: shape={value.shape}, dtype={value.dtype}")
# 打印第一个有效值
if "valid" in sample_track["state"]:
valid_idx = np.argmax(sample_track["state"]["valid"])
if valid_idx >= 0 and valid_idx < len(value):
print(f" 示例值 (index {valid_idx}): {value[valid_idx]}")
else:
print(f" - {key}: {type(value)} = {value}")
print("\n" + "="*60)
print("建议存储的字段:")
print("="*60)
# 检查必需字段
required_fields = ["position", "heading", "velocity", "valid"]
print("\n必需字段:")
all_required_exist = True
for field in required_fields:
if "state" in sample_track and field in sample_track["state"]:
print(f" ✓ {field} (存在)")
else:
print(f" ✗ {field} (缺失)")
all_required_exist = False
# 检查可选字段
optional_fields = ["length", "width", "height", "bbox"]
print("\n可选字段:")
available_optional = []
for field in optional_fields:
if "state" in sample_track and field in sample_track["state"]:
print(f" + {field} (在state中)")
available_optional.append(field)
elif "metadata" in sample_track and field in sample_track["metadata"]:
print(f" + {field} (在metadata中)")
available_optional.append(field)
else:
print(f" - {field} (不存在)")
print("\n" + "="*60)
print("推荐的trajectory_data结构:")
print("="*60)
if all_required_exist:
print("""
trajectory_data = {
"object_id": object_id,
"scenario_id": scenario_id,
"valid_mask": valid[first_show:last_show+1].copy(),
"positions": track["state"]["position"][first_show:last_show+1].copy(),
"headings": track["state"]["heading"][first_show:last_show+1].copy(),
"velocities": track["state"]["velocity"][first_show:last_show+1].copy(),
"timesteps": np.arange(first_show, last_show+1),
"start_timestep": first_show,
"end_timestep": last_show,
"length": last_show - first_show + 1
}
""")
if available_optional:
print("如果需要车辆尺寸,可选添加:")
for field in available_optional:
if field in ["length", "width", "height"]:
print(f' trajectory_data["vehicle_{field}"] = track["state" or "metadata"]["{field}"][first_show]')
else:
print("⚠️ 缺少必需字段,请检查数据转换流程")
# 如果有expert_trajectories,展示一个样本
if hasattr(env, 'expert_trajectories') and len(env.expert_trajectories) > 0:
print("\n" + "="*60)
print("expert_trajectories样本:")
print("="*60)
sample_traj = list(env.expert_trajectories.values())[0]
for key, value in sample_traj.items():
if isinstance(value, np.ndarray):
print(f" {key}: shape={value.shape}, dtype={value.dtype}")
else:
print(f" {key}: {type(value).__name__} = {value}")
env.close()
print("\n✓ 分析完成")
if __name__ == "__main__":
check_available_fields()

162
scripts/generate_expert_data.py Normal file
View File

@@ -0,0 +1,162 @@
import argparse
import os
import sys
import pickle
import numpy as np
from tqdm import tqdm
# Add project root to Python path so we can import Env module
project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if project_root not in sys.path:
sys.path.insert(0, project_root)
from metadrive.engine.asset_loader import AssetLoader
from Env.expert_replay_env import ExpertReplayEnv
def generate_data(args):
data_path = os.path.abspath(args.data_dir)
if not os.path.exists(data_path):
raise ValueError(f"Data directory {data_path} not found")
# MetaDrive's ScenarioDataManager asserts if config["num_scenarios"] > available scenarios in data_directory.
# So we always set it to -1 (load all available) and clamp the loop range by reading dataset summary.
from metadrive.scenario.utils import read_dataset_summary
_, summary_lookup, _ = read_dataset_summary(data_path)
if args.start_index >= len(summary_lookup):
raise ValueError(
f"start_index={args.start_index} out of range. Dataset has {len(summary_lookup)} scenarios."
)
max_available = len(summary_lookup) - args.start_index
num_to_run = min(args.num_scenarios, max_available)
env_config = {
"data_directory": data_path,
"is_multi_agent": True,
"num_controlled_agents": 100, # Set high to catch all vehicles in scenario
"horizon": 1000,
"use_render": False,
"sequential_seed": True,
"reactive_traffic": False, # Important: we replay, not react
"start_scenario_index": args.start_index,
# Load all scenarios available in the directory to avoid assertion failure.
# We will still only iterate `num_to_run` scenarios below.
"num_scenarios": -1,
"log_level": 50 # ERROR to reduce noise
}
expert_trajectories = []
try:
# Loop through scenarios
for i in tqdm(range(args.start_index, args.start_index + num_to_run), desc="Scenarios"):
env = ExpertReplayEnv(config=env_config)
try:
obs_dict = env.reset(seed=i)
except Exception as e:
print(f"Error resetting scenario {i}: {e}")
try:
env.close()
except Exception:
pass
continue
# Storage for current episode
# dict of lists: {agent_id: {'obs': [], 'acts': []}}
episode_data = {}
# Map agent_id to original ID if possible, but agent_id is unique enough
for step in range(env.config["horizon"]):
# Step with dummy actions
obs, rewards, dones, infos = env.step(None)
# 'obs' is next observation (t+1)
# 'infos' contains 'expert_action' which took (t -> t+1)
# Wait, usually (obs_t, act_t) -> obs_{t+1}
# expert_replay_env.step():
# calc action (t -> t+1)
# move agents to t+1
# return obs_{t+1}
# So we have obs_dict (from reset or prev step) which is at 't'
# And we have 'infos' which has action at 't'.
current_agents = list(obs_dict.keys())
for agent_id in current_agents:
if agent_id not in episode_data:
episode_data[agent_id] = {'obs': [], 'acts': []}
# Check if we have action for this agent
if agent_id in infos and 'expert_action' in infos[agent_id]:
action = infos[agent_id]['expert_action']
observation = obs_dict[agent_id]
episode_data[agent_id]['obs'].append(observation)
episode_data[agent_id]['acts'].append(action)
# Update obs_dict for next step
obs_dict = obs
if dones["__all__"]:
break
# Post-process episode data
for agent_id, data in episode_data.items():
if len(data['obs']) > 10: # Minimum length filter
expert_trajectories.append({
'obs': np.array(data['obs']),
'acts': np.array(data['acts']),
'agent_id': agent_id,
'scenario_id': i
})
env.close()
except Exception as e:
import traceback
traceback.print_exc()
print(f"Global error: {e}")
finally:
# env is closed per-scenario above (more robust for MetaDrive object lifecycle)
pass
# Save data
output_file = os.path.join(args.output_dir, f"expert_data_{args.start_index}_{args.num_scenarios}.pkl")
os.makedirs(args.output_dir, exist_ok=True)
print(f"Saving {len(expert_trajectories)} trajectories to {output_file}")
with open(output_file, 'wb') as f:
pickle.dump(expert_trajectories, f)
# Verification stats
if len(expert_trajectories) > 0:
all_acts = np.concatenate([t['acts'] for t in expert_trajectories])
print("Action Stats:")
print(f" Steering: min={all_acts[:,0].min():.3f}, max={all_acts[:,0].max():.3f}, mean={all_acts[:,0].mean():.3f}")
print(f" Accel: min={all_acts[:,1].min():.3f}, max={all_acts[:,1].max():.3f}, mean={all_acts[:,1].mean():.3f}")
# Clipping ratio diagnostics (actions are normalized to [-1, 1])
# If this ratio is high, it usually indicates max_acc/max_steering too small or noisy finite-difference.
eps = 1e-6
steer = all_acts[:, 0]
accel = all_acts[:, 1]
steer_clipped = np.isclose(np.abs(steer), 1.0, atol=eps)
accel_clipped = np.isclose(np.abs(accel), 1.0, atol=eps)
print("Clipping Stats:")
print(
f" Steering clipped (|a|==1): {steer_clipped.mean()*100:.2f}% "
f"({steer_clipped.sum()}/{len(steer_clipped)})"
)
print(
f" Accel clipped (|a|==1): {accel_clipped.mean()*100:.2f}% "
f"({accel_clipped.sum()}/{len(accel_clipped)})"
)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--data_dir", type=str, default="/home/huangfukk/MAGAIL4AutoDrive/data/exp_filtered", help="Path to Waymo pickles (or filtered index)")
parser.add_argument("--output_dir", type=str, default="/home/huangfukk/MAGAIL4AutoDrive/data/training", help="Output directory")
parser.add_argument("--start_index", type=int, default=0)
parser.add_argument("--num_scenarios", type=int, default=10)
args = parser.parse_args()
generate_data(args)

105
scripts/visualize_expert_trajectory.py Normal file
View File

@@ -0,0 +1,105 @@
import sys
import os
# 添加路径
current_dir = os.path.dirname(os.path.abspath(__file__))
project_root = os.path.dirname(current_dir)
env_dir = os.path.join(project_root, "Env")
sys.path.insert(0, project_root)
sys.path.insert(0, env_dir)
# 现在可以导入了
from scenario_env import MultiAgentScenarioEnv
from metadrive.engine.asset_loader import AssetLoader
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
class DummyPolicy:
"""
占位策略,用于数据检查时初始化环境
不需要实际执行动作,只是为了满足环境初始化要求
"""
def act(self, *args, **kwargs):
# 返回零动作 [throttle, steering]
return np.array([0.0, 0.0])
def visualize_expert_trajectory(env, scenario_idx=0):
"""
可视化专家轨迹的俯视图动画
"""
env.reset()
expert_trajs = env.expert_trajectories
if len(expert_trajs) == 0:
print("当前场景无专家轨迹")
return
# 设置绘图
fig, ax = plt.subplots(figsize=(12, 12))
# 获取所有轨迹的最大时间长度
max_timestep = max(traj["end_timestep"] for traj in expert_trajs.values())
min_timestep = min(traj["start_timestep"] for traj in expert_trajs.values())
# 绘制完整轨迹(淡色)
colors = plt.cm.tab10(np.linspace(0, 1, len(expert_trajs)))
for idx, (obj_id, traj) in enumerate(expert_trajs.items()):
positions = traj["positions"][:, :2]
ax.plot(positions[:, 0], positions[:, 1],
color=colors[idx], alpha=0.3, linewidth=1,
label=f'Vehicle {obj_id[:6]}')
# 初始化当前位置标记
scatter = ax.scatter([], [], s=200, c='red', marker='o', edgecolors='black', linewidths=2)
time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes, fontsize=14)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_title(f'Expert Trajectory Visualization - Scenario {scenario_idx}')
ax.legend(loc='upper right', fontsize=8)
ax.grid(True, alpha=0.3)
ax.axis('equal')
def update(frame):
current_time = min_timestep + frame
# 收集当前时间所有车辆的位置
current_positions = []
for traj in expert_trajs.values():
if traj["start_timestep"] <= current_time <= traj["end_timestep"]:
idx = current_time - traj["start_timestep"]
pos = traj["positions"][idx, :2]
current_positions.append(pos)
if len(current_positions) > 0:
current_positions = np.array(current_positions)
scatter.set_offsets(current_positions)
time_text.set_text(f'Time: {frame * 0.1:.1f}s (Frame {frame})')
return scatter, time_text
anim = FuncAnimation(fig, update, frames=max_timestep-min_timestep+1,
interval=100, blit=True, repeat=True)
plt.tight_layout()
plt.show()
return anim
if __name__ == "__main__":
WAYMO_DATA_DIR = r"/home/huangfukk/mdsn"
data_dir = AssetLoader.file_path(WAYMO_DATA_DIR, "exp_filtered", unix_style=False)
env = MultiAgentScenarioEnv(
config={
"data_directory": data_dir,
"is_multi_agent": True,
"num_controlled_agents": 3,
"use_render": False,
},
agent2policy=DummyPolicy()
)
# 可视化第一个场景
anim = visualize_expert_trajectory(env, scenario_idx=0)

93
scripts/visualize_replay.py Normal file
View File

@@ -0,0 +1,93 @@
import argparse
import os
import sys
import time
# Add project root to Python path so we can import Env module
project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if project_root not in sys.path:
sys.path.insert(0, project_root)
from Env.expert_replay_env import ExpertReplayEnv
def visualize_replay(args):
data_path = os.path.abspath(args.data_dir)
if not os.path.exists(data_path):
raise ValueError(f"Data directory {data_path} not found")
# Same as data generation: avoid MetaDrive assertion when requested num_scenarios > available.
from metadrive.scenario.utils import read_dataset_summary
_, summary_lookup, _ = read_dataset_summary(data_path)
if args.start_index >= len(summary_lookup):
raise ValueError(
f"start_index={args.start_index} out of range. Dataset has {len(summary_lookup)} scenarios."
)
max_available = len(summary_lookup) - args.start_index
num_to_run = min(args.num_scenarios, max_available)
env_config = {
"data_directory": data_path,
"is_multi_agent": True,
"num_controlled_agents": 100,
"horizon": args.horizon,
"use_render": True, # Enable rendering
"sequential_seed": True,
"reactive_traffic": False,
"start_scenario_index": args.start_index,
"num_scenarios": -1,
"log_level": 40, # ERROR
# "pstats": True, # For performance debugging
}
print(f"Initializing ExpertReplayEnv with data from {data_path}...")
env = ExpertReplayEnv(config=env_config)
try:
for i in range(args.start_index, args.start_index + num_to_run):
print(f"\n--- Playing Scenario {i} ---")
try:
obs = env.reset(seed=i)
except Exception as e:
print(f"Error resetting scenario {i}: {e}")
continue
print(f"Scenario loaded. Controlled agents: {len(env.controlled_agents)}")
for step in range(args.horizon):
# Step
obs, rewards, dones, infos = env.step(None)
# Render
env.render(mode="top_down",
text={
"Step": step,
"Agents": len(env.controlled_agents),
"Scenario": i
})
# Sleep to control playback speed
time.sleep(0.05)
if dones["__all__"]:
print(f"Scenario {i} finished at step {step}")
break
except KeyboardInterrupt:
print("Interrupted by user")
except Exception as e:
import traceback
traceback.print_exc()
print(f"Global error: {e}")
finally:
env.close()
print("Environment closed.")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--data_dir", type=str, default="/home/huangfukk/MAGAIL4AutoDrive/data/exp_filtered", help="Path to Waymo data")
parser.add_argument("--start_index", type=int, default=0)
parser.add_argument("--num_scenarios", type=int, default=1)
parser.add_argument("--horizon", type=int, default=500)
args = parser.parse_args()
visualize_replay(args)

521
train_magail.py Normal file
View File

@@ -0,0 +1,521 @@
import torch
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from torch.distributions import Normal
import numpy as np
import os
import argparse
import signal
import sys
from torch.utils.data import DataLoader
from dataset.magail_dataset import MAGAILExpertDataset
# --- Networks ---
class Actor(nn.Module):
def __init__(self, state_dim, action_dim, hidden_dim=256):
super(Actor, self).__init__()
self.net = nn.Sequential(
nn.Linear(state_dim, hidden_dim),
nn.Tanh(),
nn.Linear(hidden_dim, hidden_dim),
nn.Tanh(),
)
self.mu_head = nn.Linear(hidden_dim, action_dim)
self.log_std_head = nn.Parameter(torch.zeros(1, action_dim))
def forward(self, state):
x = self.net(state)
mu = torch.tanh(self.mu_head(x)) # Action range [-1, 1]
if mu.dim() == 1:
mu = mu.unsqueeze(0) # Handle single sample
log_std = self.log_std_head.expand_as(mu)
std = torch.exp(log_std)
dist = Normal(mu, std)
return dist
class Critic(nn.Module):
def __init__(self, state_dim, hidden_dim=256):
super(Critic, self).__init__()
self.net = nn.Sequential(
nn.Linear(state_dim, hidden_dim),
nn.Tanh(),
nn.Linear(hidden_dim, hidden_dim),
nn.Tanh(),
nn.Linear(hidden_dim, 1)
)
def forward(self, state):
return self.net(state)
class Discriminator(nn.Module):
def __init__(self, state_dim, action_dim, hidden_dim=256):
super(Discriminator, self).__init__()
self.net = nn.Sequential(
nn.Linear(state_dim + action_dim, hidden_dim),
nn.Tanh(),
nn.Linear(hidden_dim, hidden_dim),
nn.Tanh(),
nn.Linear(hidden_dim, 1),
nn.Sigmoid()
)
def forward(self, state, action):
x = torch.cat([state, action], dim=-1)
return self.net(x)
# --- PPO Algorithm ---
class PPO:
def __init__(self, state_dim, action_dim, lr=3e-4, gamma=0.99, eps_clip=0.2, K_epochs=10):
self.actor = Actor(state_dim, action_dim).cuda()
self.critic = Critic(state_dim).cuda()
self.optimizer_actor = optim.Adam(self.actor.parameters(), lr=lr)
self.optimizer_critic = optim.Adam(self.critic.parameters(), lr=lr)
self.gamma = gamma
self.eps_clip = eps_clip
self.K_epochs = K_epochs
self.mse_loss = nn.MSELoss()
def select_action(self, state):
with torch.no_grad():
state = torch.FloatTensor(state).cuda()
dist = self.actor(state)
action = dist.sample()
action_logprob = dist.log_prob(action).sum(dim=-1)
return action.cpu().numpy(), action_logprob.cpu().numpy()
def update(self, memory):
# Convert memory to tensors
states = torch.FloatTensor(np.array(memory['states'])).cuda()
actions = torch.FloatTensor(np.array(memory['actions'])).cuda()
logprobs = torch.FloatTensor(np.array(memory['logprobs'])).cuda()
rewards = torch.FloatTensor(np.array(memory['rewards'])).cuda()
next_states = torch.FloatTensor(np.array(memory['next_states'])).cuda()
dones = torch.FloatTensor(np.array(memory['dones'])).cuda()
# Monte Carlo estimate of state rewards (or GAE if implemented, simplistic here)
# Usually for PPO we use GAE. Let's do a simple discounted return for now or bootstrapping.
# Let's use bootstrapping from critic for returns.
returns = []
discounted_reward = 0
# This simple loop assumes full episode or consistent batch.
# For multi-agent disjoint steps, bootstrapping is better.
# But let's calculate advantage using GAE for stability.
values = self.critic(states).detach()
next_values = self.critic(next_states).detach()
# GAE
advantages = []
gae = 0
for i in reversed(range(len(rewards))):
delta = rewards[i] + self.gamma * next_values[i] * (1 - dones[i]) - values[i]
gae = delta + self.gamma * 0.95 * (1 - dones[i]) * gae
advantages.insert(0, gae)
advantages = torch.FloatTensor(advantages).cuda()
returns = advantages + values.squeeze()
# Optimize policy for K epochs:
for _ in range(self.K_epochs):
# Evaluating old actions and values :
dist = self.actor(states)
action_logprobs = dist.log_prob(actions).sum(dim=-1)
dist_entropy = dist.entropy().sum(dim=-1)
state_values = self.critic(states).squeeze()
# Finding the ratio (pi_theta / pi_theta__old):
ratios = torch.exp(action_logprobs - logprobs)
# Finding Surrogate Loss:
surr1 = ratios * advantages
surr2 = torch.clamp(ratios, 1-self.eps_clip, 1+self.eps_clip) * advantages
loss = -torch.min(surr1, surr2) + 0.5*self.mse_loss(state_values, returns) - 0.01*dist_entropy
# take gradient step
self.optimizer_actor.zero_grad()
self.optimizer_critic.zero_grad()
loss.mean().backward()
self.optimizer_actor.step()
self.optimizer_critic.step()
return loss.mean().item()
def save(self, checkpoint_path):
torch.save(self.actor.state_dict(), checkpoint_path + "_actor.pth")
torch.save(self.critic.state_dict(), checkpoint_path + "_critic.pth")
# --- Training Loop ---
def train(args):
# 1. Setup Environment (Dummy for now, usually you run simulation here)
# But for MAGAIL we need to collect generated trajectories.
# We need the Env class to be importable.
from Env.scenario_env import MultiAgentScenarioEnv
from Env.simple_idm_policy import ConstantVelocityPolicy # Just for init
# Config for Env
env_config = {
"data_directory": args.data_dir,
"is_multi_agent": True,
"num_controlled_agents": 3, # Dynamic
"horizon": 200,
"use_render": False,
"sequential_seed": True,
"start_scenario_index": 0,
"num_scenarios": args.num_scenarios # Use argument
}
# Ideally we use a wrapper for RL
# env = MultiAgentScenarioEnv(config=env_config) # This requires Waymo data loader setup
# 2. Setup Models
state_dim = 45
action_dim = 2
ppo_agent = PPO(state_dim, action_dim)
discriminator = Discriminator(state_dim, action_dim).cuda()
disc_optimizer = optim.Adam(discriminator.parameters(), lr=3e-4)
disc_criterion = nn.BCELoss()
# 3. Load Expert Data
expert_dataset = MAGAILExpertDataset(args.expert_data_dir)
# Ensure batch_size is not larger than dataset
if len(expert_dataset) < args.batch_size:
print(f"Warning: Expert dataset size {len(expert_dataset)} < batch_size {args.batch_size}. Adjusting batch_size.")
args.batch_size = len(expert_dataset)
if args.batch_size == 0:
raise ValueError("Expert dataset is empty!")
expert_loader = DataLoader(expert_dataset, batch_size=args.batch_size, shuffle=True, drop_last=True)
# Create an infinite iterator
def cycle(loader):
while True:
for batch in loader:
yield batch
expert_iter = cycle(expert_loader)
# 4. Initialize Env
from Env.expert_replay_env import ExpertReplayEnv # Using ReplayEnv for config, but we need ScenarioEnv for simulation?
# Actually we need MultiAgentScenarioEnv for interactive training, not Replay.
from Env.scenario_env import MultiAgentScenarioEnv
from Env.simple_idm_policy import ConstantVelocityPolicy # Placeholder policy for init
# 2. Setup Models
# Determine state dim from environment if possible, or use fixed
# Expert data has 45 dim?
# But Env might return something else if we are using default ScenarioEnv settings.
# ScenarioEnv returns list of obs.
# The error says: "mat1 and mat2 shapes cannot be multiplied (1x108 and 45x256)"
# This means the Env is returning 108-dim observation (MetaDrive default + Lidar),
# but our Actor expects 45 (which is what we saved in expert data).
# We must align the environment observation space with our expert data format.
# Our ExpertReplayEnv used a custom _get_all_obs.
# We need to inject that same logic into the training env, OR
# subclass MultiAgentScenarioEnv in the training script to override observation.
class MAGAILScenarioEnv(MultiAgentScenarioEnv):
def _get_all_obs(self):
# Same logic as ExpertReplayEnv to ensure compatibility
obs_dict = {}
for agent_id, vehicle in self.controlled_agents.items():
# 1. Ego State
ego_state = [
vehicle.position[0], vehicle.position[1],
vehicle.velocity[0], vehicle.velocity[1],
vehicle.heading_theta
]
# 2. Neighbors
candidates = []
for other_id, other_vehicle in self.engine.agent_manager.active_agents.items():
if other_id == agent_id:
continue
dist = np.linalg.norm(vehicle.position - other_vehicle.position)
if dist < 30.0:
candidates.append((dist, other_vehicle))
candidates.sort(key=lambda x: x[0])
top_10 = candidates[:10]
neighbor_feats = []
for _, neighbor in top_10:
neighbor_feats.extend([
neighbor.position[0] - vehicle.position[0],
neighbor.position[1] - vehicle.position[1],
neighbor.velocity[0],
neighbor.velocity[1]
])
missing = 10 - len(top_10)
if missing > 0:
neighbor_feats.extend([0.0] * (4 * missing))
obs = np.array(ego_state + neighbor_feats, dtype=np.float32)
obs_dict[agent_id] = obs
return obs_dict
env = MAGAILScenarioEnv(config=env_config, agent2policy={}) # Pass empty dict if we control all externally
print("Starting training...")
# Tensorboard
try:
from torch.utils.tensorboard import SummaryWriter
writer = SummaryWriter(log_dir=args.log_dir)
except ImportError:
print("TensorBoard not installed. Logging to console only.")
writer = None
global_step = 0
for i_episode in range(args.max_episodes):
# --- 1. Collect Rollouts (Interaction) ---
memory = {'states': [], 'actions': [], 'logprobs': [], 'rewards': [], 'next_states': [], 'dones': []}
# Prepare seed
available_scenarios = env.config["num_scenarios"]
start_index = env.config["start_scenario_index"]
seed = np.random.randint(start_index, start_index + available_scenarios)
# Reset Env
try:
# MetaDrive sometimes complains about uncleared objects if reset happens too fast or with lingering objs
# We can try to force clear before reset or handle exception
# But standard reset should handle it.
# The error "You should clear all generated objects..." means some manager didn't clear its objects.
# This is likely due to TrafficManager or AgentManager holding refs.
# Re-creating env is safer but slower.
# Let's try closing and re-creating if reset fails frequently.
# Or just ignore this error and try reset again? No, reset failing is fatal usually.
# Hack: Manually clear objects if we can access engine
if env.engine is not None:
env.engine.clear_objects(list(env.engine.get_objects().keys()))
obs_dict = env.reset(seed=seed)
except Exception as e:
# print(f"Env reset failed: {e}. Recreating environment...")
try:
env.close()
except:
pass
# Ensure engine is closed properly
from metadrive.engine.engine_utils import close_engine
try:
close_engine()
except Exception as e2:
# Force cleanup of singleton if close failed
from metadrive.engine.base_engine import BaseEngine
if BaseEngine.singleton is not None:
BaseEngine.singleton = None
# Also need to clear ShowBase
try:
from direct.showbase.ShowBase import ShowBase
if hasattr(base, 'destroy'):
base.destroy()
except:
pass
# Brutal force: delete base from builtins if it exists
import builtins
if hasattr(builtins, 'base'):
del builtins.base
# print(f"Error closing engine: {e2}")
# Explicitly delete old env object to free memory
del env
import gc
gc.collect()
env = MAGAILScenarioEnv(config=env_config, agent2policy={})
obs_dict = env.reset(seed=seed)
episode_reward = 0
steps = 0
# Rollout loop
while True:
# Select actions for all agents
actions = {}
action_logprobs = {}
# obs_dict: {agent_id: obs}
# MultiAgentScenarioEnv usually returns a dict {agent_id: obs}
# BUT wait, check scenario_env.py implementation
if isinstance(obs_dict, list):
# This happens if the environment returns a list instead of a dict
# MultiAgentScenarioEnv._get_all_obs returns a list in original implementation?
# Let's check scenario_env.py
# If it returns list, we need to map it to agent ids or just iterate
pass
# Temporary fix if it returns list (which means my previous edit to Env/expert_replay_env.py
# changed it there, but maybe not in Env/scenario_env.py which we are using here!)
if isinstance(obs_dict, list):
# We need agent IDs to step
# In MultiAgentScenarioEnv, controlled_agents is a dict.
# If obs is a list, it probably corresponds to controlled_agents.values() order?
# This is risky.
# Let's assume obs_dict is actually just observations.
# We need to keys to create action dict.
current_agent_ids = list(env.controlled_agents.keys())
# Ensure length matches
if len(obs_dict) != len(current_agent_ids):
# print(f"Warning: Obs list len {len(obs_dict)} != agents {len(current_agent_ids)}")
pass
# Reconstruct dict
new_obs_dict = {}
for i, agent_id in enumerate(current_agent_ids):
if i < len(obs_dict):
new_obs_dict[agent_id] = obs_dict[i]
obs_dict = new_obs_dict
for agent_id, obs in obs_dict.items():
act, logprob = ppo_agent.select_action(obs) # Select action returns numpy
actions[agent_id] = act.flatten() # (2,)
action_logprobs[agent_id] = logprob # scalar
# Step Env
next_obs_dict, rewards, dones, infos = env.step(actions)
# Store in memory
for agent_id, obs in obs_dict.items():
if agent_id in actions:
memory['states'].append(obs)
memory['actions'].append(actions[agent_id])
memory['logprobs'].append(action_logprobs[agent_id])
# Store standard environmental reward for logging (not used for update in GAIL)
# For GAIL update we use Discriminator reward later
memory['rewards'].append(0) # Placeholder
# Next state
if agent_id in next_obs_dict:
memory['next_states'].append(next_obs_dict[agent_id])
memory['dones'].append(False)
else:
# Agent finished/vanished
# We need a dummy next state or handle done correctly
# Just duplicate current state and mark done?
memory['next_states'].append(obs)
memory['dones'].append(True)
obs_dict = next_obs_dict
steps += 1
if dones["__all__"] or steps >= 200: # Limit horizon
break
# Initialize losses to 0/None before potential loop skip
disc_loss = torch.tensor(0.0)
ppo_loss = 0.0
all_gail_rewards = [0.0]
# --- 2. Train Discriminator ---
# Convert policy memory to tensors
policy_states = torch.FloatTensor(np.array(memory['states'])).cuda()
policy_actions = torch.FloatTensor(np.array(memory['actions'])).cuda()
# Sample expert batch
expert_batch = next(expert_iter)
expert_states = expert_batch['state'].cuda()
expert_actions = expert_batch['action'].cuda()
# Minibatch size matching
batch_size = min(policy_states.size(0), expert_states.size(0))
if batch_size > 0: # Only train if we have data
policy_states = policy_states[:batch_size]
policy_actions = policy_actions[:batch_size]
expert_states = expert_states[:batch_size]
expert_actions = expert_actions[:batch_size]
# Update Discriminator
# Label 1 for Expert, 0 for Policy
# Train Expert
disc_optimizer.zero_grad()
exp_preds = discriminator(expert_states, expert_actions)
exp_loss = disc_criterion(exp_preds, torch.ones_like(exp_preds))
pol_preds = discriminator(policy_states.detach(), policy_actions.detach()) # Detach policy data
pol_loss = disc_criterion(pol_preds, torch.zeros_like(pol_preds))
disc_loss = exp_loss + pol_loss
disc_loss.backward()
disc_optimizer.step()
# --- 3. Update Policy with GAIL Rewards ---
# Reward = -log(1 - D(s, a))
# Or more stable: log(D(s, a)) ? Original GAIL uses -log(1-D) which is log(D) roughly.
# Let's use -log(1 - D(s, a) + eps)
# Actually PPO needs the full trajectory for GAE.
# So we should compute rewards for ALL policy samples in memory.
all_policy_states = torch.FloatTensor(np.array(memory['states'])).cuda()
all_policy_actions = torch.FloatTensor(np.array(memory['actions'])).cuda()
with torch.no_grad():
all_d_val = discriminator(all_policy_states, all_policy_actions)
all_gail_rewards = -torch.log(1 - all_d_val + 1e-8).cpu().numpy().flatten()
# Replace placeholders
memory['rewards'] = all_gail_rewards.tolist()
# Update PPO
ppo_loss = ppo_agent.update(memory)
# Clean up memory
del policy_states, policy_actions, expert_states, expert_actions, exp_preds, exp_loss, pol_preds, pol_loss
del all_policy_states, all_policy_actions, all_d_val
torch.cuda.empty_cache()
else:
print(f"Episode {i_episode}: No data collected (Env might have crashed or no agents). Skipping update.")
# --- 4. Logging ---
if writer:
writer.add_scalar('Loss/Discriminator', disc_loss.item(), i_episode)
writer.add_scalar('Loss/Policy', ppo_loss, i_episode)
writer.add_scalar('Reward/Mean_GAIL', np.mean(all_gail_rewards), i_episode)
print(f"Episode {i_episode}: Disc Loss {disc_loss.item():.4f} | PPO Loss {ppo_loss:.4f} | Mean Reward {np.mean(all_gail_rewards):.4f}")
if i_episode % 50 == 0:
ppo_agent.save(os.path.join(args.log_dir, f"model_{i_episode}"))
env.close()
if writer:
writer.close()
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--expert_data_dir", type=str, default="data/training_data", help="Directory with .pkl expert data")
parser.add_argument("--data_dir", type=str, default="data/exp_filtered", help="Waymo data dir for Env")
parser.add_argument("--batch_size", type=int, default=1024)
parser.add_argument("--max_episodes", type=int, default=1000)
parser.add_argument("--num_scenarios", type=int, default=100)
parser.add_argument("--log_dir", type=str, default="runs/magail_exp")
args = parser.parse_args()
# Create log dir
os.makedirs(args.log_dir, exist_ok=True)
train(args)