本文由一个简单的例子作为切入点，对Nav2行为树插件引擎的原理进行分析。
一个完整的应用demo包含以下工程和工具
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实时监控的，实际是一个基于ZMQ实现的loggerBT::PublisherZMQ，Groot支持的logger包括：
BT::StdCoutLoggerBT::MinitraceLoggerBT::FileLoggerBT::PublisherZMQ开启Groot实时监视功能需要通过下面3个步骤
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实时监控端也要相应修改。
如果端口和IP没有问题，并且行为树已经在运行，则会从目标端接收行为树结构并实时显示当前执行状态。否则右侧的行为树显示区域会一直空白。
行为树运行库需要在tx2平台上运行，方便起见，直接使用已经搭建好开发环境的容器tx2-ros2-dev进行开发
从gitbub下载最新的行为树源代码
$ cd ~/workspace
$ git clone https://github.com.cnpmjs.org/BehaviorTree/BehaviorTree.CPP.git
在tx2-ros2-dev容器中编译和安装BehaviorTree.CPP
$ cd BehaviorTree.CPP
$ git submodule update --init --recursive
$ mkdir build
$ cd build
$ cmake -DCMAKE_TOOLCHAIN_FILE=~/workspace/ros2/toolchain.cmake ..
$ make install
执行完make install之后，构建生成头文件和运行库将会安装到/home/admin/workspace/ros2/install
行为树插件引擎可以动态加载xml行为树描述文件和叶节点插件，从而实现在运行时加载叶节点和构造行为树。
动态加载叶节点的关键函数：
void BT::BehaviorTreeFactory::registerFromPlugin(const std::string &file_path);
其中file_path为叶节点插件动态库的路径如/home/nvidia/sysctrl/libWaitActionBtNode.so。
动态加载叶节点代码如下：
BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries)
{
BT::SharedLibrary loader;
for (const auto& p : plugin_libraries) {
factory_.registerFromPlugin(loader.getOsName(p));
}
}
动态构建行为树函数原型：
void BT::XMLParser::loadFromText(const std::string& xml_text);
void BT::XMLParser::loadFromFile(const std::string& filename);
BehaviorTree.CPP支持从文件和xml文本字符串构建行为树。
构建代码如下：
BT::Tree
BehaviorTreeEngine::buildTreeFromText(
const std::string & xml_string,
BT::Blackboard::Ptr blackboard)
{
BT::XMLParser p(factory_);
p.loadFromText(xml_string);
return p.instantiateTree(blackboard);
}
自定义的action插件包含以下内容：
创建一个ROS2 package wait_action
在包目录中创建自定义action文件action/Wait.action
其内容如下：
builtin_interfaces/Duration time
---
builtin_interfaces/Duration total_elapsed_time
---
builtin_interfaces/Duration time_left
修改package.xml文件，添加以下内容
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
修改CMakeLists.txt文件，添加以下内容
find_package(rclcpp_action REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/wait.action"
)
ament_export_dependencies(rosidl_default_runtime)
构成一个Action叶节点插件的要素如下：
#include <wait_action/action/Wait.hpp>
class WaitAction: public BtActionNode<wait_action::action::Wait>;
BT_REGISTER_NODES(factory)
{
BT::NodeBuilder builder = []
(const std::string & strName, const BT::NodeConfiguration & config)
{
return std::make_unique<WaitAction>(strName, "wait", config);
};
factory.registerBuilder<WaitAction>("wait", builder);
}
CMakeLists.txt增加如下内容
add_library(WaitActionBtNode SHARED src/WaitAction.cpp)
rosidl_target_interfaces(WaitActionBtNode ${PROJECT_NAME} "rosidl_typesupport_cpp")
target_compile_definitions(WaitActionBtNode PUBLIC BT_PLUGIN_EXPORT)
ament_target_dependencies(WaitActionBtNode
rclcpp
rclcpp_action
behavior_tree_engine
)
注意：编译插件时，要定义宏BT_PLUGIN_EXPORT，否则生成的插件无法被动态加载，原因参照这篇文章。
拓展
除了action之外，还有condition，control和decorator插件，请自行探索
行为树action叶节点本身是一个ROS2 action client节点，其只是调用具体的功能，本身并不实现具体功能，ROS2 action服务节点为行为树action叶节点提供服务。
#include <wait_action::action::Wait.hpp>
class WaitActSrv: public ActionServerNode<wait_action::action::Wait>;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto wait_act_srv = std::make_shared<WaitActSrv>();
rclcpp::spin(wait_act_srv);
rclcpp::shutdown();
return 0;
}
修改CMakeLists.txt添加如下内容：
add_executable(WaitActionServerNode src/WaitActionServerNode.cpp)
rosidl_target_interfaces(WaitActionServerNode ${PROJECT_NAME} "rosidl_typesupport_cpp")
ament_target_dependencies(WaitActionServerNode
rclcpp
rclcpp_action)
一般的ROS2中一个action的执行步骤可以概括如下：
而在Nav2行为树引擎中结合了行为树和ROS2 action机制
其流程概括如下：