ego_planner_node.cpp
1.1 初始化节点，设置句柄，并进入初始化函数
ros::init(argc, argv, "ego_planner_node");
ros::NodeHandle nh("~");
EGOReplanFSM rebo_replan;
rebo_replan.init(nh);
ego_replan_fsm.cpp
2.1 设置waypoint参数
nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
- 尝试从参数服务器中获取名为"fsm/waypoint" + to_string(i) + "x"的参数，并将其值赋给waypoints[i][0]。如果参数服务器中没有这个参数，那么waypoints_[i][0]将被设置为-1.0。
<param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
- 在plan_manager/lunch/advanced_param.xml文件中，定义了一个名为fsm/waypoint0_x的参数，其值来源于启动文件中的point0_x参数或变量，并且这个参数的类型是双精度浮点数。这样的定义允许在ROS节点之间共享和访问这个参数，使得不同的节点可以使用相同的路径点坐标。
<arg name="point0_x" value="15" />
- 在plan_manager/lunch/single_run_in_sim文件中，arg标签用于在启动文件中定义参数，这些参数可以在启动文件的后续部分被引用，以配置节点或其他组件的行为。
2.2 模块初始化，以规划可视化初始化为例
PlanningVisualization(ros::NodeHandle &nh);
- PlanningVisualization 类的一个构造函数，它接受一个 ros::NodeHandle 类型的引用作为参数
typedef std::shared_ptr<PlanningVisualization> Ptr;
- 这意味着 Ptr 现在可以用作 std::shared_ptr
的简短别名，使得代码更加简洁易读.PlanningVisualization 是一个自定义的类，用于执行某种规划算法的可视化。通过使用 std::shared_ptr 作为其智能指针类型。
PlanningVisualization::Ptr visualization_;
- 定义了一个指向PlanningVisualization类型对象的智能指针，名为visualization_
visualization_.reset(new PlanningVisualization(nh));
- reset方法用于将智能指针重新指向一个新的对象（或空指针），并释放它当前指向的对象（如果有的话）。创建一个新的PlanningVisualization对象。
2.3 定时回调函数
exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this);
- createTimer：这是NodeHandle类的一个成员函数，用于创建一个定时器。第一个参数表示定时器的时间间隔0.01秒。第二个参数是一个函数指针，指向当定时器到期时应该调用的回调函数。第三个参数（在这个例子中是this）是指向类实例的指针，用于在回调函数内部访问类的成员变量和成员函数。这是因为回调函数是类的成员函数，需要知道是哪个类的实例在调用它。在回调函数execFSMCallback中，根据不同的状态运行不同的程序。
2.4 飞行模式选择，如果是MANUAL_TARGET,就跳转到waypointCallback
if (target_type_ == TARGET_TYPE::MANUAL_TARGET)
{
waypoint_sub_ = nh.subscribe("/move_base_simple/goal", 1, &EGOReplanFSM::waypointCallback, this);
2.5 waypointCallback
Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, 1.0);
planNextWaypoint(end_wp);
- 从消息msg中获取坐标，并进行路标点规划planNextWaypoint
2.6 planNextWaypoint
success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
- 跳转到planGlobalTraj
2.7 planGlobalTraj
Eigen::Vector3d inter_pt = points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
- 如果两点距离大于设定值，使用贝塞尔曲线进行插值，中间插入点仍然为直线
gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
- 使用minimun snap优化
2.8 进入planNextWaypoint函数，改变状态机，从WAIT_TARGET变成GEN_NEW_TRAJ。waypointCallback函数运行结束。
2.9 init函数顺序执行结束，并每隔0.05秒进入回调函数的相应状态。
execFSMCallback状态机GEN_NEW_TRAJ
3.1 进入planFromGlobalTraj，主要调用callReboundReplan函数，运行成功则返回true。
3.2 进入callReboundReplan函数，首先运行getLocalTarget函数，返回局部目标点和局部点速度。
3.3 进入reboundReplan函数，先将多项式路径转换为B样条曲线，再用initControlPoints将控制点移除障碍物，随后进入BsplineOptimizeTrajRebound函数，该函数主要调用rebound_optimize函数。该函数主要调用costFunctionRebound，costFunctionRebound再调用combineCostRebound。此时，可以看到代码：
calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);
calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness);
calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);
- 分别用来计算光滑项，距离项和可行性。
3.4 在reboundReplan函数中继续运行，运行refineTrajAlgo函数，实现时间重分配。
3.5 回到callReboundReplan函数，bspline_pub_.publish(bspline)发布路径，在traj_server.cpp 中订阅该路径。