Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
59 commits
Select commit Hold shift + click to select a range
e022be1
CARLA 全局路径规划节点 Docker 兼容优化版
dream1112221 Sep 22, 2025
14a4457
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 24, 2025
0f502d7
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 24, 2025
99f7926
对车辆控制代码作优化
dream1112221 Sep 24, 2025
b6f41b7
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 24, 2025
485cba8
对于原车辆控制代码做简单优化
dream1112221 Sep 24, 2025
f6fd729
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 24, 2025
53011e5
对manual_control代码作了简单优化
dream1112221 Sep 24, 2025
84cda12
Delete D/system-commandline-sentinel-files directory
dream1112221 Sep 25, 2025
4fdd0c5
Delete src/car_control/optimized_details.md
dream1112221 Sep 25, 2025
4cce5ce
Add files via upload
dream1112221 Sep 25, 2025
f1e1e6a
对车辆控制代码作简单优化并加简单注释
dream1112221 Sep 24, 2025
44ffae2
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 25, 2025
adf08e7
Merge branch 'OpenHUTB:main' into main
dream1112221 Sep 29, 2025
5ae1ab3
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 13, 2025
c89dd63
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 14, 2025
4ec27fb
修改文件名,优化代码
dream1112221 Oct 14, 2025
cf1e070
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 14, 2025
657adcc
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 20, 2025
6657d00
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 23, 2025
09708ca
增加manual_control.launch文件
dream1112221 Oct 23, 2025
dc905bc
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 28, 2025
e6e14cf
修改按键控制小车代码
dream1112221 Oct 28, 2025
1f219c7
Merge branch 'OpenHUTB:main' into main
dream1112221 Oct 29, 2025
5fcecf7
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 3, 2025
6baf95d
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 3, 2025
c6205b0
优化ros到carla坐标转换代码
dream1112221 Nov 4, 2025
9d71344
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 4, 2025
92e8e3f
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 19, 2025
ea0d3ab
优化carla可视化目标路点函数,增强鲁棒性
dream1112221 Nov 19, 2025
23de29a
优化监控进程内存占用函数,避免内存占用过高致系统崩溃
dream1112221 Nov 19, 2025
117d716
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 20, 2025
240a015
优化可视化carla目标路点函数代码
dream1112221 Nov 20, 2025
2576d12
Merge branch 'OpenHUTB:main' into main
dream1112221 Nov 21, 2025
31d773a
优化按键控制小车代码
dream1112221 Nov 21, 2025
6e34d2d
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 1, 2025
8aa9834
完善launch文件,增加新节点
dream1112221 Dec 1, 2025
2fe8f1c
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 2, 2025
af95792
删除原来单个节点的launch文件并优化路径规划代码
dream1112221 Dec 2, 2025
d0c6681
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 7, 2025
999a032
补充launch文件,添加节点完善功能
dream1112221 Dec 7, 2025
5510380
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 8, 2025
75c1e56
优化代码,修复排序 lambda、减少重复属性访问
dream1112221 Dec 8, 2025
13060bb
优化HUD.tick 方法以提高健壮性与性能
dream1112221 Dec 8, 2025
c6cad1c
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 15, 2025
ba20ac8
完善launch文件,实现SLAM
dream1112221 Dec 15, 2025
ed9cb78
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 15, 2025
509cdd3
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 16, 2025
9b5ed5d
完善main.py和README文件,集成路径规划和SLAM功能
dream1112221 Dec 16, 2025
2239445
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 16, 2025
f584e8b
简化函数逻辑,提升代码简洁性
dream1112221 Dec 16, 2025
0f2158d
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 16, 2025
fc3e15a
优化 plan_path_cb 方法,提升可维护性
dream1112221 Dec 16, 2025
bea4b13
Merge branch 'main' of https://github.com/dream1112221/nn into main
dream1112221 Dec 16, 2025
58e82eb
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 16, 2025
2a0d979
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 17, 2025
156ee6e
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 17, 2025
7f9c836
修改README文件,将SLAM建图功能完善
dream1112221 Dec 17, 2025
d0eaca7
Merge branch 'OpenHUTB:main' into main
dream1112221 Dec 17, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 60 additions & 17 deletions src/carla_slam_gmapping/README.md
Original file line number Diff line number Diff line change
@@ -1,16 +1,16 @@
# 人车模拟
# CARLA 全局路径规划节点
# CARLA 全局路径规划和SLAM节点

实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。

新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。

