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容器中编译和安装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包

自定义的action插件包含以下内容：

• 自定义的action消息
• 用于动态加载的叶节点插件
• 用于提供功能实现的ros2action服务节点

自定义Ros 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_generatorsbuildtool_depend>
<depend>action_msgsdepend>
<exec_depend>rosidl_default_runtimeexec_depend>
<member_of_group>rosidl_interface_packagesmember_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叶节点插件

构成一个Action叶节点插件的要素如下：

• ActionClient ROS2节点
• 使用BT_REGISTER_NODES宏，导出一个Action构造器。

#include 

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插件，请自行探索

ROS2 action服务节点

行为树action叶节点本身是一个ROS2 action client节点，其只是调用具体的功能，本身并不实现具体功能，ROS2 action服务节点为行为树action叶节点提供服务。

#include 

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)

行为树action与ROS2 action相结合实现功能和逻辑的解耦

一般的ROS2中一个action的执行步骤可以概括如下：

1. 一个Action包含3个成员：Goal,Feedback和Result。
2. Client向Server节点发起一个Goal请求，并等待Server应答结果。
3. Client在收到Server回复的Goal后，进入到Action执行状态，并向Server发出一个Result请求，并等待Server返回Result。
4. 由于Server执行Action可能会比较耗时，所以会定期的发布Feedback话题以便Client及时获知当前Action执行状态。
5. 当Server执行完Action后，会返回Result，Client端收到Result即完成了一个Action传输流程。

而在Nav2行为树引擎中结合了行为树和ROS2 action机制

其流程概括如下：

1. 行为树控制业务逻辑，Action Server 提供具体功能的实现。
2. 行为树加载完成后，业务逻辑从Root节点陆续执行子节点，
3. 运行到Wait Action叶节点时，通过ROS2的Action机制，使得提供相应服务实现的Action节点执行服务。
4. Action服务节点执行过程中，行为树对应的叶节点处于“运行中”状态。
5. 当Action服务节点执行完成，行为树对应的叶节点运行完，并继续按照行为树运行。