EGOSwarm—Prometheus代码阅读笔记【二】——节点实现

Prometheusv2中的EGOSwarm代码阅读笔记，个人学习用。
目前还有部分代码没看明白，欢迎大家交流！
EGOPlanner—Prometheus代码阅读笔记汇总

主要参考资料：
Fast Planner——代码解读参考资料整理
EGO-Swarm代码阅读笔记汇总
代码来源：
https://github.com/ZJU-FAST-Lab/ego-planner
https://github.com/amov-lab/Prometheus

---

---

（一）EGOPlanner主节点：ego_planner_node

1.源文件位置

• plan_manage\src\ego_planner_node.cpp
• plan_manage\src\ego_replan_fsm.cpp

在ego_planner_node.cpp中只进行了EGOReplanFSM 类的实例化和初始化，话题订阅与发布在EGOReplanFSM.init中实现，具体功能在EGOReplanFSM 类中实现

2.订阅话题

• /odom_world：保存无人机当前里程计信息，包括位置、速度和姿态
• /uav*_planning/swarm_trajs
• /planning/broadcast_bspline_to_planner
• /uav*/prometheus/motion_planning/goal
• /uav*/traj_start_trigger

3.发布话题

• /uav*_planning/swarm_trajs
• /planning/broadcast_bspline_from_planner
• /planning/bspline
• /planning/data_display

4.定时器

• exec_timer_：状态机切换
• safety_timer_：安全检查

（二） 发布控制指令节点：traj_server_for_prometheus

1.源文件位置

• plan_manage\src_for_prometheus\traj_server_for_prometheus.cpp

2.订阅话题

• uav*/planning/bspline，回调函数bsplineCallback。得到优化后均匀B样条的控制点、阶数、区间间隔等，接着将轨迹的位置、速度和加速度存储在traj_等待定时器cmdCallback发布。

3.发布话题

• uav*/position_cmd，控制指令，包括期望位置、速度、加速度和偏航角
• uav*/prometheus/command，发布给Prometheus控制

4.定时器

• cmdCallback根据得到的轨迹发布话题