## 功能概述
该节点是 CARLA 自动驾驶仿真系统中的核心模块之一,主要功能包括:
- **CARLA 交互**:建立与 CARLA 服务器的连接,获取地图数据和可导航的生成点(目标点候选)
- **路径规划服务**:提供 ROS 2 服务 `plan_to_random_goal`,接收起始点里程计信息,生成到随机目标点的全局路径
- **路径可视化**:通过 `visualization_marker` 话题发布路径标记,在 RViz 中以绿色线条直观展示规划结果
- **鲁棒性保障**:包含路径有效性校验(确保路径长度满足需求)、异常捕获和错误日志输出,避免无限循环或崩溃

- **3D 建图**:集成 GMapping 算法,利用激光雷达数据进行实时三维地图构建

## 环境配置
### 基础环境
Expand Down Expand Up @@ -45,8 +45,11 @@
```bash
sudo apt install -y python3-colcon-common-extensions
```


5. **GMapping 依赖** 安装 slam_gmapping 包以支持 3D 建图功能:
'''
bash
sudo apt install ros-foxy-slam-gmapping
'''
## 快速开始
### 1. 工作空间准备
假设你的 ROS 2 工作空间为 `carla_ws`,将该节点所在的功能包 `carla_global_planner` 放入 `src` 目录:
Expand Down Expand Up @@ -83,10 +86,11 @@ cd ~/carla_ws
# 激活 ROS 2 环境和工作空间环境
source /opt/ros/foxy/setup.bash
source install/setup.bash
# 启动节点
ros2 run carla_global_planner carla_global_planner_node
# 启动节点和建图功能
ros2 launch carla_global_planner carla_slam_launch.py
```
- 节点启动成功后,会输出日志 `CARLA全局路径规划服务已启动`
- GMapping 建图节点同时启动,开始接收激光雷达数据并构建地图
- 若脚本卡住无报错,说明节点正常运行(等待服务请求);若报错 "CARLA客户端初始化失败",需检查 CARLA 服务器是否已启动,或连接地址/端口是否正确

#### 步骤 3:调用路径规划服务
Expand All @@ -101,7 +105,7 @@ ros2 service call /plan_to_random_goal carla_global_planner/srv/PlanGlobalPath "
- 服务调用成功后,节点会输出路径信息(如 `成功规划路径,包含XX个路点`)

#### 步骤 4:可视化路径
打开新终端,启动 RViz 查看路径:
打开新终端,启动 RViz 查看路径和建图结果:
```bash
source /opt/ros/foxy/setup.bash
rviz2
Expand All @@ -110,7 +114,32 @@ rviz2
1. 设置 `Fixed Frame` 为 `map`
2. 添加 `Marker` 显示项,设置 `Topic` 为 `/visualization_marker`
3. 路径会以绿色线条显示在场景中

4. 添加 Map 显示项,设置 Topic 为 /map 查看实时构建的地图

3D 建图算法说明
GMapping 算法原理
本项目采用经典的 GMapping 算法实现 3D 建图功能。GMapping 是一种基于粒子滤波的 SLAM( simultaneous localization and mapping)算法,它通过以下步骤实现地图构建:

粒子滤波: 使用多个粒子表示机器人的可能位姿,每个粒子都携带一个地图副本
运动模型更新: 根据里程计数据预测每个粒子的新位姿
观测模型评估: 利用激光雷达扫描数据评估每个粒子的权重
重采样: 根据权重重新分布粒子集,保留高似然的粒子
地图更新: 基于最优粒子的位姿估计更新全局地图
数据流
输入传感器数据:
激光雷达扫描数据 (/scan)
里程计数据 (/odom)
输出:
2D 占据栅格地图 (/map)
机器人位姿估计 (/map 坐标系下的变换)
参数配置
GMapping 算法的关键参数已在 launch 文件中预设,可根据具体场景调整:

maxUrange: 激光雷达最大使用距离 (单位: 米)
sigma: 扫描匹配的标准差
particles: 粒子数量
delta: 地图分辨率 (单位: 米/cell)
temporalUpdate: 地图更新频率

