行为树其他常用代码

类 Subtree

BT::Tree有一个成员是 std::vector< Subtree::Ptr > subtrees。类BT::Tree::Subtree的成员函数如下:

1
2
3
4
5
6
7
Blackboard::Ptr   blackboard

std::string instance_name

std::vector< TreeNode::Ptr > nodes

std::string tree_ID

instance_name的结果:

1
2
3
subA::5
subB::16
subB::16/SubC::20

其中SubC是子树SubB的子树。

tree_ID是把instance_name的数字去掉了。

emitStateChanged

TreeNode::emitStateChanged()TreeNode 类中的一个方法。它用于发出信号,告知树节点的状态已发生变化。此方法对于在行为树中启用响应式和异步行为至关重要。当节点状态发生变化(例如,从running变为成功或失败)时,系统会调用 emitStateChanged() 来通知树,并可能触发进一步的操作或更新。 类似Qt中的信号和槽的机制。


行为树注册Action函数

registerSimpleAction

1
2
3
4
5
6
void BT::BehaviorTreeFactory::registerSimpleAction (const std::string&  ID,
const SimpleActionNode::TickFunctor & tick_functor,
PortsList ports = {}
)

typedef std::function<NodeStatus(TreeNode&)> BT::SimpleActionNode::TickFunctor

registerSimpleAction help you register nodes of type SimpleActionNode.

其实就是std::bind的用法,如果在类内的成员函数里注册,应该如下使用

1
2
3
4
5
6
7
8
9
10
BT::NodeStatus TestBT()
{
printf("TestBT\n");
return BT::NodeStatus::SUCCESS;
}

void memberFunc()
{
this->registerSimpleAction("TestBT", std::bind(&BtFactory::TestBT, this) );
}

外部调用时,将类对象本身的地址作为this指针传入

1
2
Class a;
obj.registerSimpleAction(std::bind(&Class::TestBT, &a, placeholders::_1, placeholders::_2));

registerNodeType

行为树注册时,是注册节点的ModelName,而不是InstanceName

报错

  1. 不识别新添加的Action类

factory.registerNodeType<ActionReportError>("ReportError");报了很多错,都是undefine error。解决方法:到CMake里先把对应的cpp文件去掉,编译后报错,再添加回来,成功


throw和catch
warning: catching polymorphic type ‘class std::exception’ by value [-Wcatch-value=] catch (std::exception e) {

这个警告是因为我们以值捕获的方式(byvalue)捕获了一个多态类型(polymorphic type)的异常(例如std:exception)。在 C++中,多态类型(即具有虚函数的类)通常应该通过引用(最好是const引用)来捕获,以避免对象切片(slicing)问题,并确保正确的多态行为。

代码中写了类似:catch(std:exception e) 按值捕获而正确的做法应该是:catch(const std:exception&e) ,按 const引用捕获

编译器选项—Wcatch—value(被—Wall启用)会对此发出警告,并且有三个级别:—Wcatch—value=1:警告按值捕获多态类型(默认,相当于—Wcatch—value)—Wcatch—value=2:警告按值捕获所有类类型—Wcatch—value=3:警告所有没有通过引用捕获的类型因此,解决这个警告的方法就是将按值捕获改为按引用捕获。


行为树两个Exception

  • LogicError is related to problems which require code refactoring to be fixed.

  • RuntimeError is related to problems that are relted to data or conditions that happen only at run-time

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
BT::Tree  tree;
try{
factory.registerBehaviorTreeFromFile("/home/user/test.xml");
tree = factory.createTree("maintree", maintree_bb);
} catch (BT::RuntimeError &error) {
printf("behavior tree RuntimeError: %s", error.what() );
return false;
} catch (BT::LogicError &error) {
printf("behavior tree LogicError: %s", error.what() );
return false;
}
catch (...) {
printf("create behavior tree failed, check it out");
return false;
}

ROS2自定义msg, action, srv

ROS2中msg的命名规范:必须以大写字母开头,并且不能有下划线,应当是 ^[A-Z][A-Za-z0-9]*$。像moving_target.msg, Moving_target.msg都是不规范的,应该是MovingTarget.msg

msg文件里的成员命名:只能是小写+下划线,不能出现大写。int32 id_number是合适的,int32 ID_number就不合适

参考 官方教程,注意对package.xml做下面修改

1
2
3
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

在其他包使用自定义的msg/Action包的时候,编译完成后,去build文件夹里找到可执行文件,结果发现有一个.so没有链接成功

1
2
3
ldd libmy_node.so  | grep my_defined
libmy_defined_msgs__rosidl_typesupport_cpp.so => /home/user/j36_project/install/my_defined_msgs/lib/libmy_defined_msgs__rosidl_typesupport_cpp.so (0x00007a9426d69000)
libmy_defined_msgs__rosidl_generator_c.so => not found

如果直接运行会出错,但如果用ros2 run运行是正常的,应当是ROS2运行时会建立链接,应当是ament命令在运行期发挥了作用


roslaunch的写法

launch目录里创建launch文件或py文件后,ros2 launch不能直接找到,会报错

1
file 'file.launch' was not found in the share directory of package 'package_name' which is at '/home/user/catkin_ws/install/package_name/share/package_name'

需要在CMakeLists里加一句
1
2
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})

