diff --git a/src/carla_slam_gmapping/README.md b/src/carla_slam_gmapping/README.md index 3973b3231a..abb80cebcb 100644 --- a/src/carla_slam_gmapping/README.md +++ b/src/carla_slam_gmapping/README.md @@ -1,8 +1,8 @@ # 人车模拟 -# CARLA 全局路径规划节点 +# CARLA 全局路径规划和SLAM节点 实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。 - +新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。 ## 功能概述 该节点是 CARLA 自动驾驶仿真系统中的核心模块之一,主要功能包括: @@ -10,7 +10,7 @@ - **路径规划服务**:提供 ROS 2 服务 `plan_to_random_goal`,接收起始点里程计信息,生成到随机目标点的全局路径 - **路径可视化**:通过 `visualization_marker` 话题发布路径标记,在 RViz 中以绿色线条直观展示规划结果 - **鲁棒性保障**:包含路径有效性校验(确保路径长度满足需求)、异常捕获和错误日志输出,避免无限循环或崩溃 - +- **3D 建图**:集成 GMapping 算法,利用激光雷达数据进行实时三维地图构建 ## 环境配置 ### 基础环境 @@ -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` 目录: @@ -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:调用路径规划服务 @@ -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 @@ -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 服务 @@ -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 占据栅格地图 + ## 常见问题排查 @@ -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 ## 代码结构说明 @@ -168,6 +205,8 @@ carla_global_planner/ │ └── PlanGlobalPath.srv # 路径规划服务定义 ├── utilities/ │ └── planner.py # 路径计算工具(compute_route_waypoints 函数) +├── launch/ +│ └── carla_slam_launch.py # 启动文件,包含建图功能 ├── CMakeLists.txt # 编译配置 └── package.xml # 依赖和包信息配置 ``` @@ -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. **提交流程** 准备提交代码前,请确保: @@ -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) - 可视化标记格式说明 \ No newline at end of file +4. [visualization_msgs/Marker 消息定义](https://docs.ros.org/en/foxy/api/visualization_msgs/html/msg/Marker.html) - 可视化标记格式说明 +5. slam_gmapping 官方文档 - GMapping 算法详细说明 \ No newline at end of file diff --git a/src/carla_slam_gmapping/main.py b/src/carla_slam_gmapping/main.py index 1e6eb639dd..bad0e3a8a1 100644 --- a/src/carla_slam_gmapping/main.py +++ b/src/carla_slam_gmapping/main.py @@ -3,11 +3,7 @@ CARLA全局路径规划节点 - 增强版 提供从起始点到随机目标点的路径规划服务,并将规划结果通过ROS消息发布和可视化 新增功能: -- 路径长度估算 -- 连接状态监控 -- 改进的错误处理 -- 性能统计 -- 配置参数化 + """ import rclpy @@ -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): @@ -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( @@ -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): """ - 获取有效的路径 - 增强版 + 获取有效的路径 - 返回路径和目标路点元组 尝试多次生成路径,直到满足最小路点数量要求或达到最大尝试次数 """ @@ -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}),返回最长路径 " @@ -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