ego_planner算法的仿真环境(主要是ros)-算法的解耦实现.

本系列内容主要是深入分析ego_planner算法的实现,并且将核心的算法部分转化为非ros依赖的形式(Cmake项目),实现上我们主要通过共享内存与ros的仿真环境进行数据交换.

项目链接:https://github.com/chan-yuu/ego_planner_standalone.git

如果觉得有用的话不要忘了star哦.

text 复制代码

├── 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: 生成轨迹

使用方法

bash 复制代码

pip install pyside6

bash 复制代码

python application/ego_planner_gui.py

到build界面中,编译两个部分:

编译完成后,就可以直接启动了,其中前面两个大的是总的启动按钮,可以运行四个start

或者按照顺序启动下面的四个start即可.

对应的命令是:

bash 复制代码

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