ROS1导航状态机与ROS2导航行为树

ROS1和ROS2导航框架中用到的各种底层算法基本相同，比如代价地图，全局路径规划和局部路径规划等，它们最大的不同在于整个系统框架设计。

一，ROS1 导航状态机

ROS1导航功能包move_base是一个状态机，从软件设计上来看，使用了典型的状态设计模式，如下图所示，机器人状态在PLANNING、CONTROLLING、CLEARING和FREE之间进行转换。

当收到NewGoal后，系统先进入PLANNING状态，进行全局路径规划，规划成功后进入CONTROLLING状态，进行局部路径规划，如果多次规划失败则进入CLEARING状态。在CONTROLLING状态下，如果失败时间不是很长，则会重新进入全局路径规划，如果在一个地方震荡，或者失败了很长一段时间，则系统也进入CLEARING状态，如果到点目标点，则进入FREE状态。 进入CLEARING后，机器人主要进行各种恢复(recovery)动作，包括原地旋转、清楚代价地图等，每成功进行一次恢复动作，系统重新进入PLANNING状态。

二，ROS2导航行为树

ROS2对整个导航框架进行了重写，引入了行为树对各个任务进行调度。行为树本质上也是一种有限状态机，但它极大地将软件进行了解耦，迎合松耦合、高内聚的软件设计思想。Ros2 导航到点的行为树如下：

ROS2导航行为树：https://navigation.ros.org/behavior_trees/index.html

行为树在结构上就是树形，有节点(此节点与ROS里的节点定义不一样，就是树状结构里的节点)的概念，根据功能，节点可分为：动作节点(Action Nodes)，条件节点(Condition Nodes)，装饰节点(Decorator Nodes)和控制节点(Control Nodes)，其中控制节点又分为序列控制器(PipelineSequence)、恢复控制器(Recovery)和循环控制器(RoundRobin)，这些分类，是将各种可能用到的处理逻辑抽象出来形成这些节点。

上图中蓝色方框代表一个动作节点，执行一个具体的动作，比例如ComputePathToPose执行全局路径规划，FollowPath执行局部路径跟随等；RateController是一个装饰器，表示系统会以该装饰器给定的频率调用其子树；绿色方块代表控制节点，黄色方框代表条件节点。

整个行为树顶层是一个Recovery控制节点，可分为左边导航子树和右边恢复子树，该类型控制节点表示，当左边导航子树返回SUCCESS，整个就是SUCCESS，左子树返回FAILED，则会触发右子树的Recovery尝试恢复，每进行一次Recovery，则重新执行一次左子树，直到下列条件满足则退出此次回复循环：
1， 右子树恢复次数达到number_of_retries次数；
2， 左子树返回SUCCESS；
3， 右子树返回FAILED。

NavigateWithReplanning是一个序列控制节点，它会先以RateController装饰节点规定的频率先执行左子树，待左子树返回SUCCESS则会执行右子树，其左右两个子树均是恢复节点，恢复节点的逻辑与前文介绍的一致，这样的序列表示只有全局规划成功了才可以执行局部路径跟随。

循环节点RoundRobin会从左到右依次执行各个节点，如果有一个返回SUCCESS，则该节点返回SUCCEESS，如果所有节点都返回FAILED，则该节点最终返回FAILED。

每一个ReactiveFallback控制节点左子树是一个条件节点GoalUpdated，表示如果目标被更新了，则该子树返回SUCCESS。

行为树通过自定义的节点和逻辑，使得调度逻辑和功能算法分开，减轻了功能算法过程中的相互依赖问题，更加方便团队合作开发。此外，ROS2中的行为树便于编辑，还可以在运行时进行可视化，方便开发者进行调试，详见链接ROS2中的行为树 BehaviorTree。