BehaviorCPP版本从3到4.6的变化

可以使用convert_v3_to_v4.py将版本3的xml文件转为版本4,没发现反过来转换的。

  • classes / XML tags 发生的变化
Name in 3.8+ Name in 4.x Where
NodeConfiguration NodeConfig C++
SequenceStar SequenceWithMemory C++ and XML
AsyncActionNode ThreadedAction C++
BT::Optional BT::Expected C++
  • <root> 变成了 <root BTCPP_format="4">

  • ControlNodes and Decorators 必须支持新的节点状态 NodeStatus:SKIPPED

The purpose of this new status is to be returned when a PreCondition is not met. When a Node returns SKIPPED, it is notifying to its parent (ControlNode or Decorator) that it hasn’t been executed.

  • Ticking in a While Loop

版本3常常是这样进行tick的

1
2
3
4
5
while(status != NodeStatus::SUCCESS || status == NodeStatus::FAILURE)
{
tree.tickRoot();
std::this_thread::sleep_for(sleep_ms);
}

版本4引入新的sleep函数

1
Tree::sleep(std::chrono::milliseconds timeout)

This particular implementation of sleepcan be interrupted if any node in the tree invokes the method TreeNode::emitWakeUpSignal. This allows the loop to re-tick the tree immediately.

Tree::tickRoot() 已经消失了

1
2
3
4
5
6
7
8
//  status = tree.tickOnce();
while(!BT::isStatusCompleted(status) )
{
//---- or, even better ------
// tree.tickWhileRunning(sleep_ms);
tree.tickOnce();
tree.sleep(sleep_ms);
}

Tree::tickWhileRunning is the new default and it has its own internal loop; the first argument is a timeout of the sleep inside the loop.

或者这两个:

  • Tree::tickExactlyOnce(): equivalent to the old behavior in 3.8+
  • Tree::tickOnce() is roughly equivalent to tickWhileRunning(0ms). It may potentially tick more than once.
  • getInput的变化
1
BT::Optional<std::string> port_plugin_name = getInput<std::string>("plugin_name");

原型: Result BT::TreeNode::getInput(const std::string& key, T& destination ) const [inline]

Read an input port, which, in practice, is an entry in the blackboard. If the blackboard contains a std::string and T is not a string, convertFromString<T>() is used automatically to parse the text.

如果没有默认值的InputPort,不能获取值,运行会报错

1
2
3
BT::Expected<int> t = getInput<int>("ttt");
if(t)
cout << "Expected ttt: " << t.value() << endl;
  • SequenceWithMemory 取代 SequenceStar

Use this ControlNode when you don’t want to tick children again that already returned SUCCESS.

加载行为树的方式变化

1
2
3
4
5
6
7
Tree BT::BehaviorTreeFactory::createTree(const std::string& tree_name,
Blackboard::Ptr blackboard = Blackboard::create() )

Tree BT::BehaviorTreeFactory::createTreeFromFile(const std::string& file_path, Blackboard::Ptr blackboard = Blackboard::create() )

Tree BT::BehaviorTreeFactory::createTreeFromText(const std::string& text,
Blackboard::Ptr blackboard = Blackboard::create() )


1
- void BT::BehaviorTreeFactory::registerBehaviorTreeFromFile(const std::string&  filename)

Load the definition of an entire behavior tree, but don’t instantiate it. You can instantiate it later with BehaviorTreeFactory::createTree(tree_id), where “tree_id” come from the XML attribute <BehaviorTree id="tree_id">

1
void BT::BehaviorTreeFactory::registerBehaviorTreeFromText(const std::string &_xml_text_)

Same of registerBehaviorTreeFromFile, but passing the XML text, instead of the filename

  • 版本3的用法

auto tree = factory.createTreeFromFile("test.xml", maintree_bb);

版本4的用法如下:

1
2
3
4
factory.registerBehaviorTreeFromFile("test.xml");

// xml文件的开头是 <root BTCPP_format="4" main_tree_to_execute = "test" >
auto tree = factory.createTree("test", maintree_bb);

main_tree_to_execute的值一定要和createTree的第一个参数一致,否则会出错,出错的根源在 Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard, std::string main_tree_ID)

每次在Groot2里修改行为树后,保存会让main_tree_to_execute部分消失,我认为这是个设计上的bug

tick系列函数

  • 版本4不再有函数 BT::Tree::tickRoot(TickOption opt, std::chrono::milliseconds sleep_time)
1
2
3
4
5
6
7
enum BT::Tree::TickOption
{
Enumerator
EXACTLY_ONCE
ONCE_UNLESS_WOKEN_UP
WHILE_RUNNING
}

