# auto_navigation **Repository Path**: libaos/auto_navigation ## Basic Information - **Project Name**: auto_navigation - **Description**: 自动驾驶 - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2025-03-25 - **Last Updated**: 2025-03-25 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 自动导航系统 (Auto Navigation System) 这个ROS包提供了一套完整的工具，用于处理KITTI数据集，构建NDT地图，使用NDT进行定位和导航，以及目标点可行性检查。 ## 功能特性 - KITTI数据集转换为ROS bag格式 - 点云数据预处理和过滤 - NDT地图构建 - NDT定位 - 点云地图可视化 - 点云数据导出和分析 - 目标点可行性检查与自动修正 - 全局路径规划 - 局部路径规划避障 ## 系统要求 - ROS Noetic - PCL 1.10+ - Eigen 3.3+ - KITTI数据集 ## 安装 ```bash cd ~/catkin_ws/src git clone auto_navigation cd ~/catkin_ws catkin_make source devel/setup.bash ``` ## 环境配置 为了避免每次都需要手动执行`source devel/setup.bash`命令，可以使用以下命令将ROS环境自动加载到`.bashrc`文件中： ```bash # 添加自动加载ROS环境到.bashrc bash /root/catkin_ws/src/auto_navigation/scripts/auto_setup.sh # 使更改立即生效 source ~/.bashrc ``` 这将会在`.bashrc`文件中添加必要的命令，使ROS环境在每次打开终端时自动加载，并创建以下便捷命令别名： - `build_map` - 构建NDT地图 - `run_ndt` - 运行NDT定位 - `create_bag` - 创建ROS bag - `play_bag` - 播放ROS bag - `export_map` - 导出点云数据 - `git_upload` - 上传到Git仓库 ## 代码上传到Git仓库 可以使用以下命令将代码上传到Gitee仓库： ```bash # 执行Git上传脚本 bash /root/catkin_ws/src/auto_navigation/scripts/upload_to_git.sh # 或使用别名（如果已设置自动加载环境） git_upload ``` 该脚本将会： 1. 设置Git全局配置 2. 初始化仓库 3. 添加并提交所有文件 4. 将代码推送到远程仓库 ## 使用方法 ### 1. KITTI数据集转换为ROS bag ```bash # 转换KITTI数据集为ROS bag create_bag # 播放转换后的ROS bag play_bag ``` ### 2. NDT地图构建 ```bash # 使用ROS bag构建NDT地图 build_map [bag_file_path] ``` ### 3. NDT定位 ```bash # 使用ROS bag和NDT地图进行定位 run_ndt [bag_file_path] [map_file_path] ``` ### 4. 点云地图可视化 ```bash # 可视化点云地图 roslaunch auto_navigation visualize_map.launch map_file:=/path/to/map.pcd ``` ### 5. 点云数据导出和分析 ```bash # 导出点云数据为ASCII格式 export_map # 生成点云2D投影图 python3 /root/catkin_ws/src/auto_navigation/scripts/generate_projections.py ``` ### 6. 目标点可行性检查系统 #### 6.1 启动系统 ```bash # 启动ROS核心（如果尚未运行） roscore # 启动全局规划节点 cd ~/catkin_ws && source devel/setup.bash roslaunch /root/catkin_ws/src/auto_navigation/launch/global_planning.launch ``` #### 6.2 设置目标点 ##### 方法一：通过RViz图形界面 1. 在RViz工具栏中选择"2D Nav Goal"工具（箭头图标） 2. 在地图上点击您想要导航到的位置 3. 拖动箭头可设置方向（朝向） ##### 方法二：通过命令行 ```bash rostopic pub -1 /move_base_simple/goal geometry_msgs/PoseStamped '{header: {frame_id: "map"}, pose: {position: {x: X, y: Y, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}' ``` #### 6.3 系统输出 ##### 可视化输出（RViz） - **点云地图**：显示环境的3D点云数据 - **原始目标点**：通过`/move_base_simple/goal`话题显示（蓝色箭头） - **有效目标点**：通过`/planning/valid_goal`话题显示（红色箭头） - **规划路径**：通过`/planning/global_path`话题显示（绿色线条） ##### 日志输出 系统会在终端输出详细日志： ``` [INFO] Received goal: (-3.71, -4.13) [WARN] Goal position (-3.71, -4.13) is occupied, searching for nearest valid position [INFO] Found valid goal position at (-4.15, -4.20), 0.45 m from original goal [WARN] Original goal at (-3.71, -4.13) was occupied, using modified goal at (-4.15, -4.20) [INFO] Planning path from (0.00, 0.00) to (-4.15, -4.20) [INFO] Path found with 272 waypoints ``` ### 7. 局部路径规划避障系统 局部路径规划避障系统能够使机器人在执行全局路径时，实时检测和避开路径上的动态障碍物，确保导航安全可靠。 #### 7.1 系统架构 局部路径规划避障系统由以下组件构成： 1. **传感器数据处理模块**：处理来自激光雷达、深度相机等传感器的实时数据 2. **局部地图构建模块**：基于传感器数据动态构建机器人周围的局部代价地图 3. **局部规划算法模块**：在局部代价地图上计算最优避障路径 4. **速度指令生成模块**：将规划路径转化为机器人控制指令 #### 7.2 启动系统 ```bash # 启动局部路径规划避障功能 cd ~/catkin_ws && source devel/setup.bash roslaunch /root/catkin_ws/src/auto_navigation/launch/local_planning.launch ``` #### 7.3 实现方法 ##### 7.3.1 基于Dynamic Window Approach (DWA)的实现 DWA算法是一种常用的局部路径规划算法，通过在速度空间中搜索最优轨迹来避开障碍物。 ```cpp // DWA算法核心代码示例 std::vector generateTrajectories(const geometry_msgs::Twist& current_velocity) { std::vector trajectories; // 在速度空间采样可能的速度组合 for (double v = v_min; v <= v_max; v += v_step) { for (double w = w_min; w <= w_max; w += w_step) { // 生成轨迹 Trajectory traj = simulateTrajectory(v, w, simulation_time); // 评估轨迹 double score = evaluateTrajectory(traj); traj.score = score; trajectories.push_back(traj); } } return trajectories; } // 轨迹评估函数 double evaluateTrajectory(const Trajectory& traj) { double obstacle_cost = calculateObstacleCost(traj); double goal_cost = calculateGoalCost(traj); double path_cost = calculatePathCost(traj); // 总成本 = 障碍物成本*权重 + 目标成本*权重 + 路径成本*权重 return obstacle_weight * obstacle_cost + goal_weight * goal_cost + path_weight * path_cost; } ``` ##### 7.3.2 基于Timed Elastic Band (TEB)的实现 TEB是一种考虑运动学约束的局部规划方法，能够生成平滑、高效的避障路径。 ```cpp // TEB参数配置示例 ``` #### 7.4 参数配置 局部路径规划避障系统的关键参数： ```xml ``` #### 7.5 局部避障模式 局部路径规划避障系统支持以下避障模式： ##### 7.5.1 常规避障模式 适用于普通动态环境，提供基本的避障功能。 ##### 7.5.2 紧急避障模式 当检测到高风险障碍物时，启动紧急避障模式，暂停跟随全局路径，优先执行避障动作。 ##### 7.5.3 狭窄通道通过模式 针对窄通道环境优化轨迹，提高通过成功率。 #### 7.6 系统输出 局部路径规划避障系统会输出以下信息： ##### 可视化输出（RViz） - **局部代价地图**：通过`/local_costmap/costmap`话题显示 - **局部规划路径**：通过`/local_plan`话题显示 - **可视化轨迹**：通过`/dwa_planner/trajectories`话题显示所有评估的轨迹 - **选中轨迹**：通过`/dwa_planner/selected_trajectory`话题显示最终选中的轨迹 ##### 日志输出 系统会在终端输出详细日志： ``` [INFO] Local planner initialized [INFO] Local costmap updated, size: 100x100 [WARN] Obstacle detected at (1.2, 0.5) with velocity (0.1, 0.0) [INFO] Generated 243 trajectories for evaluation [INFO] Selected trajectory with score 0.78 [INFO] Command velocity: linear=0.3, angular=0.1 ``` #### 7.7 与全局规划的集成 局部路径规划避障系统与全局路径规划系统通过以下方式集成： 1. 全局规划生成长期路径，指导机器人从起点到目标点 2. 局部规划以一定频率获取当前位置附近的全局路径段 3. 局部规划基于当前感知信息生成避障轨迹，尽量跟随全局路径 4. 如遇不可避让的障碍物，请求全局规划重新规划路径 ```cpp // 集成示例代码 void LocalPlanner::updatePlan(const std::vector& global_plan) { // 存储全局路径 global_path_ = global_plan; // 提取局部目标点 local_goal_ = extractLocalGoal(global_plan, lookahead_distance_); // 如果局部目标被障碍物阻挡，请求重新规划 if (isLocalGoalBlocked(local_goal_)) { requestGlobalReplanning(); } } ``` ## 参数配置 ### NDT算法参数 - 体素大小 (Voxel Size): 0.3 - NDT分辨率 (NDT Resolution): 1.0 - 最大迭代次数 (Max Iterations): 30 - 步长 (Step Size): 0.1 - 收敛阈值 (Convergence Threshold): 0.01 ### 目标点可行性检查参数 ```xml ``` ## 文件结构 ``` auto_navigation/ ├── CMakeLists.txt ├── package.xml ├── README.md ├── launch/ │ ├── navigation_with_best_map.launch │ ├── kitti_ndt_mapping_with_bag.launch │ ├── kitti_ndt_localization_with_bag.launch │ ├── play_kitti_bag.launch │ └── visualize_map.launch ├── rviz/ │ ├── ndt_mapping.rviz │ ├── ndt_localization.rviz │ └── view_bag.rviz ├── scripts/ │ ├── auto_setup.sh │ ├── build_ndt_map_from_bag.sh │ ├── create_kitti_rosbag.sh │ ├── export_map_simple.sh │ ├── generate_projections.py │ ├── kitti_to_rosbag.py │ ├── play_kitti_bag.sh │ ├── run_ndt_localization_with_bag.sh │ ├── setup_env.sh │ └── upload_to_git.sh ├── src/ │ ├── mapping/ │ │ └── ndt_mapper_node.cpp │ ├── localization/ │ │ └── ndt_localizer_node.cpp │ └── utils/ │ └── initial_pose_publisher.cpp └── maps/ └── best_map.pcd ``` ## 注意事项 1. 确保在运行脚本前已经正确设置ROS环境 2. 检查KITTI数据集路径是否正确 3. 对于大型点云数据，可能需要调整内存限制 4. 在使用NDT定位前，确保已经构建了高质量的NDT地图 5. 目标点可行性检查系统需要有效的点云地图 6. 确保RViz配置正确以显示所有必要的信息 ## 故障排除 ### 常见问题 #### 1. 启动错误 **问题**: 无法启动launch文件 ``` RLException: [global_planning.launch] is neither a launch file in package [auto_navigation] nor is [auto_navigation] a launch file name ``` **解决方案**: ```bash cd /root/catkin_ws && source devel/setup.bash roslaunch /root/catkin_ws/src/auto_navigation/launch/global_planning.launch ``` #### 2. 地图显示问题 **问题**: RViz中没有显示点云地图 **解决方案**: 1. 确认点云话题是否正在发布: ```bash rostopic hz /cloud_pcd ``` 2. 检查RViz配置是否正确: ```bash rosrun rviz rviz -d /root/catkin_ws/src/auto_navigation/rviz/path_planning.rviz ``` 3. 在RViz中手动添加PointCloud2显示，并将Topic设置为"/cloud_pcd" #### 3. 路径规划失败 **问题**: 系统无法找到路径或目标点始终被拒绝 **解决方案**: 1. 检查日志输出是否表明目标点在障碍物上 2. 扩大目标点搜索半径参数(`goal_search_radius`) 3. 减小点云地图栅格分辨率以获得更精细的规划 4. 检查TF变换是否正确: ```bash rosrun tf tf_echo map base_link ``` #### 4. 局部避障失败 **问题**: 机器人无法绕过障碍物或在障碍物前停止 **解决方案**: 1. 确认传感器数据是否正确输入到局部规划器: ```bash rostopic echo /scan ``` 2. 检查局部代价地图更新: ```bash rostopic hz /local_costmap/costmap ``` 3. 调整避障参数: - 增大`min_obstacle_dist`以提前开始避障 - 增加`path_distance_bias`让机器人更严格地跟随全局路径 - 减小`goal_distance_bias`以减少直接前往目标的趋势 4. 检查速度设置: ```bash rosparam get /DWAPlannerROS/max_vel_x rosparam get /DWAPlannerROS/max_vel_rot ``` #### 5. 轨迹过于激进 **问题**: 机器人避障轨迹不平滑，运动激进 **解决方案**: 1. 减小最大速度和加速度限制 2. 增加轨迹评估中平滑度的权重 3. 使用TEB规划器替代DWA以获得更平滑的轨迹 4. 调整参数以在安全和效率之间取得平衡 ### 高级调试 对于局部路径规划器，可以使用以下命令可视化规划器内部状态： ```bash # 可视化DWA所有可能轨迹 rosrun rqt_reconfigure rqt_reconfigure # 在打开的界面中，选择DWAPlannerROS，勾选"publish_traj_pc"选项 # 可视化代价地图 rosrun rqt_image_view rqt_image_view # 在下拉菜单中选择"/local_costmap/costmap/costmap" ``` ## 许可证 MIT License