用BT实现的waypoints控制

1 起源

在nav2的导航控制中Nav2hunter的控制软件结构设计发现，路径规划不合理，所以需要采用waypoints来指定路径点，当时时间紧迫来不及学习调用nav2的waypoint 服务，直接采用自己的行为树来实现了路径点导航。

2 原来BT

原来的单目标点导航的BT图如下

分两步是为了先nav2导航到距离目标点2米处，在采用自己的位姿调整算法阿克曼转向车型导航末段位姿调整控制到目标点

3 增加路径点后的BT

在原来的BT前面增加了一个判断节点（一直运行直到失败），那就会逐个（按命名规则的顺序）去查找保存的点位，做为路径点，如果找不到路径点返回错误，则进入下一步就是原来的控制流程。
找到路径点后就把路径点作为nav2导航目标，调用Nav2Pose节点，使用Nav2导航到那个路径点。但是要注意的是Nav2导航有可能会失败（位置被障碍占据等），Nav2返回失败要让系统导航向下一个路径点，所以有一个强制成功，否则它的返回错误会向上到KeepRunningUntilFailure就会返回错误，这样就没法调出下一个路径点了。

4 运行情况