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分离为两部分：

1. ROS Bridge (ego-planner/src/ros_bridge): 处理ROS消息和共享内存通信
2. Planner Standalone (planner_standalone): 无ROS依赖的纯C++规划算法

当前架构

数据流

仿真环境 (simulator)
    ↓
ROS话题订阅
    ↓
ROS Bridge Node
    ↓
共享内存 (Shared Memory)
    ↓
Planner Standalone (FSM + 算法)
    ↓
共享内存 (Shared Memory)
    ↓
ROS Bridge Node
    ↓
ROS话题发布 → Traj Server → 控制器

输入数据（从仿真环境）

1. 定位数据 (/visual_slam/odom)

   • 位置、速度、姿态
   • 频率: ~100Hz

2. 点云数据 (/grid_map/occupancy_inflate)

   • 已膨胀的障碍物点云
   • 频率: ~10Hz
   • 注意: 仿真已完成膨胀，算法端不需要再处理

3. 目标航点 (/waypoint_generator/waypoints)

   • 目标位置
   • 事件驱动（用户在RViz中点击）

输出数据（给控制器）

1. B样条轨迹 (/planning/bspline)

   • 控制点
   • 节点向量 (knots)
   • 轨迹ID
   • 发布给 traj_server

2. 轨迹可视化 (/planning/bspline_path)

   • Path消息格式
   • 用于RViz显示

3. 规划器状态 (/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