版本4使用下面3个函数

  • BT::NodeStatus BT::Tree::tickWhileRunning(std::chrono::milliseconds sleep_time = std::chrono::milliseconds(10) )

Call tickOnce until the status is different from RUNNING. Note that between one tick and the following one, a Tree::sleep() is used

  • BT::NodeStatus BT::Tree::tickExactlyOnce()

Tick the root of the tree once, even if a node invoked emitWakeUpSignal()

  • BT::NodeStatus BT::Tree::tickOnce()

by default, tickOnce() sends a single tick, But as long as there is at least one node of the tree invoking TreeNode::emitWakeUpSignal(), it will be ticked again.


Gazebo Harmonic 配置仿真环境

按照这个包的步骤来,最后启动

1
ros2 launch gazebo_differential_drive_robot robot.launch.py

手动控制还是用cmd


Gazebo Harmonic 安装和常用命令

ubuntu 24.04安装jazzy后,对应的gazebo和以前不太一样了。我们需要安装的是 gazebo Harmonic

image.png

安装

安装过程参考 Gazebo Harmonic 和 ROS2 jazzy 安装和测试

最后还要安装

1
2
3
4
5
6
7
8
9
10
sudo apt-get install -y ros-jazzy-ros-gz 
sudo apt-get install -y ros-jazzy-ros-gz-sim-demos
sudo apt-get install -y ros-jazzy-ros-gz-bridge
sudo apt-get install -y ros-jazzy-ros-gz-image
sudo apt-get install -y ros-jazzy-ros-gz-interfaces
sudo apt-get install -y ros-jazzy-ros-gz-sim
sudo apt-get install -y ros-jazzy-joint-state-publisher
sudo apt-get install -y ros-jazzy-xacro
sudo apt-get install -y ros-jazzy-teleop-twist-keyboard
sudo apt-get install -y ros-jazzy-teleop-twist-joy

基础命令

  • 启动命令: gz sim 或者 gz sim -v 4. -v: 指定日志详细级别,4 表示最高详细程度。

  • 可以使用-s(仅服务器)或-g(仅GUI)参数来分别运行服务器或GUI

插件的加载方式已被更改,需要在模型文件(SDF/URDF)或 Gazebo 的配置中添加。

  • 启动制定的sdf文件: gz sim shapes.sdf

启动之后,有3个进程: gz sim, gz sim server, gz sim gui

  • ROS2的方式启动: ros2 launch ros_gz_sim gz_sim.launch.py gz_args:=empty.sdf

  • 启动图像管道: ros2 launch ros_gz_sim_demos image_bridge.launch.py

重要的sdf文件: diff_drive_skid.sdf, actor.sdf

从ROS 2 Jazzy开始,Gazebo通过vendor包的形式在ROS包仓库中可用。如果您的包直接依赖于Gazebo库,而不是仅依赖于ros_gz,请参考相关文档学习如何使用Gazebo vendor包。

话题相关命令

gazebo的相关命令从ROS2里独立出来了

  • gz topic -l 显示所有话题

  • gz topic -it /model/tugbot/pose 显示话题信息,结果如下

1
2
3
Publishers [Address, Message Type]:
tcp://172.27.36.158:38969, gz.msgs.Pose
No subscribers on topic [/model/tugbot/pose]
  • -et echo 话题
  • -ft 显示话题频率
  • -pt 发布话题

gz topic -h可显示其他命令

在新的架构中,Gazebo Sim 与ROS 2的集成通过ros_gz桥接包实现。该桥接包允许 ROS 2 和 Gazebo Sim 之间的消息传递

问题

docker里运行gazebo报错: libGL error: MESA-LOADER: failed to retrieve device information ,这是本机的显卡没装好


31007

gtest的使用

AddressSanitizer(ASan)的使用

ASan 是GCC 和Clang 编译器的一部分,所以无需单独安装.

AddressSanitizer与Valgrind相比

  1. Valgrind通过模拟CPU来检测内存错误,导致会以较慢的速度运行程序;而AddressSanitizer是在编译阶段插入检查的逻辑,执行速度比Valgrind快很多
  2. Valgrind是一个独立的工具,可以使用在任何程序上;而AddressSanitizer与编译器紧密集成,可以在构建时自动启用
  3. 在错误信息的展示上,AddressSanitizer提供的错误信息比Valgrind容易理解,但Valgrind更详细
  4. AddressSanitizer作为编译器的一部分,通过编译选项启用;而Valgrind作为独立的工具,需要更多的配置和学习才能使用
  5. AddressSanitizer通过编译时插桩和运行时检查来检测内存错误,误报率较低

AddressSanitizer能检测的错误类型
截图 2025-07-09 09-35-44.png

CMake中的设置:

1
2
3
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fsanitize=address")

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")

比如这样的典型代码

1
Test* t = new Test();

