这里说的是官方教程中的拆分，实际开发中，如果行为树太大，应该根据程序逻辑拆分成几个xml文件，不同子逻辑加载不同的xml

拆分行为树

拆分行为树有两种方法，我倾向用不修改C++的方法

main_tree.xml如下

1
2
3
4
5
6
7
8
9
10
11

<root main_tree_to_execute = "MainTree">
    <include path="./subtree_A.xml" />
    <include path="./subtree_B.xml" />
    <BehaviorTree ID="MainTree">
        <Sequence>
            <SaySomething message="starting MainTree" />
            <SubTree ID="SubTreeA" />
            <SubTree ID="SubTreeB" />
        </Sequence>
    </BehaviorTree>
<root>

1
2
3
4
5
6
7
8
9
10
11

<root>
    <BehaviorTree ID="SubTreeA">
        <SaySomething message="Executing Sub_A" />
    </BehaviorTree>
</root>

<root>
    <BehaviorTree ID="SubTreeB">
        <SaySomething message="Executing Sub_B" />
    </BehaviorTree>
</root>

include path可以用绝对路径，也可以用相对路径。

1

factory.createTreeFromFile("main_tree.xml")

拆分后，用Groot2打开main_tree.xml，可以正常编辑和跳转到子树。

函数Tree BT::BehaviorTreeFactory::createTreeFromFile写的不好，加载xml失败也不知道，最好返回类型是Bool，false代表加载失败。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

Tree BehaviorTreeFactory::createTreeFromFile(const std::string& file_path,
                                              Blackboard::Ptr blackboard)
 {
   if(!parser_->registeredBehaviorTrees().empty()) {
     std::cout << "WARNING: You executed BehaviorTreeFactory::createTreeFromFile "
                  "after registerBehaviorTreeFrom[File/Text].\n"
                  "This is NOTm probably, what you want to do.\n"
                  "You should probably use BehaviorTreeFactory::createTree, instead"
               << std::endl;
   }
 
   XMLParser parser(*this);
   parser.loadFromFile(file_path);
   auto tree = parser.instantiateTree(blackboard);
   tree.manifests = this->manifests();
   return tree;
 }

虽然stack和queue中也可以存放元素，但在STL中并没有将其划分在容器的行列，而是将其称为容器的适配器，这是因为stack和queue只是对其他容器的接口进行了包装，STL中stack和queue默认使用deque。

适配器其实是一种设计模式，该种模式是将一个类的接口转换成用户希望的另外一个接口。

deuqe平时很少作为单独的数据结构容器来使用，而是仅仅作为stack和queue等数据结构的适配容器。

stack是一种后进先出的特殊线性数据结构，因此只要具有push_back()和pop_back()操作的线性结构，都可以作为stack的底层容器，比如vector和list都可以；queue是先进先出的特殊线性数据结构，只要具有push_back和pop_front操作的线性结构，都可以作为queue的底层容器，比如list。但是STL中对stack和queue默认选择deque作为其底层容器，主要是因为：

1. stack和queue不需要遍历(因此stack和queue没有迭代器)，只需要在固定的一端或者两端进行操作。

2. 在stack中元素增长时，deque比vector的效率高(扩容时不需要搬移大量数据)；queue中的元素增长时，deque不仅效率高，而且内存使用率高。

deque支持stack和queue的全部操作，因此，只需要在stack和queue接口的实现中调用deque容器对应的接口，就可以完成对stack和deque的模拟实现。

${CMAKE_CURRENT_LIST_DIR} 和 ${CMAKE_CURRENT_SOURCE_DIR} 一般都是当前CMakeLists.txt所在的目录。

