# ads

**Repository Path**: eowyn0406/ads

## Basic Information

- **Project Name**: ads
- **Description**: ads测试代码
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 1
- **Created**: 2025-11-13
- **Last Updated**: 2025-12-16

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# CARLA Map - 自动驾驶地图与控制功能包

[![ROS2](https://img.shields.io/badge/ROS2-Humble-blue.svg)](https://docs.ros.org/en/humble/)
[![License](https://img.shields.io/badge/License-MIT-green.svg)](LICENSE)

基于 ROS2 的 CARLA 仿真器地图可视化与自动驾驶控制功能包。该功能包实现了 Lanelet2 地图的加载与可视化、基于道路拓扑的路径规划、Stanley 横向控制、交通灯管理等完整的自动驾驶功能。

## 📋 目录

- [功能特性](#功能特性)
- [系统要求](#系统要求)
- [安装步骤](#安装步骤)
- [使用方法](#使用方法)
- [功能模块说明](#功能模块说明)
- [话题与消息](#话题与消息)
- [配置说明](#配置说明)
- [常见问题](#常见问题)

## ✨ 功能特性

### 🗺️ 地图可视化
- **Lanelet2 地图加载**: 支持 OSM 格式的 Lanelet2 高精地图
- **实时可视化**: 在 RViz2 中实时显示道路边界、中心线
- **坐标转换**: 自动处理地图坐标系到 CARLA 世界坐标系的转换

### 🚗 自动驾驶控制
- **智能路径规划**: 基于道路拓扑的 A* 路径规划算法
- **自动对齐功能**: 起点和终点自动对齐到最近的道路中心线（误差 >10cm 时触发）
- **Stanley 控制器**: 实现精确的横向控制
- **纵向速度控制**: PID 速度跟踪控制
- **交通灯识别**: 自动识别前方交通灯状态并做出响应

### 🚦 交通灯管理
- **从地图提取**: 自动从 Lanelet2 地图中提取交通灯位置
- **状态发布**: 发布交通灯状态信息（红、黄、绿）
- **可视化显示**: 在 RViz2 中显示交通灯位置和状态

### 📊 可视化功能
- **车辆模型**: 3D 车辆模型显示
- **规划路径**: 显示完整的规划路径
- **当前目标点**: 显示当前跟踪的路径点
- **速度曲线**: 可视化速度规划曲线
- **交通灯标记**: 实时显示交通灯状态

## 🔧 系统要求

### 操作系统
- Ubuntu 22.04 LTS

### ROS2 版本
- ROS2 Humble Hawksbill

### 依赖项

#### ROS2 包依赖
```bash
# 核心依赖
rclpy
geometry_msgs
visualization_msgs
nav_msgs
std_msgs
tf2_ros
tf2_geometry_msgs
rviz2

# Lanelet2 支持
python3-lanelet2
```

#### Python 依赖
```bash
numpy
```

## 📦 安装步骤

### 1. 创建工作空间（如果还没有）
```bash
mkdir -p ~/carla_ws/src
cd ~/carla_ws/src
```

### 2. 克隆或复制功能包
```bash
# 将 carla_map 功能包放入 src 目录
cp -r /path/to/carla_map ~/carla_ws/src/
```

### 3. 安装依赖
```bash
cd ~/carla_ws
rosdep install --from-paths src --ignore-src -r -y
```

### 4. 安装 Lanelet2（如果未安装）
```bash
sudo apt update
sudo apt install ros-humble-lanelet2 python3-lanelet2
```

### 5. 编译功能包
```bash
cd ~/carla_ws
colcon build --packages-select carla_map
```

### 6. 配置环境变量
```bash
source install/setup.bash
# 建议添加到 ~/.bashrc
echo "source ~/carla_ws/install/setup.bash" >> ~/.bashrc
```

## 🚀 使用方法

### 快速启动

#### 方式一：一键启动（推荐）
启动完整的自动驾驶系统（包括地图、控制、交通灯和 RViz）：

```bash
ros2 launch carla_map carla_map_control.launch.py
```

#### 方式二：分别启动节点
如果需要单独启动某个节点进行调试：

```bash
# 1. 启动 TF 发布器
ros2 run carla_map tf_publisher

# 2. 启动地图发布器
ros2 run carla_map osm_map_publisher_lanelet2

# 3. 启动交通灯管理器
ros2 run carla_map traffic_light_manager

# 4. 启动控制节点
ros2 run carla_map control

# 5. 启动 RViz（可选）
rviz2 -d install/carla_map/share/carla_map/config/carla_map_ego.rviz
```

### 基本操作流程

1. **启动系统**
   ```bash
   ros2 launch carla_map carla_map_control.launch.py
   ```

2. **在 RViz2 中设置起点**
   - 点击工具栏的 `2D Pose Estimate` 按钮
   - 在地图上点击并拖动设置起点位置和朝向
   - 系统会自动将起点对齐到最近的道路中心线

3. **设置目标点**
   - 点击工具栏的 `2D Nav Goal` 按钮
   - 在地图上点击并拖动设置目标位置和朝向
   - 系统会自动将目标点对齐到最近的道路中心线
   - 车辆将自动规划路径并开始行驶

4. **观察车辆行驶**
   - 绿色路径线：规划的行驶路径
   - 蓝色车辆模型：当前车辆位置
   - 红色球体：当前跟踪的目标路径点
   - 彩色标记：交通灯位置和状态

### Launch 文件参数

```bash
# 启动时不显示 RViz
ros2 launch carla_map carla_map_control.launch.py rviz:=false

# 使用自定义 RViz 配置文件
ros2 launch carla_map carla_map_control.launch.py rviz_config:=/path/to/custom.rviz
```

## 📚 功能模块说明

### 1. osm_map_publisher_lanelet2
**功能**: Lanelet2 地图发布节点

**主要任务**:
- 加载 OSM 格式的 Lanelet2 地图文件
- 将地图坐标转换到 CARLA 世界坐标系
- 发布道路边界线的可视化标记

**发布话题**:
- `/my_map` (visualization_msgs/MarkerArray): 地图可视化标记

### 2. control
**功能**: 自动驾驶车辆控制器

**主要任务**:
- 接收起点和终点位置
- 自动将起点/终点对齐到道路中心线（误差 >10cm）
- 基于道路拓扑使用 A* 算法规划路径
- Stanley 横向控制器跟踪路径
- PID 纵向速度控制
- 交通灯识别与响应
- 发布车辆控制指令和可视化信息

**订阅话题**:
- `/initialpose` (geometry_msgs/PoseWithCovarianceStamped): 初始位置
- `/goal_pose` (geometry_msgs/PoseStamped): 目标位置
- `/traffic_lights` (carla_map/TrafficLightArray): 交通灯状态

**发布话题**:
- `/vehicle_control` (carla_map/VehicleControl): 车辆控制指令
- `/planned_path` (nav_msgs/Path): 规划的路径
- `/vehicle_marker` (visualization_msgs/Marker): 车辆可视化
- `/current_waypoint` (visualization_msgs/Marker): 当前目标点
- `/control_status` (std_msgs/String): 控制状态信息

**关键特性**:
- **自动对齐**: 如果起点/终点距离最近道路中心线 >10cm，自动调整
- **密集采样**: 道路中心线每 10cm 一个采样点，确保精确定位
- **智能规划**: 基于道路拓扑关系搜索最优路径
- **平滑控制**: Stanley 算法确保平滑的横向控制

### 3. traffic_light_manager
**功能**: 交通灯管理器

**主要任务**:
- 从 Lanelet2 地图中提取交通灯信息
- 模拟交通灯状态变化（可接入真实传感器）
- 发布交通灯状态和可视化信息

**发布话题**:
- `/traffic_lights` (carla_map/TrafficLightArray): 交通灯状态数组
- `/traffic_light_markers` (visualization_msgs/MarkerArray): 交通灯可视化

### 4. tf_publisher
**功能**: TF 坐标变换发布器

**主要任务**:
- 发布 `map` -> `odom` 静态坐标变换
- 确保 RViz2 能正确显示地图坐标系

## 📡 话题与消息

### 自定义消息类型

#### VehicleControl.msg
车辆控制指令消息

```
float64 steering      # 转向角 (rad)，范围 [-1.0, 1.0]
float64 throttle      # 油门，范围 [0.0, 1.0]
float64 brake         # 刹车，范围 [0.0, 1.0]
float64 target_speed  # 目标速度 (m/s)
```

#### TrafficLight.msg
单个交通灯状态消息

```
int32 id                              # 交通灯唯一ID
uint8 state                           # 状态: 0=红, 1=黄, 2=绿
geometry_msgs/Point position          # 交通灯位置
int32[] associated_lanelets           # 关联的车道ID
builtin_interfaces/Time timestamp     # 时间戳
```

#### TrafficLightArray.msg
交通灯数组消息

```
TrafficLight[] traffic_lights         # 交通灯数组
builtin_interfaces/Time stamp         # 发布时间戳
```

#### SpeedProfile.msg
速度规划曲线消息

```
std_msgs/Header header
int32[] waypoint_indices              # 路径点索引
float64[] speeds                      # 对应速度 (m/s)
float64 total_distance                # 总路径长度 (m)
```

### 话题列表

| 话题名称 | 消息类型 | 描述 |
|---------|---------|------|
| `/my_map` | visualization_msgs/MarkerArray | 地图可视化 |
| `/initialpose` | geometry_msgs/PoseWithCovarianceStamped | 初始位置 |
| `/goal_pose` | geometry_msgs/PoseStamped | 目标位置 |
| `/vehicle_control` | carla_map/VehicleControl | 车辆控制指令 |
| `/planned_path` | nav_msgs/Path | 规划路径 |
| `/vehicle_marker` | visualization_msgs/Marker | 车辆可视化 |
| `/current_waypoint` | visualization_msgs/Marker | 当前目标点 |
| `/traffic_lights` | carla_map/TrafficLightArray | 交通灯状态 |
| `/traffic_light_markers` | visualization_msgs/MarkerArray | 交通灯可视化 |
| `/control_status` | std_msgs/String | 控制状态 |

## ⚙️ 配置说明

### 地图文件配置

地图文件路径需要在以下文件中配置：

1. **osm_map_publisher_lanelet2.py** (第 37 行)
   ```python
   self.lanelet_map = lanelet2.io.load("/home/eowyn-lenovo/code/copy/map/new_lanelet2_maps_fixed.osm", projector)
   ```

2. **traffic_light_manager.py** (第 115 行)
   ```python
   self.lanelet_map = lanelet2.io.load(
       "/home/eowyn-lenovo/code/copy/map/new_lanelet2_maps_fixed.osm",
       projector
   )
   ```

3. **control-with-traffic-light.py** (第 464 行)
   ```python
   self.lanelet_map = lanelet2.io.load("/home/eowyn-lenovo/code/copy/map/new_lanelet2_maps_fixed.osm", projector)
   ```

**注意**: 请根据实际地图文件位置修改这些路径。

### 坐标转换参数

坐标转换参数在控制节点中定义（control-with-traffic-light.py）：

```python
OFFSET_1000 = 1000.0                   # local_x/local_y 基准偏移
OSM_ORIGIN_IN_WORLD_X = 76.82245635986328
OSM_ORIGIN_IN_WORLD_Y = -1.5535324811935425
CORRECTION_OFFSET_Y = -2.5             # Y方向校正
```

### 控制器参数

在 `control-with-traffic-light.py` 中可调整以下参数：

```python
# 车辆模型参数
L = 2.5                    # 轴距 (m)
max_steer = np.pi/4        # 最大转向角 (rad)
max_accel = 3.0            # 最大加速度 (m/s²)
max_decel = -5.0           # 最大减速度 (m/s²)

# Stanley 控制器参数
k = 0.5                    # 横向误差增益
Kp = 1.0                   # 航向误差增益

# 速度控制参数
target_speed = 10.0        # 目标速度 (m/s)，约 36 km/h
lookahead_distance = 5.0   # 前视距离 (m)
```

## 🎯 使用示例

### 示例 1：简单的点到点导航

```bash
# 1. 启动系统
ros2 launch carla_map carla_map_control.launch.py

# 2. 在 RViz 中设置起点（2D Pose Estimate）
#    - 点击地图上的起始位置

# 3. 设置目标点（2D Nav Goal）
#    - 点击地图上的目标位置

# 4. 观察车辆自动行驶到目标点
```

### 示例 2：监控控制状态

```bash
# 在新终端中监听控制状态
ros2 topic echo /control_status

# 监听车辆控制指令
ros2 topic echo /vehicle_control

# 查看规划路径
ros2 topic echo /planned_path
```

### 示例 3：查看交通灯状态

```bash
# 查看所有交通灯状态
ros2 topic echo /traffic_lights

# 查看话题频率
ros2 topic hz /traffic_lights
```

## 🔍 常见问题

### Q1: 启动后 RViz 中看不到地图？

**A**: 检查以下几点：
1. 确认地图文件路径正确
   ```bash
   ls /home/eowyn-lenovo/code/copy/map/new_lanelet2_maps_fixed.osm
   ```
2. 查看节点日志：
   ```bash
   ros2 topic echo /rosout
   ```
3. 确认 `/my_map` 话题有数据：
   ```bash
   ros2 topic echo /my_map
   ```

### Q2: 设置起点/终点后车辆不动？

**A**: 可能的原因：
1. 起点/终点距离道路太远（>10m）
2. 地图中没有从起点到终点的连通路径
3. 查看控制状态：
   ```bash
   ros2 topic echo /control_status
   ```

### Q3: 如何更换地图文件？

**A**: 
1. 准备新的 Lanelet2 OSM 地图文件
2. 修改三个 Python 文件中的地图路径（见[配置说明](#配置说明)）
3. 重新编译：
   ```bash
   colcon build --packages-select carla_map
   source install/setup.bash
   ```

### Q4: 如何调整车辆速度？

**A**: 
编辑 `src/carla_map/src/carla_map/control-with-traffic-light.py`，修改：
```python
# 大约在第 430 行附近
target_speed = 10.0  # 修改为你想要的速度 (m/s)
```
然后重新编译功能包。

### Q5: 交通灯状态如何更新？

**A**: 
当前版本使用模拟的交通灯状态。如需接入真实传感器：
1. 修改 `traffic_light_manager.py` 中的状态更新逻辑
2. 订阅来自感知模块的交通灯检测结果

### Q6: 编译时出现 "lanelet2 not found" 错误？

**A**: 
安装 Lanelet2：
```bash
sudo apt update
sudo apt install ros-humble-lanelet2 python3-lanelet2
```

## 📝 目录结构

```
carla_map/
├── CMakeLists.txt              # CMake 构建配置
├── package.xml                 # 功能包清单
├── README.md                   # 本文档
├── config/                     # 配置文件
│   └── carla_map_ego.rviz     # RViz 配置文件
├── launch/                     # 启动文件
│   └── carla_map_control.launch.py
├── msg/                        # 消息定义
│   ├── VehicleControl.msg
│   ├── SpeedProfile.msg
│   ├── TrafficLight.msg
│   └── TrafficLightArray.msg
└── src/
    └── carla_map/             # Python 源代码
        ├── __init__.py
        ├── osm_map_publisher_lanelet2.py    # 地图发布器
        ├── control-with-traffic-light.py    # 控制节点
        ├── traffic_light_manager.py          # 交通灯管理器
        └── tf_publisher.py                   # TF 发布器
```

## 🤝 贡献

欢迎提交 Issue 和 Pull Request！

## 📄 许可证

MIT License

## 👨‍💻 维护者

- 维护者: eowyn
- Email: eowyn0406@gmail.com

## 🙏 致谢

- CARLA 仿真器
- Lanelet2 地图框架
- ROS2 社区

---

**最后更新**: 2025-11-13