基于给定路径点的全局轨迹规划及可视化

基于给定路径点的全局轨迹规划及可视化解析

本文将详细解读 EGOReplanFSM::planGlobalTrajbyGivenWps() 函数中每一行代码的含义，该函数实现了根据一系列给定的路径点进行全局轨迹规划，并利用可视化工具展示规划结果的功能。

代码解析：

std::vector<Eigen::Vector3d> wps(waypoint_num_);: 创建一个名为 'wps' 的向量，用于存储所有路径点，向量大小由 'waypoint_num_' 变量决定，每个元素为三维向量 (Eigen::Vector3d)，表示路径点的三维坐标。
for (int i = 0; i < waypoint_num_; i++): 利用循环遍历每一个路径点。
wps[i](0) = waypoints_[i][0];: 将 'waypoints_' 数组中第 i 个路径点的 x 坐标赋值给 'wps' 向量中第 i 个元素的 x 坐标。
wps[i](1) = waypoints_[i][1];: 将 'waypoints_' 数组中第 i 个路径点的 y 坐标赋值给 'wps' 向量中第 i 个元素的 y 坐标。
wps[i](2) = waypoints_[i][2];: 将 'waypoints_' 数组中第 i 个路径点的 z 坐标赋值给 'wps' 向量中第 i 个元素的 z 坐标。
bool success = planner_manager_->planGlobalTrajWaypoints(odom_pos_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), wps, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());: 调用 'EGOPlannerManager' 类中的 'planGlobalTrajWaypoints()' 函数，该函数根据当前位置 ('odom_pos_')、路径点 ('wps') 等参数进行全局路径规划，并将规划结果存储在 'success' 变量中，用于表示规划是否成功。
for (size_t i = 0; i < (size_t)waypoint_num_; i++): 再次循环遍历每个路径点。
visualization_->displayGoalPoint(wps[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i);: 调用 'PlanningVisualization' 类中的 'displayGoalPoint()' 函数，在可视化界面中将路径点 'wps[i]' 显示为指定颜色和大小的目标点。
ros::Duration(0.001).sleep();: 程序休眠 0.001 秒，确保目标点在可视化界面中能够被看到。
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);: 计算全局轨迹的持续时间除以时间步长后的整数部分，得到全局轨迹的离散点数。
std::vector<Eigen::Vector3d> gloabl_traj(i_end);: 创建一个名为 'gloabl_traj' 的向量，用于存储全局轨迹上的所有离散点，向量大小为 'i_end'，每个元素为三维向量，表示轨迹点的坐标。
for (int i = 0; i < i_end; i++): 循环遍历每个离散点。
gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(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')。
ros::Duration(0.001).sleep();: 程序休眠 0.001 秒，确保后续可视化内容能够被看到。
visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);: 调用 'PlanningVisualization' 类中的 'displayGlobalPathList()' 函数，在可视化界面中将全局轨迹 'gloabl_traj' 显示为指定大小和颜色的路径。
ros::Duration(0.001).sleep();: 程序休眠 0.001 秒，确保路径能够在可视化界面中被看到。

总结：

EGOReplanFSM::planGlobalTrajbyGivenWps() 函数实现了根据给定的路径点进行全局轨迹规划，并利用可视化工具展示规划结果的功能，函数内部包含了路径点读取、全局路径规划、轨迹生成、状态切换以及可视化等步骤，清晰地展现了基于路径点的全局轨迹规划流程。