再编译后,会把launch文件复制到share目录里。

  • 使用xml
1
2
3
4
5
6
7
8
<launch>
<node pkg="nav2_map_server" exec="map_server" name="map_server">
<param name="yaml_filename" value="/home/zzp/catkin_ws/maps/test.yaml"/>
</node>

<node pkg="nav2_planner" exec="planner_server" name="planner_server" >
</node>
</launch>

args的写法一直查不到,按ROS1的写法是错的,节点会退出: <node pkg ="tf2_ros" exec="static_transform_publisher" name="map_odom_tf" args="1 0 0 0 0 0 map odom 100" />

  • 使用python
1
2
3
cd ~/ros2_ws
mkdir launch
vim launch/both_launch.py

加入下面内容:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
return LaunchDescription([
Node(
package='duckiebot',
namespace='duckietown',
executable='duckiebot_node',
name='duckiebot_node1'
),
Node(
package='control',
namespace='duckietown',
executable='control_node',
name='control_node1'
)
])


不同命名空间的两个类互相调用

两个类互相引用的问题,一般用前置声明解决,但是两个不同命名空间的类就有些复杂了,两个类的头文件和源文件这样写:

类A的头文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
namespace bbb
{
class B;
}

namespace aaa {

class A
{
public:
A();
private:
bbb::B* b;
};
}

类A的源文件
1
2
3
4
5
6
7
8
9
#include "a.h"
#include "b.h"

namespace aaa {
A::A()
{
cout << "A" << endl;
}
}

类B的头文件:

1
2
3
4
5
6
7
8
9
10
11
#include "a.h"

namespace bbb {
class B
{
public:
B();
private:
aaa::A* a;
};
}

类B的源文件

1
2
3
4
5
6
7
8
#include "b.h"

namespace bbb {
B::B()
{
cout << "B" << endl;
}
}

1
2
3
4
5
file.cc:19:69:   required from here
/usr/include/c++/13/ext/aligned_buffer.h:93:28: error: invalid application of ‘sizeof’ to incomplete type ‘aaa::A’
93 | : std::aligned_storage<sizeof(_Tp), __alignof__(_Tp)>
| ^~~~~~~~~~~
/usr/include/c++/13/ext/aligned_buffer.h:93:28: error: invalid application of ‘sizeof’ to incomplete type ‘aaa::A’

解决了带命名空间的两个类互相调用的问题后,编译又出错误。看来是初始化aaa::A时,sizeof计算出错

前置声明告知编译器某个类或者模板的存在,但不提供其完整定义的方式。在初始化前置声明类时,需要分配内存,也就需要使用sizeof,它的计算发生在代码编译时期,此时无法计算,就会报错。解决方法就是在源文件里include前置声明类的头文件,比如这里我们在b.cppinclude "a.h"


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.


BT::Tree在版本3里有两个成员变量

1
2
3
std::vector<Blackboard::Ptr>  blackboard_stack

std::vector<TreeNode::Ptr> nodes

在版本4里这两个都消失了,增加了成员std::vector<Subtree::Ptr> subtrees,可能会造成不便。


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 ,这是本机的显卡没装好