0. 先看效果
三种规划模式同时运行时，RViz 界面呈现的效果非常直观——绿色体素层代表规划实际使用的碰撞图，蓝色线条是 A* 搜索输出的 warm start 路径，紫红色曲线则是经 B-spline 优化后的最终轨迹。不同模式下的轨迹行为差异显著：UA V 模式可在空中自由穿越，Ground 模式必须紧贴地面支撑前进，而 2D 模式则将三维空间信息投影到平面上进行路径规划。
0.1 3D UA V 自由空间规划
无人机模式无需地面支撑，只要采样点位于地图边界内、不落入 occupied 体素、且满足膨胀半径要求，即可视为可通行区域。这是最灵活的规划模式，轨迹可以上下穿越楼层间的空隙，只要那片空间确实是 free 的。
3D UA V 自由空间规划
0.2 3D Ground 有地面支撑
地面机器人虽然运行在 3D 体素地图中，但不能将空中的 free space 当作可行道路。每个采样点都必须找到可靠的地面支撑结构，否则轨迹可能悬空或贴着楼梯边缘生成不可执行的路径。这一约束大幅缩小了搜索空间，但同时也导致连通性更容易断裂。
3D Ground 有地面支撑
0.3 2D Ground OccupancyGrid
2D ground 模式将 OctoMap 投影为 OccupancyGrid。只有同时满足 ground-supported 且具有 clearance 的格子才被标记为 free，其余区域为 unknown 或 occupied。关键点在于，2D 并非简单地将地图外接矩形全部视为可走平面——它只是将“地面可通行”这一三维判断压缩到 XY 平面上。
2D Ground OccupancyGrid
0.4 单测全绿
Core 层所有单元测试均通过，覆盖体素化、A* 搜索、B-spline 优化、碰撞检测、safety monitor 等核心模块。这是验证规划链路每一步都正确工作的基础验证手段。
单测全绿
1. 为什么要做这个
1.1 论文到工程的距离
EGO-Planner 是一篇非常优秀的论文，但从论文实现到能在自己地图上运行的工程化版本，中间还有很长的路要走。核心问题在于，论文关注的是算法正确性和对比实验，而工程需要的是可测试、可调试、可接入下游的完整链路。本项目的初衷非常纯粹——搞清楚这段路上每一步具体在做什么，不是读懂代码，而是亲手把它组装起来、跑通、测透。
1.2 具体目标
因此，本项目目标非常明确：输入是一份 PCD 点云文件，输出是 RViz 或 Web 界面中可见的平滑轨迹。中间的每一步——体素化、A* 搜索、B-spline 优化、局部避障、safety replan——都要有对应的测试和可观察的中间状态。规划核心以 ROS-free C++ 实现，再用 ROS2 bridge 和 Web UI 封装，这样验证链路足够短，无需整套 ROS 环境也能先跑通核心逻辑。
2. 整体链路
2.1 从点云到轨迹的六步
整条规划链路拆解来看，每一步的输入输出都非常清晰。PCD 点云先经过过滤和体素化，变成可查询的地图结构；然后 A* 或 JPS 在离散网格上搜索出一条拓扑可行的路径作为 warm start；B-spline 优化器利用 L-BFGS 将控制点推入可通行空间；最后由碰撞检测和 fallback FSM 兜底。整条链路的设计原则是：每个阶段的职责明确，可以单独测试和调试。
PCD 点云 → 点云过滤 + Voxel/OctoMap 构建 → 2D/3D A*（或 JPS）离散搜索
← warm start 来源
→ B-spline 控制点初始化
→ L-BFGS 优化（平滑 + 避障代价）
→ 局部 A* rebound cue ← 替代 ESDF 梯度
→ 二次轨迹碰撞采样检查
→ fallback FSM（缩短目标 / 急停）
→ ROS2 bridge → RViz / Web2.2 关键实现文件
每一步都有对应的代码模块支撑。bspline_optimizer.cpp 负责处理 warm start 和 L-BFGS 优化，包含 rebound cue 的生成逻辑；ego_planner_core.cpp 串联整条链路并执行 fallback FSM；safety_monitor.cpp 负责沿轨迹进行前向碰撞预警和急停决策。也就是说，Web、RViz、ROS topic 只是展示层，真正决定轨迹质量的是 core 中的地图语义、搜索结果、优化代价和碰撞复查。
3. ESDF 去哪了？
3.1 原版方案的代价
EGO-Planner 原版使用 ESDF（欧式符号距离场）为 B-spline 优化提供避障梯度。ESDF 的优势在于梯度信息连续、方向准确，但工程代价也很明显：需要维护全局距离场，每次地图更新都要重新计算距离传播，内存占用和计算量都不小。尤其在点云更新频繁、局部障碍不断进入视野的动态场景中，ESDF 既要更新距离场，又要提供稳定梯度，这两个需求本身存在矛盾。
3.2 局部 A* rebound cue
本项目中采用了一种更直接的方式。在 B-spline 优化的内层循环中，L-BFGS 检测到某个控制点位于 occupied 区域或 clearance 不足时，会从该控制点向最近 free 体素执行一次小范围 3D A* 搜索，将这条局部逃离路径的方向作为 rebound cue 注入代价函数。L-BFGS 继续优化，直到控制点回到可通行空间。具体实现位于 bspline_optimizer.cpp:335 的 buildReboundCues()：
// core/planner_core/src/bspline/bspline_optimizer.cpp:335
// buildReboundCues()：局部 A* 替代 ESDF 梯度
//
// 1. 遍历所有控制点，检测 occupied 或 clearance 不足
// 2. 对碰撞控制点启动小范围 3D A*（搜索半径受限）
// 3. A* 路径第一段方向 → rebound direction
// 4. 注入 L-BFGS 代价函数作为软约束3.3 工程取舍的直觉
直观理解：ESDF 梯度告诉你“几何上应该往哪里推”，而局部 A* cue 告诉你“从当前地图看，往哪里逃是通的”。在建筑、楼梯、狭窄走廊等离散结构明显的地图中，后者更容易与搜索结果保持一致。代价是梯度信息不再连续——每次 rebound cue 都是一次离散的局部重搜索，对高频动态障碍场景的梯度平滑性有所牺牲。
3.4 二次碰撞检查兜底
优化结束后，collision::TrajectoryChecker（ego_planner_core.cpp:300）会再进行一次连续采样碰撞检查。如果曲线在控制点之间翻越了薄障碍，这一步会将其拦截，而不是仅相信优化器的最终状态。若检查仍然失败，fallback FSM 依次尝试：保守 path-following → 提高 collision_weight → 缩短目标 → EmergencyStop。
4. A* + B样条 + L-BFGS，为什么不直接上 MPC
4.1 各阶段职责解耦
这个问题也曾纠结过。最终选择这套组合的原因在于各阶段职责清晰、便于分开调试。A* 提供拓扑可行的路径作为 B-spline 的 warm start，解决“从哪里走”的问题；L-BFGS 在光滑代价函数上收敛快，解决“怎么走得平滑”的问题；B-spline 的均匀节点向量天然保证 C² 连续，解决“轨迹能否被下游跟踪器执行”的问题。三步串联，每一步出问题都可以单独定位。
4.2 和 MPC 的本质差别
MPC 在每个控制周期都重新求解一个有限时域优化问题，对动态障碍反应最快，但计算开销高、调参复杂、可预测性差。本方案是一次规划产生完整轨迹，动态障碍通过事件触发的 safety replan 处理。这意味着对障碍的响应不是每帧进行，而是检测到碰撞风险时才介入。对于地图相对稳定、动态障碍仅为偶发事件的场景——比如仓库中的行人、临时放置的货物——这一取舍是合理的。
5. 动态障碍怎么处理
5.1 两个运行时输入
静态地图规划完成后，动态障碍通过两个 ROS topic 进入系统。/na v3d/current_pose 提供机器人当前位姿，/na v3d/local_pointcloud 提供局部感知到的新点云，用于更新 LocalGrid。这两个输入是运行时局部能力的基础——若不提供，系统只能执行静态地图上的规划结果，对环境变化完全无感知。
/na v3d/current_pose 当前机器人位姿（frame_id=map）
/na v3d/local_pointcloud 局部感知点云（更新 LocalGrid）5.2 SafetyMonitor 的判断逻辑
SafetyMonitor（safety_monitor.cpp）沿当前轨迹向前检查，根据 LocalGrid 的更新状态判断前方是否存在碰撞风险。近距离碰撞直接零速停机，发布 safety_emergency_stop；稍远的碰撞触发 safety_replan_needed，从当前位姿到 active goal 重新规划一条轨迹。重规划成功则发布新轨迹继续执行，失败则降级为急停。
local_pointcloud_updated → safety_replan_needed
├─ safety_replan_success → 从当前位姿重规划，发布新轨迹
└─ safety_replan_emergency_stop → 被阻断，零 cmd_vel5.3 验证这条链路
运行内置 smoke 脚本即可验证整条 safety replan 链路。脚本会模拟局部点云更新，触发 SafetyMonitor 判断，观察 /na v3d/status 的状态转换是否符合预期。添加参数可模拟机器人沿轨迹推进、每 N 秒在前方和左右生成一组世界坐标系的 sticky 障碍物：
# 基础验证
ros2 run na v3d_ros2_bridge na v3d_local_replan_smoke.py
# 完整模拟：机器人推进 + 周期障碍
ros2 run na v3d_ros2_bridge na v3d_local_replan_smoke.py \
--follow-trajectory --follow-speed 1.0 \
--follow-local-obstacle-count 4 --follow-local-side-offset 0.3
# launch 方式（默认每 5s 前方/左/右 spawn 3 个 cluster，5m 后丢弃）
ros2 launch na v3d_ros2_bridge na v3d_local_replan_smoke.launch.py \
pcd_path:="$pcd_path"6. Web 控制台
6.1 定位：RViz 之外的交互入口
除了 RViz，还有一个浏览器端控制台（React 19 + Three.js），通过 rosbridge websocket 连接 ROS2。它的定位是精确坐标输入和多地图切换的辅助工具——适合需要输入精确 XYZ 坐标、在浏览器中做 3D 选点、或者不想在 RViz 中操作的场景。Web 控制台不做任何本地规划计算，只显示 bridge 返回的真实轨迹。
# bridge 开启 rosbridge
ros2 launch na v3d_ros2_bridge na v3d_bridge.launch.py \
pcd_path:="$pcd_path" rosbridge:=true \
rosbridge_port:=9090 rviz:=false
# Web
cd web_ui && npm install && npm run dev -- --port 51736.2 路径分色和状态指示
打开 http://localhost:5173/，看到状态栏显示“已连接”即可开始使用。路径分为三色：全局路径（钴蓝）、局部路径（紫红，前方约 2.5 m）、已走轨迹（暗灰半透明，自动淡出）。顶部 HUD 显示 plan 状态 chip（完整/部分/失败/待规划），partial=true 表示系统找到了一个缩短后的次优落点，并不代表真正到达目标。
6.3 地图切换和选点
右侧 dock 提供地图下拉切换（预置 4 张参考地图），选中后向 /na v3d/load_pcd_path 发布 PCD 路径并清空当前规划。选点方式：先点击“起点”按钮，再在 3D 体素上点击一次，然后切换到“目标”再点击一次或多次。也可以直接使用 XYZ 数字输入精确坐标，适合需要对照特定位置反复测试的场景。
7. 总结
目前的代码是一个可测试的原型，并非完整的生产级 Na v2 controller server。partial=true 是明确的缩短目标 fallback，不应解释为到达 requested goal。运行时局部规划必须由下游持续提供 /na v3d/current_pose 和 /na v3d/local_pointcloud，若不提供则只能执行静态地图上的规划结果。后续可向真机迁移。