## 核心接口说明
### 1. ROS 2 服务
Expand All @@ -131,9 +160,12 @@ nav_msgs/Path path
```

### 2. ROS 2 话题
| 话题名称 | 消息类型 | 功能描述 |
|------------------------|-----------------------------------|-------------------------------------------|
| `/visualization_marker`| `visualization_msgs/Marker` | 发布路径可视化标记(绿色 LINE_STRIP 类型) |
话题名称 消息类型 功能描述
/visualization_marker visualization_msgs/Marker 发布路径可视化标记(绿色 LINE_STRIP 类型)
/scan sensor_msgs/LaserScan 激光雷达扫描数据,用于建图
/odom nav_msgs/Odometry 里程计数据,用于定位和建图
/map nav_msgs/OccupancyGrid 构建的 2D 占据栅格地图



## 常见问题排查
Expand All @@ -157,6 +189,11 @@ nav_msgs/Path path

4. **编译报错 "找不到 carla_global_planner/srv/PlanGlobalPath"**
- 确认 `srv` 目录下已定义 `PlanGlobalPath.srv` 文件,且 `CMakeLists.txt`/`package.xml` 已正确配置服务依赖
5. **RViz 中看不到地图**

确认 Map 显示项已添加且 Topic 设置为 /map
检查是否有地图数据发布:ros2 topic echo /map
确保激光雷达数据正常:ros2 topic echo /scan


## 代码结构说明
Expand All @@ -168,6 +205,8 @@ carla_global_planner/
│ └── PlanGlobalPath.srv # 路径规划服务定义
├── utilities/
│ └── planner.py # 路径计算工具(compute_route_waypoints 函数)
├── launch/
│ └── carla_slam_launch.py # 启动文件,包含建图功能
├── CMakeLists.txt # 编译配置
└── package.xml # 依赖和包信息配置
```
Expand All @@ -186,10 +225,13 @@ carla_global_planner/
- 关键逻辑变更需同步更新 README.md

2. **功能扩展建议**
- 支持自定义目标点(而非仅随机目标点)
- 增加路径规划算法选择(如 A*、RRT*)
- 添加路径平滑处理,优化车辆行驶轨迹
- 增加单元测试(基于 `pytest` 和 `ros2 test`)
支持自定义目标点(而非仅随机目标点)
增加路径规划算法选择(如 A*、RRT*)
添加路径平滑处理,优化车辆行驶轨迹
增加单元测试(基于 pytest 和 ros2 test)
集成其他 SLAM 算法(如 Hector SLAM、Karto SLAM)以提供更多建图选项



