ego_planner算法的仿真环境(主要是ros)-算法的解耦实现.
本系列内容主要是深入分析ego_planner算法的实现,并且将核心的算法部分转化为非ros依赖的形式(Cmake项目),实现上我们主要通过共享内存与ros的仿真环境进行数据交换.
项目链接:https://github.com/chan-yuu/ego_planner_standalone.git
如果觉得有用的话不要忘了star哦.
├── application # gui界面,用于快速启动程序 │ └── ego_planner_gui.py ├── docs │ ├── 1.0 │ ├── png │ └── scripts ├── ego-planner # ros环境部分,包括感知信息 │ ├── modified_realsense2_camera.zip │ └── src ├── planner_standalone # 独立的算法模块. │ ├── build # 编译到build中的可执行文件. │ ├── CMakeLists.txt │ ├── include │ └── src └── README.md ego_planner_standalone
本项目将原始的ROS EGO-Planner分离为两部分：
- ROS Bridge (
ego-planner/src/ros_bridge): 处理ROS消息和共享内存通信 - Planner Standalone (
planner_standalone): 无ROS依赖的纯C++规划算法
当前架构
数据流
仿真环境 (simulator) ↓ ROS话题订阅 ↓ ROS Bridge Node ↓ 共享内存 (Shared Memory) ↓ Planner Standalone (FSM + 算法) ↓ 共享内存 (Shared Memory) ↓ ROS Bridge Node ↓ ROS话题发布 → Traj Server → 控制器 输入数据（从仿真环境）
- 定位数据 (
/visual_slam/odom)- 位置、速度、姿态
- 频率: ~100Hz
- 点云数据 (
/grid_map/occupancy_inflate)- 已膨胀的障碍物点云
- 频率: ~10Hz
- 注意: 仿真已完成膨胀，算法端不需要再处理
- 目标航点 (
/waypoint_generator/waypoints)- 目标位置
- 事件驱动（用户在RViz中点击）
输出数据（给控制器）
- B样条轨迹 (
/planning/bspline)- 控制点
- 节点向量 (knots)
- 轨迹ID
- 发布给
traj_server
- 轨迹可视化 (
/planning/bspline_path)- Path消息格式
- 用于RViz显示
- 规划器状态 (
/planning/planner_state)- FSM状态 (INIT, WAIT_TARGET, GEN_NEW_TRAJ, etc.)
算法流程
整个规划器的运行逻辑由 main.cpp 驱动，通过 EGOPlannerManager 协调各个模块。
核心规划层基础设施层后端优化循环1.读取状态/环境是2.提供障碍物距离路径点平滑/碰撞/可行性未收敛收敛3.写入轨迹否GridMap: 地图更新需要重规划?DynAStar: 前端路径搜索B-Spline: 轨迹初始化计算代价 & 梯度开始优化L-BFGS: 迭代更新控制点时间重新分配main.cpp: 主循环IOInterface: 共享内存读写程序启动PolynomialTrajectory: 生成轨迹
使用方法
pip install pyside6 python application/ego_planner_gui.py 
到build界面中,编译两个部分:
编译完成后,就可以直接启动了,其中前面两个大的是总的启动按钮,可以运行四个start
或者按照顺序启动下面的四个start即可.
对应的命令是:
cd ego-planner;source devel/setup.bash && roslaunch ego_planner_bridge sim_only.launch cd ego-planner;source devel/setup.bash && roslaunch grid_map_standalone test_grid_map.launch cd ego-planner;source devel/setup.bash && roslaunch ego_planner_bridge run_bridge.launch cd planner_standalone/build;./ego_planner_standalone 
