EGOReplanFSM::waypointCallback 函数代码详解 - ROS路径规划 - 常规

以下是void EGOReplanFSM::waypointCallback(const nav_msgs::PathConstPtr &msg)函数中每一行代码的详细解释：

if (msg->poses[0].pose.position.z < -0.1) return;: 检查接收到的路径点消息中第一个路径点的z坐标是否小于-0.1，如果是则直接返回，不进行处理。
cout << 'Triggered!' << endl;: 输出调试信息，表示路径点消息已触发。
trigger_ = true;: 将触发标志trigger_设置为true，表示已经触发了路径点消息。
init_pt_ = odom_pos_;: 将当前的里程计位置odom_pos_赋值给初始位置init_pt_。
bool success = false;: 创建一个布尔变量success并初始化为false，用于记录路径规划是否成功。
end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0;: 将接收到的路径点消息中第一个路径点的x、y坐标分量赋值给终点end_pt_，并将z坐标设置为1.0。
success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());: 调用EGOPlannerManager类中的planGlobalTraj()函数，根据给定的起始位置、终点和其他参数进行全局路径规划，并将结果赋值给success变量。
visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);: 通过调用PlanningVisualization类中的displayGoalPoint()函数，将终点以指定的颜色和大小显示为目标点。
if (success) { ... }: 如果路径规划成功，则执行下面的代码块。
constexpr double step_size_t = 0.1;: 创建一个常量step_size_t，表示每个时间步长的大小。
int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);: 计算全局轨迹的时间长度除以步长step_size_t的整数部分，得到全局轨迹的点数。
vector<Eigen::Vector3d> gloabl_traj(i_end);: 创建一个大小为i_end的Eigen::Vector3d类型的向量gloabl_traj，用于存储全局轨迹的点。
for (int i = 0; i < i_end; i++) { ... }: 循环遍历每个点。
gloabl_traj[i] = planner_manager_->global_data_.getPosition(i * step_size_t);: 将全局轨迹在时间点i * step_size_t处的位置赋值给gloabl_traj向量中的第i个元素。
end_vel_.setZero();: 将终点的速度end_vel_设置为零向量。
have_target_ = true;: 将have_target_标志设置为true，表示已有目标。
have_new_target_ = true;: 将have_new_target_标志设置为true，表示有新的目标。
changeFSMExecState(GEN_NEW_TRAJ, 'TRIG');: 调用changeFSMExecState()函数，将执行状态改变为GEN_NEW_TRAJ，并打印出触发原因为'TRIG'的状态改变。
visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);: 通过调用PlanningVisualization类中的displayGlobalPathList()函数，将全局轨迹gloabl_traj以指定的大小和颜色显示为路径。

该函数的主要作用是处理收到的路径点消息，并根据路径点进行全局路径规划，并在可视化中显示目标点和路径。

EGOReplanFSM::waypointCallback 函数代码详解 - ROS路径规划