3. **提交流程**
准备提交代码前,请确保:
Expand All @@ -202,4 +244,5 @@ carla_global_planner/
1. [CARLA 官方文档](https://carla.readthedocs.io/en/0.9.15/) - 了解 CARLA Python API 和地图数据结构
2. [ROS 2 Foxy 官方文档](https://docs.ros.org/en/foxy/) - 学习 ROS 2 服务、话题和节点开发
3. [nav_msgs/Path 消息定义](https://docs.ros.org/en/foxy/api/nav_msgs/html/msg/Path.html) - 路径消息格式说明
4. [visualization_msgs/Marker 消息定义](https://docs.ros.org/en/foxy/api/visualization_msgs/html/msg/Marker.html) - 可视化标记格式说明
4. [visualization_msgs/Marker 消息定义](https://docs.ros.org/en/foxy/api/visualization_msgs/html/msg/Marker.html) - 可视化标记格式说明
5. slam_gmapping 官方文档 - GMapping 算法详细说明
97 changes: 52 additions & 45 deletions src/carla_slam_gmapping/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,7 @@
CARLA全局路径规划节点 - 增强版
提供从起始点到随机目标点的路径规划服务,并将规划结果通过ROS消息发布和可视化
新增功能:
- 路径长度估算
- 连接状态监控
- 改进的错误处理
- 性能统计
- 配置参数化

"""

import rclpy
Expand All @@ -23,6 +19,7 @@
import time # 新增:用于性能统计
import math # 新增:用于距离计算
from tf_transformations import quaternion_from_euler
from typing import Optional, Tuple, List, Any


class GlobalPlannerNode(Node):
Expand Down Expand Up @@ -134,63 +131,74 @@ def _publish_performance_stats(self):

def plan_path_cb(self, request, response):
"""
路径规划服务回调函数 - 增强版
接收起始点,规划到随机目标点的路径并返回
新增性能统计和更好的错误处理

不足:原方法过长,包含多个职责,难以维护和测试。
优化:拆分为子方法,提高可读性和可维护性。
"""
start_time = time.time() # 新增:开始计时
start_time = time.time()

# 新增:检查CARLA连接状态
if not self.carla_connected or not self.map:
self.get_logger().error("CARLA连接未建立或地图未初始化,无法规划路径")
response.success = False # 假设服务定义中有success字段
if not self._validate_connection(response):
return response

try:
# 将ROS坐标转换为CARLA坐标
start_location = self._ros_to_carla_location(request.start)
start_wp = self.map.get_waypoint(start_location)

if not start_wp: # 新增:检查起始路点有效性
start_wp = self._get_start_waypoint(request.start)
if not start_wp:
self.get_logger().error("无法在起始位置找到有效的路点")
return response

# 规划足够长的路径
min_waypoints = self.get_parameter('min_waypoints').value
max_attempts = self.get_parameter('max_planning_attempts').value

route, goal_wp = self._get_valid_route(start_wp, min_waypoints, max_attempts)
route, goal_wp = self._plan_route(start_wp)
if not route:
self.get_logger().error("无法生成有效的路径")
return response

# 新增:计算路径总长度
total_distance = self._calculate_path_distance(route)

# 构建路径消息并可视化
path_msg = self._build_path_message(route)
self._visualize_path_and_goal(path_msg, goal_wp) # 修改:同时可视化路径和目标点

path_msg = self._process_route(route, goal_wp)
response.path = path_msg

# 新增:更新性能统计
planning_time = time.time() - start_time
self.last_planning_time = planning_time
self.total_planning_time += planning_time
self.planning_count += 1
self._update_performance_stats(time.time() - start_time, len(route), self._calculate_path_distance(route))

self.get_logger().info(
f"成功规划路径 - "
f"路点数: {len(route)}, "
f"总距离: {total_distance:.2f}m, "
f"用时: {planning_time:.3f}s"
)
return response

except Exception as e:
self.get_logger().error(f"路径规划过程中发生错误: {str(e)}")
return response

def _validate_connection(self, response) -> bool:
"""验证CARLA连接状态"""
if not self.carla_connected or not self.map:
self.get_logger().error("CARLA连接未建立或地图未初始化,无法规划路径")
response.success = False
return False
return True

def _get_start_waypoint(self, start_msg) -> Optional[carla.Waypoint]:
"""获取起始路点"""
start_location = self._ros_to_carla_location(start_msg)
return self.map.get_waypoint(start_location)

def _plan_route(self, start_wp: carla.Waypoint) -> Tuple[Optional[List], Optional[carla.Waypoint]]:
"""规划路径"""
min_waypoints = self.get_parameter('min_waypoints').value
max_attempts = self.get_parameter('max_planning_attempts').value
return self._get_valid_route(start_wp, min_waypoints, max_attempts)

def _process_route(self, route: List, goal_wp: Optional[carla.Waypoint]) -> Path:
"""处理路径:构建消息并可视化"""
path_msg = self._build_path_message(route)
self._visualize_path_and_goal(path_msg, goal_wp)
return path_msg

def _update_performance_stats(self, planning_time: float, waypoint_count: int, total_distance: float):
"""更新性能统计"""
self.last_planning_time = planning_time
self.total_planning_time += planning_time
self.planning_count += 1
self.get_logger().info(
f"成功规划路径 - "
f"路点数: {waypoint_count}, "
f"总距离: {total_distance:.2f}m, "
f"用时: {planning_time:.3f}s"
)

def _ros_to_carla_location(self, odom_msg):
"""将ROS里程计消息中的位置转换为CARLA坐标"""
return carla.Location(
Expand All @@ -201,7 +209,7 @@ def _ros_to_carla_location(self, odom_msg):

def _get_valid_route(self, start_wp, min_waypoints=50, max_attempts=10):
"""
获取有效的路径 - 增强版
获取有效的路径 -
返回路径和目标路点元组
尝试多次生成路径,直到满足最小路点数量要求或达到最大尝试次数
"""
Expand Down Expand Up @@ -245,7 +253,6 @@ def _get_valid_route(self, start_wp, min_waypoints=50, max_attempts=10):
if len(route) >= min_waypoints:
return route, goal_wp # 找到满足要求的路径,直接返回

# 返回最佳路径(即使不满足最小长度要求)
if best_route:
self.get_logger().warning(
f"达到最大尝试次数({max_attempts}),返回最长路径 "
Expand Down Expand Up @@ -362,10 +369,10 @@ def _create_path_marker(self, header, action=Marker.ADD, points=None, line_width
marker.color.g = 1.0 # 绿色
marker.color.b = 0.0

# 新增:设置生存时间,自动清理旧标记

marker.lifetime.sec = 60 # 60秒后自动删除

# 添加点集(仅用于ADD动作)

if points and action == Marker.ADD:
marker.points = points

Expand Down
Loading