# Nav2 controller_server长路径每周期耗时随剩余位姿数线性增长

> 原文链接：https://robotics.hellobike.com/pitfalls/2026-11-10-navigation2-6759
> 频道：pitfalls | 更新日期：2026-11-10 | 标签：auto, github-issue, navigation2
> 核心摘要：Nav2控制器在处理超长全局路径时单周期耗时线性增加，建议改用累积长度表计算剩余距离并封装结构体管理路径裁剪以优化性能。

# Nav2 controller_server长路径每周期耗时随剩余位姿数线性增长

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[controller_server: per-cycle cost grows linearly with remaining plan length (FeasiblePathHandler prune erase, remaining_path_length)](https://github.com/ros-navigation/navigation2/issues/6759)（GitHub ros-navigation/navigation2 #6759）

**状态**：有绕过办法

## 问题

Nav2 `controller_server` 在处理长全局路径（如40万位姿的覆盖路径）时，每控制周期耗时随剩余位姿数线性增长。实测400k位姿时单周期额外开销达7.21ms。瓶颈在于：1. `FeasiblePathHandler::prunePlan()` 使用 `std::vector::erase` 导致内存搬移；

## 环境

Nav2 1.5.2 (Lyrical apt binaries), x86-64 i7-10700K (单核绑定), max_robot_pose_search_dist: 10.0, 机器人每周期前进5cm

## 解决办法

1. 针对 `remaining_path_length` 计算：在 `setPlannerPath()` 中一次性构建累积长度表，运行时通过 `total - prefix[start_index_]` 获取剩余长度，将复杂度从 O(N) 降为 O(1)。
2. 针对路径裁剪：封装包含路径、起始偏移量和累积表的专用结构体，提供统一 `prune()` 方法同步更新状态，避免直接对 `std::vector` 执行 erase 操作。
3. 保持 `PathIterator` 和 `PathHandler` 接口不变，利用已分配内存空间换取CPU性能，避免引入 `std::deque` 导致的类型不兼容或额外拷贝开销。
4. 维护者确认该方案为合理的内存换CPU权衡，可作为PR提交审查。

讨论原作者：@liltaket 等，版权归原作者所有。
:::

## 原始参考来源 (Sources)

- [GitHub ros-navigation/navigation2 #6759：controller_server: per-cycle cost grows linearly with remaining plan length (FeasiblePathHandler prune erase, remaining_path_length)](https://github.com/ros-navigation/navigation2/issues/6759)