Navigation2中PathLongerOnApproach行为树插件的路径更新检测问题分析

在ROS2 Navigation2导航系统的行为树插件中，PathLongerOnApproach插件负责检测路径是否更新的功能存在一个值得注意的实现问题。这个问题会影响导航系统对路径变化的判断准确性，进而影响整个导航行为的可靠性。

问题背景

PathLongerOnApproach插件的主要功能是比较新旧路径，判断路径是否发生了更新。在实现上，它通过比较新旧路径的最后一个位姿点来判断路径是否更新。然而，当前的实现方式存在一个潜在的问题点。

问题分析

在现有的代码实现中，路径更新检测使用了完整的PoseStamped比较，这包括了头部信息(header)和位姿(pose)两部分。具体来说，比较逻辑如下：

1. 首先检查新旧路径是否不同
2. 然后确认新旧路径都不为空
3. 最后比较新旧路径最后一个位姿点的PoseStamped是否相同

问题出在第三步的比较上。PoseStamped的比较不仅包含位置信息，还包含了时间戳信息。在实际应用中，即使两条路径的几何形状完全相同，由于生成时间不同，它们的时间戳也会不同，导致比较结果为false。

技术细节

PoseStamped的比较运算符实现会检查两个部分：

• 头部信息(header)比较，包括时间戳
• 位姿(pose)比较，包括位置和方向

在导航系统运行过程中，即使路径的几何形状没有变化，由于路径规划器每次生成的路径都会带有新的时间戳，这会导致PathLongerOnApproach插件误判路径没有更新。

解决方案

正确的实现应该只比较位姿(pose)部分，而不比较头部信息。具体修改方案是将： old_path.poses.back() == new_path.poses.back() 改为： old_path.poses.back().pose == new_path.poses.back().pose

这样修改后，插件将只关注路径的几何形状变化，而不受时间戳变化的影响，从而更准确地检测路径是否真正更新。

影响范围

这个问题特别会影响使用SMAC规划器等会频繁更新路径时间戳的规划算法。值得注意的是，在使用NavFn规划器时这个问题可能不明显，因为NavFn规划器生成的路径时间戳保持不变。

结论

路径更新检测是导航系统中的重要功能，确保其准确性对导航性能至关重要。通过修正PoseStamped的比较方式，可以使PathLongerOnApproach插件在各种规划器下都能可靠工作，提高导航系统的鲁棒性。这个问题的修复已经通过社区贡献的方式完成，体现了开源协作的优势。