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

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

文章目录

Nav2 行为树插件引擎原理和应用
- 一个完整应用demo
- - Groot行为树设计和监视器
  - - 安装
    - 使用Groot实时监视行为树
  - 行为树运行库
  - 行为树插件引擎
  - 自定义action包
  - - 自定义Ros action
    - 行为树action叶节点插件
    - ROS2 action服务节点
- 行为树action与ROS2 action相结合实现功能和逻辑的解耦

一个完整应用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个步骤

在行为树应用程序中初始化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实时监控端也要相应修改。

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

进入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_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叶节点插件

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

ActionClient ROS2节点
使用BT_REGISTER_NODES宏，导出一个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(WaitActionBtNoderclcpprclcpp_actionbehavior_tree_engine)

注意：编译插件时，要定义宏BT_PLUGIN_EXPORT，否则生成的插件无法被动态加载，原因参照这篇文章。

拓展

除了action之外，还有condition，control和decorator插件，请自行探索

ROS2 action服务节点

行为树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(WaitActionServerNoderclcpprclcpp_action)

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

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

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

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

其流程概括如下：

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

ros2 nav2 行为树插件引擎原理和应用相关推荐

ROS2+nav2+激光雷达导航(上)
目录写在前面安装nav2 nav2介绍 nav2实践 map 地图保存地图读取状态估计(TF变换) 所需TF坐标 base_link -> sensor frames odom -> ...
osgEarth的Rex引擎原理分析（一二六）rex瓦片组织方式
目标:(一二五)中问题212 通过如下确定瓦片的组织方式 ,核心是profile osgEarth/Map.cpp void Map::calculateProfile() {// collect t ...
Android 插件化原理学习 —— Hook 机制之动态代理
前言为了实现 App 的快速迭代更新,基于 H5 Hybrid 的解决方案有很多,由于 webview 本身的性能问题,也随之出现了很多基于 JS 引擎实现的原生渲染的方案,例如 React Nat ...
【Android 插件化】插件化原理 ( 类加载器 )
Android 插件化系列文章目录 [Android 插件化]插件化简介 ( 组件化与插件化 ) [Android 插件化]插件化原理 ( JVM 内存数据 | 类加载流程 ) [Android 插件 ...
HashMap底层实现原理，红黑树，B+树，B树的结构原理，volatile关键字，CAS（比较与交换）实现原理
HashMap底层实现原理,红黑树,B+树,B树的结构原理,volatile关键字,CAS(比较与交换)实现原理首先HashMap是Map的一个实现类,而Map存储形式是键值对(key,value) ...
11.InfluxDB引擎原理
11.InfluxDB引擎原理转自:https://www.yisu.com/zixun/36153.html 11.1.引言 InfluxDB是一款Go语言写的时序数据库.时序数据库主要用于存储基 ...
hbase 中的LSM树存储引擎
LSM的原理:将对数据的修改增量保存在内存中,达到指定大小限制之后批量把数据flush到磁盘中,磁盘中树定期可以做merge操作,合并成一棵大树,以优化读性能.不过读取的时候稍微麻烦一些,读取时看这些 ...
【Unity入门教程】第八章人工智能【中国大学MOOC游戏引擎原理及应用】
以下均为来自中国大学mooc 游戏引擎原理及应用时的学习笔记,不含商用,仅供学习交流使用,如果侵权请联系作者删除. 文章目录 8.1 自动寻路 8.2 巡逻 8.3 视野 8.4 自动攻击 8.5 追 ...
交易所撮合引擎原理及实现代码
交易撮合引擎(Matching/Trading Engine),顾名思义是用来撮合交易的软件,广泛地应用在金融.证券.加密货币交易等领域.交易引擎负责管理加密资产市场中所有的开口订单(Open Ord ...
Nmap脚本引擎原理
转载自博客园,随风浪子~的文章:nmap脚本引擎原理http://www.cnblogs.com/liun1994/p/7041373.html Nmap脚本引擎原理 Nmap脚本引擎原理一.NSE ...

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