ros2 nav2 行为树插件引擎原理和应用

Nav2 行为树插件引擎原理和应用

本文由一个简单的例子作为切入点，对Nav2行为树插件引擎的原理进行分析。

一个完整应用demo

一个完整的应用demo包含以下工程和工具

• Groot行为树设计器和监视器
• 行为树运行库
• 行为树插件引擎
• 一个行为树action叶节点插件
• ROS2 action服务节点

Groot行为树设计和监视器

BehaviorTree.CPP具有结构简单易懂，和灵活等优点，非常适合做机器人的业务逻辑，可以从github中获取到源代码。

Groot是一套用于设计行为树以及实时监测行为树状态的辅助工具。通过Groot可以非常方便的实现拖拽式的编程以及快速定位到业务逻辑上的问题。

新版本的BehaviorTree.CPP及其Groot工具提供很多有用的特性，这是ROS2 dashing 所不具备的，所以建议从最新源代码安装。

安装

BehaviorTree.CPP依赖libzmq3-dev库，这是一个通讯库，如果没有安装libzmq3-dev，虽然可以编译通过Groot和BehaviorTree.CPP，但是却会阉割掉Groot实时监视功能。

$ sudo apt-get install libzmq3-dev libboost-dev
$ git clone https://github.com.cnpmjs.org/BehaviorTree/Groot.git

编译和安装Groot

$ cd Groot
$ git submodule update --init --recursive
$ mkdir build
$ cd build
$ cmake ..
$ make

使用Groot实时监视行为树

用于支持Groot实时监控的，实际是一个基于ZMQ实现的loggerBT::PublisherZMQ，Groot支持的logger包括：

• BT::StdCoutLogger
• BT::MinitraceLogger
• BT::FileLogger
• BT::PublisherZMQ

开启Groot实时监视功能需要通过下面3个步骤

1. 在行为树应用程序中初始化ZMQ日志用于支持Groot实时监控

BT::PublisherZMQ publisher_zmq(pTree);

其中，BT::PublisherZMQ的构造原型:

PublisherZMQ(const BT::Tree& tree,
             unsigned max_msg_per_second = 25,
             unsigned publisher_port = 1666,
             unsigned server_port = 1667);

对于其他三个参数max_msg_per_second, publisher_port和server_port一般不用修改，采用默认构造即可，如果想要自定义其他端口，则Groot实时监控端也要相应修改。

2. 运行Groot,再初始化界面选择Monitor，然后点击Start按钮

3. 进入Monitor模式的主界面，输入运行行为树的host的ip地址，ServerPort
   

如果端口和IP没有问题，并且行为树已经在运行，则会从目标端接收行为树结构并实时显示当前执行状态。否则右侧的行为树显示区域会一直空白。

行为树运行库

行为树运行库需要在tx2平台上运行，方便起见，直接使用已经搭建好开发环境的容器tx2-ros2-dev进行开发

从gitbub下载最新的行为树源代码

$ cd ~/workspace
$ git clone https://github.com.cnpmjs.org/BehaviorTree/BehaviorTree.CPP.git

在tx2-ros2-dev容器中编译和安装Beha