file (GLOB_RECURSE SOURCE_FILES ${CMAKE_CURRENT_LIST_DIR}/*.cpp) 把路径下的所有cpp文件名都加入变量SOURCE_FILES

C++中的优先队列（priority queue）是一种特殊的队列，它允许在队列中添加元素时自动根据元素的优先级进行排序，
以便能够快速访问具有最高优先级的元素。优先队列可以用来解决很多算法问题，例如Dijkstra算法、Prim算法等。

底层实现是二叉堆，特别是二叉最小堆（对于最大优先级队列）或二叉最大堆（对于最小优先级队列）。堆是一种特殊的完全二叉树，其中任一节点的值都不大于或不小于其子节点的值。这种属性让堆非常适合快速访问最大或最小元素。

• push() 基于优先级在适当的位置加入新的元素
• top() 返回最高优先级的元素，但不删除该元素
• pop() 删除最高优先级的元素
• size() 返回队列中元素的个数
• empty() 如果队列为空返回true，否则返回false

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

#include <queue>
#include <iostream>
using namespace std;

int main()
{
    priority_queue<int> pq;
    pq.push(3);
    pq.push(1);
    pq.push(4);
    pq.push(1);
    pq.push(5);

    while (!pq.empty()) {
        int top = pq.top();
        pq.pop();
        cout << top << " ";
    }
    // 如果要保存队列中的所有元素，可以将它们复制到另一个数据结构中，例如vector或数组
    return 0;
}

优先队列不支持随机访问，只允许访问队头元素，不允许访问其余的数据

创建包，相关文件

创建C++包: ros2 pkg create --build-type ament_cmake --node-name node_name package_name

指明依赖项: ros2 pkg create --build-type ament_cmake --node-name node_name package_name --dependencies rclcpp std_msgs

创建python包： ros2 pkg create --build-type ament_python --node-name node_name package_name

如果在创建包时，忘了指定依赖项，需要手动修改package.xml 和 CMakeLists.txt

比如在package.xml 的 <buildtool_depend> 之后添加

1
2

<depend>rclcpp</depend>
<depend>std_msgs</depend>

1
2
3
4

find_package(rclcpp  REQUIRED)
find_package(std_msgs  REQUIRED)

ament_target_dependencies(my_node rclcpp std_msgs)

工作空间

• build目录放的是中间文件．例如调用CMake时，每个包都会生成一个子文件夹．

• install目录是放包的安装文件．默认情况下，每个包会被安装到一个独立的子目录．

• log目录包含每次执行colcon的各种日志信息．

对比catkin，没有了devel目录

package.xml中包含该功能包的依赖信息，它可以帮助编译工具colcon确定多个功能包编译的顺序。

CMakeList.txt当中，必须有

1
2

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)

find_package(rclcpp_lifecycle REQUIRED)用于生命周期管理，平时可以不添加

例如package名称为project(first_node)，必须修改的部分如下

1
2
3
4
5

add_executable(test_node src/test.cpp)
ament_target_dependencies(test_node rclcpp std_msgs)
install(TARGETS test_node DESTINATION lib/install_node)

target_link_libraries(test_node  behaviortree_cpp_v3)

• add_executable不必解释

• ament_target_dependencies是官方推荐的方式去添加依赖项。它将使依赖项的库、头文件和自身的依赖项被正常找到。如果不添加库名称，include头文件时会找不到

• install是安装库的语句。它将在工作空间生成文件 install/first_node/lib/install_node/test_node

• ament_package() 最后一句，不要修改。项目安装是通过ament_package()完成的，并且每个软件包必须恰好执行一次这个调用。ament_package()会安装package.xml文件，用ament索引注册该软件包，并安装CMake的配置（和可能的目标）文件，以便其他软件包可以用find_package找到该软件包。由于ament_package()会从CMakeLists.txt中收集大量信息，因此它应该是CMakeLists.txt中的最后一个调用。

可选项

ament_export_dependencies(${dependencies})

这句会将依赖项导出到下游软件包。这样该库使用者也就不必为那些依赖项调用find_package了。

问题

1. ROS2 ConnectionResetError: [Errno 104] Connection reset by peer

执行ros2 node list遇到的错误，有一大堆信息，这是最后一句。按如下步骤解决:

a. 检查守护进程是否确实在运行

ros2 daemon status 或者 systemctl status ros2-daemon.service

b. 重新启动守护进程

如果守护进程没有运行，尝试启动它: ros2 daemon start

然后再次检查状态: ros2 daemon status。现在ros2 node list正常了。

尽量不要用行为树，除非公司里有个人，本来就很擅长行为树，那么可以交给他，其他人没必要参与了。行为树入门难，原因不是它本身多么复杂，而是网上的资料太少，尤其groot的资料更少，全靠自己摸索，一个并不复杂的问题，摸索一整天。有不少资料是游戏设计方面的，用于机器人领域的开源方案目前只有ROS2。资料这么少，肯定是有原因的。

最近从行为树版本3切换到版本4，发现有一些变化，又走了一遍坑之后，我还是认为最好不要用行为树。

准确地说，是BehaviorTree.CPP和Groot太垃圾，行为树本身在游戏行业已经用了很多年，不需要讨论其可用性。

行为树的缺点

• 一个简单的操作都需要定义Action或Condition节点，行为树很容易做的复杂化
• 每次从根节点开始逻辑，若树的很庞大，效率将变得低下，占用CPU更多，注意行为树的刷新频率

• 黑板变量，有的人在XML文件里赋值，有的人在C++里赋值，我如果用groot2看行为树，无法看到C++里赋值的情况

• 有的人会自己定义控制节点和修饰节点，然后使用。其实默认的节点也能实现相同的逻辑。其他人不得不先看这些节点的定义，造成行为树的可读性不好。

• Timeout节点疑似不能生效

网上找不到修饰节点Timeout的使用方法，从API里找到了说明：The TimeoutNode will halt() a running child if the latter has been RUNNING for more than a give time. The timeout is in milliseconds and it is passed using the port “msec”.

但是我反复试验，发现这个节点总不生效，它修饰的节点运行时间早就超过Timeout规定时间了，但是不会停止

安装

安装依赖项，然后去github的release里下载最新的版本

1
2
3
4
5

sudo apt-get sqlite3
sudo apt-get install libzmq3-dev libboost-dev
sudo apt-get install libboost-coroutine-dev # 需要用到协程

sudo apt-get install qtbase5-dev libqt5svg5-dev libzmq3-dev libdw-dev

cmake老三样编译安装。

普通的cmake设置如下

1
2
3
4
5
6
7
8
9
10

cmake_minimum_required(VERSION 3.10.2)
 
project(simple_bt)
 
set(CMAKE_CXX_SaTANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(behaviortree_cpp)
 
add_executable(${PROJECT_NAME} "simple_bt.cpp")
target_link_libraries(${PROJECT_NAME} BT::behaviortree_cpp)

ROS2环境的设置如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

cmake_minimum_required(VERSION 3.8)
project(test_node)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_CXX_STANDARD 17)

add_compile_options(-Wextra -Wpedantic -Wno-unused-parameter -g)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(ament_index_cpp)
find_package(behaviortree_cpp)

INCLUDE_DIRECTORIES(/home/user/catkin_ws/src/test_node/include)

add_executable(test_node  src/test.cpp)

ament_target_dependencies(test_node   
  rclcpp
  behaviortree_cpp
    ${BTCPP_LIBRARY}
)

install(TARGETS
  test_node
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

package.xml如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>test_node</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="user@todo.todo">zzp</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>behaviortree_cpp</depend>
  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

行为树的优点:

• 使用groot编辑，直观容易理解
• 行为逻辑和状态数据分离(减少了耦合)，任何节点写好以后可以反复利用
• 重用性高，可用通过重组不同的节点来实现不同的行为树
• 线性的方式扩展，所以扩展性好
• 黑板变量用起来很灵活

缺点:

• 任何一个简单的操作都需要使用节点
• 每次从根节点开始逻辑，占用CPU更多

行为树本身并不具体实现机器人的执行内容，它只负责将执行内容进行编排。以Navigation2为例，具体的执行内容实现是放在各个server中的。行为树上的节点与server进行通信，请求具体的执行内容，然后获得反馈。根据反馈结果又可以请求另外的执行内容。这些不同执行内容间的跳转就是由行为树控制的。

另一种比较常见的组织机器人行为的方式是状态机。ROS1中的move_base就是基于状态机的。它与行为树最显著的区别是状态与执行内容是绑定在一起的。当执行内容需要在多个状态中执行时，各个状态下都需要放置执行内容的逻辑。当业务逻辑代码分散在各处时就不太好维护了

ROS2行为树动态库默认安装在/opt/ros/galactic/lib/libbehaviortree_cpp_v3.so，头文件在/opt/ros/foxy/include/behaviortree_cpp_v3

xml 文件

xml格式有以下几个要点

• root这个tag是必须的，而且它需要有main_tree_to_execute的属性，表示执行哪颗树
• root的子元素必须有BehaviorTree，BehaviorTree必须有ID属性
• 如果root拥有多个BehaviorTree，那么BehaviorTree的ID的属性值必须是不同的，此时必须指定main_tree_to_execute标签
• 如果root只有一个BehaviorTree，那么main_tree_to_execute属性可有可无
• BehaviorTree的子元素就是树节点（tree nodes）
• TreeNodesModel标签，主要用于Groot可视化。对于C++程序来说，可以没有。

createTreeFromFile的文件路径错误时，会报错 what(): Internal error in realpath(): No such file or directory

调试

行为树的各种调试工具

Groot1的日志工具很不好用，在fbl文件比较大的时候，很容易卡死。如果总的行为树很大，需要放大才能观看流程，十分不方便。Groot2的日志工具据说很好用，但是收费。

拆分行为树后，在Groot里不方便直接打开子树，只能再开一个Groot打开子树的xml文件. 后来Groot2解决了这个问题，在设置的Editor标签里还可设置是否每次都加载<include>标签对应的文件。

统一的代码规范对于整个团队来说十分重要，通过git/svn在提交前进行统一的 ClangFormat 格式化，可以有效避免由于人工操作带来的代码格式问题。

1
2
3
4
5
6
7

// 以 LLVM 代码风格格式化main.cpp, 结果输出到 stdout
clang-format -style=LLVM  main.cpp
// 以 LLVM 风格格式化 main.cpp,  结果直接写到 main.cpp
clang-format -style=LLVM  -i main.cpp

// 以 Google 风格格式化 main.cpp,  结果直接写到 main.cpp
clang-format -style=google -i main.cpp

• 批量格式化代码

find . -path '*/src/*.cpp' -o -path '*/include/*.h' ! -name 'sigslot.h' | xargs clang-format -style=file -i

在使用时，出现一个奇怪的bug，clang-format把我修改的文件的所有者都改成了root，再修改时还得先把用户改回来，极其不方便，不过我是在VMware里遇到的bug

参考: clang-format的介绍和使用

安装 colcon : sudo apt install python3-colcon-common-extensions。 ROS2的build没有了ROS1中的devel概念

• colcon build     编译所有包

• colcon build —packages-select pkg     只编译一个包

• colcon build —cmake-args -DCMAKE_BUILD_TYPE=Release    

不编译测试单元

colcon test --packages-select YOUR_PKG_NAME --cmake-args -DBUILD_TESTING = 0

运行编译的包的测试

colcon test

• 允许通过更改src下的部分文件改变install，这样每次修改Python脚本时不必重新 build

colcon build --symlink-install

• colcon build —symlink-install pkg   

• colcon build —symlink-install —packages-ignore pkg    

减小colcon的CPU占用

指定并行的线程数

1

colcon build --symlink-install --parallel-workers 1

不使用并行编译

1

colcon build --symlink-install  --executor sequential

两种编译不要混合用，否则每次都会重新编译

显示CMake中的MESSAGE

colcon build —event-handlers console_direct+ —packages-select test_node

console_direct 换成 console_cohesion 也可以

如果不想编译特定的包，在该包目录里面创建一个名为COLCON_IGNORE文件，这样子这个包就不会被索引到了

问题

1. 编译时找不到 ament_cmake

source /opt/ros/jazzy/setup.bash

1. 报警: WARNING:colcon.colcon_ros.prefix_path.catkin:The path ‘/home/user/catkin_ws/install/nav2_map_server’ in the environment variable CMAKE_PREFIX_PATH doesn’t exist

即使删除了install, build, log也无法解决，要想彻底解决只能重建一个工作空间。或者用临时方法，但新终端又会失效

1
2

unset AMENT_PREFIX_PATH
unset CMAKE_PREFIX_PATH

不搜索指定的文件夹

可以使用以下JSON格式的示例进行配置：

1
2
3
4
5
6
7
8
9

{
  "search.exclude": {
    "**/node_modules": true,
    "**/build": true,
    "**/dist": true,
    "**/.git": true,
    "**/.vscode": true
  }
}

"**/node_modules": true - 这将排除项目中的node_modules文件夹，通常包含依赖库。

"**/build": true - 这将排除build文件夹，如果您的项目使用构建工具生成构建文件，可以排除它。

"**/dist": true - 这将排除dist文件夹，如果您的项目包含编译后的分发文件，可以排除它。

"**/.git": true - 这将排除.git文件夹，以防止搜索Git版本控制文件。

"**/.vscode": true - 这将排除.vscode文件夹，以防止搜索Visual Studio Code配置文件。

• VS Code 按快捷键 ctrl+p 可以弹出一个小窗，在上面的 输入框输入文件名，下拉框点击一个文件

• 设置VsCode 多文件分行(栏)排列显示，打开vscode的设置，搜索wrap tabs，勾选上就可以了

• 修改鼠标滚轮效果： VSCode 设置页面搜索 mouseWheelScrollSensitivity，放大前两个系数