ASan的报错如下
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
=================================================================

==2816467==ERROR: LeakSanitizer: detected memory leaks

Direct leak of 8 byte(s) in 1 object(s) allocated from:

#0 0x72cdcdefe548 in operator new(unsigned long) ../../../../src/libsanitizer/asan/asan_new_delete.cpp:95

#1 0x5cbc2c68d2c3 in main /home/zzp/qt_projects/untitled/main.cpp:10

#2 0x72cdcd62a1c9 in __libc_start_call_main ../sysdeps/nptl/libc_start_call_main.h:58

#3 0x72cdcd62a28a in __libc_start_main_impl ../csu/libc-start.c:360

#4 0x5cbc2c68d1e4 in _start (/home/zzp/qt_projects/build-untitled-Desktop_Qt_5_9_9_GCC_64bit-Debug/untitled+0x21e4) (BuildId: 1061b9fe4580a7e3af28420ed615c3664d393550)

SUMMARY: AddressSanitizer: 8 byte(s) leaked in 1 allocation(s).


Groot的使用

缺点

  1. 对一个控制节点,用Create Subtree创建子树后,无法恢复不是子树的状态。在左侧的树列表里,千万不要删除任何树,否则无法恢复
  2. 有的人在groot里添加SetBlackboard设置黑板变量,有的人是在C++里设置,而平时看行为树逻辑一般都在groot里,会不知道C++里做了什么
  3. 如果当前行为树设计有错,比如有多余节点,groot无法使用保存按钮

黑板变量的设置

Real Time Monitor Mode

目前只有Groot2 Pro版本可以使用

在版本4里,BT::PublisherZMQ已经消失了。取而代之的是BT::Groot2Publisher,可以这样用

1
2
3
4
5
6
7
8
9
10
factory.registerBehaviorTreeFromFile("/home/user/test.xml");
auto tree = factory.createTree("test", maintree_bb);
// createTree之后,tick之前
BT::Groot2Publisher publisher(tree);

while (true)
{
tree.tickOnce();
tree.sleep(std::chrono::milliseconds(100) );
}

打开groot之后,点击Connect毫无反应,官方说明没有帮助,读源码发现构造函数里有默认端口号:Groot2Publisher(const BT::Tree& tree, unsigned server_port = 1667);

使用1667后就成功了。Host需要看情况作修改

截图 2025-07-05 16-40-03.png
参考: BT::Groot2Publisher Class Reference


行为树的节点

行为树是由控制节点、装饰节点、行为节点组成的一棵树。中间节点一般为控制节点和装饰节,用于控制行为树的执行流程,它们相对固定,一旦确定几乎不会变化。叶子节点由Action节点或Condition节点组成,使用者的大部分工作都是设计树的逻辑和行为节点。

行为树的每个节点都有一个返回值,它们分别是:

  • 成功(Success)
  • 失败(failed)
  • 运行中(running):表示当前帧没有执行完成、下帧继续执行。

控制类节点

控制节点一般为中间节点,用于控制行为树的执行流程,决定了其子节点是以顺序、并行、随机或其它方式执行。

  • 顺序节点

依次执行所有子节点,若当前子节点返回成功,则继续执行下一个子节点;若子当前节点返回失败,则中断后续子节点的执行,并把结果返回给父节点。节点1返回成功,继续执行节点2;节点2返回失败,则把结果返回给Sequences的父节点,节点3并不会执行。顺序节点相当于and语义。

  • 选择节点(Selector)

依次执行所有子节点,若当前子节点返回成功,则中断后续节点运行,并把结果返回给父节点。相当于or语义

当有的行为节点对应的代码执行较长,例如:播放动画,此时,这个行为节点会向父节点返回running,于是选择节点便不再执行后续节点,直接向父节点返回 running

  • 并行节点(Parallel)

依次执行所有子节点,无论失败与否,都会把所有子节点执行一遍。至于Parallel节点该返回什么值给父节点,这要看需求。比如:成功数 > 失败数返回成功,否则返回失败。

  • 随机节点(Random)

随机选择一个子节点来运行。机器人领域应该不会使用。在游戏设计里,AI角色每天会根据自己的心情选择是呆在家里、工作或是出门游玩,可以采用随机选择节点

  • 记忆节点(MemSequences、MemSelector)

使用很少。功能和顺序节点、选择节点类似,唯一不同是会保存当前执行进度(比如:保存当前子节点索引),下一帧继续执行当前节点,如果当前节点是中间节点,则会跳过前面的节点。

修饰节点(Decorator)

  • 逆变节点(Inverter):tick孩子节点一次,孩子节点失败返回SUCCESS,成功返回FAILURE,孩子节点运行则返回RUNNING。也就是对子节点的返回值取反,相当于not语义,它只会有一个子节点
  • 成功节点 (ForceSuccessNode):tick子节点一次,不管其子节点返回何值,都会返回Success给父节点
  • 重复节点 (Repeater):重复执行n次子节点(n作为数据输入),直到子节点返回失败,则该节点返回FAILURE,若子节点返回RUNNING ,则同样返回RUNNING。
  • RetryNode: 最多tick子节点 n 次,(n作为数据输入),直到子节点返回成功,则该节点返回 SUCCESS,若子节点返回RUNNING ,则同样返回RUNNING
  • 执行一段时间(MaxTime):重复执行子节点一段时间

  • action

动作节点通常实现服务客户端和动作客户端,也可以是一些简单的执行程序。action通常作为行为树中的叶子节点,负责具体行为和功能的实现。但这些具体的功能代码并没有在叶子节点中而是在对应的服务端。执行这种节点,可能只需要一帧就可以完成,也可能需要多帧才能完成。

它至少包含两个函数:

  • Init:用于初始化节点,比如读取配置数据初始化当前节点,只会执行一次。
  • OnTick:每一帧都会执行,节点的主要逻辑都在此函数中实现或调用。

  • condition

这是条件控制节点。比如判断电池电量,某一开关信号等等。

  • control

这是行为树中的控制流。类似c++语言中的if else,switch等等。它负责构建行为树的逻辑结构。sequeence,fallback等等就属于这个范畴。

黑板

行为树中的共有数据是存放在Blackboard中的。Blackboard是以键值对的形式存储数据的。各个行为树的叶子节点均可以通过key访问到需要的数据。节点的输入端口可以从黑板读取数据,节点的输出端口可以写入黑板。

Ports是用于在节点间交换数据而存在的。一个行为树叶子节点可以定义输入的Ports和输出的Ports。当不同叶子节点的Port有相同的key名称时,可以认为它们是相通的。当在行为树叶子节点中声明了这些Ports时,也需要同时在xml文件中描述这些Ports。

注意设置子树的__shared_blackboard属性为true,否则C++程序可能报错

获取全局黑板变量,也就是说它也用了单例模式。

1
auto blackboard = DecisionBlackboard::GetInstance().GetBlackBoard();

程序中如果加载了多个行为树,黑板变量可以共享。


Nav2框架

安装

依赖项

1
2
3
4
5
6
7
8
9
10
sudo apt install -y ros-jazzy-tf2*
rm ~/.config/ros.org/rqt_gui.ini
# 重启rqt,然后再rqt中plugin->visualization中可以看到到 tf tree

sudo apt-get install gazebo-ros-pkgs

sudo apt install -y ros-jazzy-navigation2
sudo apt install -y ros-jazzy-nav2-bringups
sudo apt install -y ros-jazzy-turtlebot3*
sudo apt install -y ros-jazzy-test-msgs ros-jazzy-geographic-msgs

下载jazzy分支的Navigation2,一般能正常编译。

在Nav2存储库的根目录中运行doxygen。它将生成一个包含文档的/doc/*目录。文档的入口点在浏览器中是index.html。

运行 Nav2 框架

使用 turtlebot3: ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

不需要设置 export GZ_SIM_RESOURCE_PATH

  • 手动控制: ros2 run turtlebot3_teleop teleop_keyboard


  • 建图程序: ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True


  • 保存地图: ros2 run nav2_map_server map_saver_cli -f test


  • 导航程序: ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=maps/test.yaml

节点和话题

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
/amcl

/behavior_server
/bt_navigator
/bt_navigator_navigate_through_poses_rclcpp_node
/bt_navigator_navigate_to_pose_rclcpp_node

/collision_monitor
/controller_server
/docking_server
/global_costmap/global_costmap
/local_costmap/local_costmap

/launch_ros_3856071
/lifecycle_manager_localization
/lifecycle_manager_navigation
/map_server
/nav2_container
/planner_server
/robot_state_publisher
/ros_gz_bridge
/ros_gz_image
/rviz2
/rviz_navigation_dialog_action_client

/smoother_server
/velocity_smoother
/waypoint_follower

速度输出是 /cmd_vel_smoothed

问题

  1. 因为显卡的驱动问题,有时加载不出机器人的模型

ROS2中的有用工具

rqt_graph 能够可视化节点和主题之间的连接。这个命令和ROS1一样,也是ROS2为数不多的GUI可视界面


ros2doctor可以检测大概的ros2整体配置,ros2doctor并不是一个调错的工具,对你的代码调试没有帮助. 它一般查找你系统中警告部分,只有UserWarning: ERROR:开头的可能是错误。另外可以检查发布者没有订阅者的问题。

运行 ros2 doctor 或者 ros2 doctor --report