CMake 常用宏和技巧|cmake/qmake

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

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

优先队列 std::priority_queue|C++C++ 模板与STL

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;
}

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

glog记录程序崩溃及扩展|第三方库
abstract Welcome to my blog, enter password to read.
Read more
ROS2的package.xml和cmake|ROSROS2

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是官方推荐的方式去添加依赖项。它将使依赖项的库、头文件和自身的依赖项被正常找到。

  • 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了。

相机的类型|相机

双目

代表：图漾, Astro Dabai, ZED

结构光

代表: OAK, 奥比中光, Realsense

行为树常用代码|路径规划行为树
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
class Test : public BT::SyncActionNode {
public:
    /**
     * @param name node name
     * @param config Node parameters and configuration information
     */
    Test(const std::string& name, const BT::NodeConfiguration& config);
    // 构造函数里 setRegistrationID
    /**
     * @brief Define node input parameters
     * @return BT::PortsList parameter list
     */
    static BT::PortsList providedPorts() { return {}; }
private:
    // 实现 tick 函数
    virtual BT::NodeStatus tick() override;
};

factory.registerNodeType<Test>("TestAction");
factory.registerSimpleAction("SimpleAction", SimpleActionFunc);
1
2
3
class Pipeline : public BT::ControlNode 

class RateController : public BT::DecoratorNode

每次tickRoot()函数执行都会遍历整个树，对于asynActionNode，因为其本身有循环(在单独线程里)，所以循环没有结束时会返回RUNNING状态。不同的控制流节点对RUNNING的处理不一样。

装饰节点

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
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include "behaviortree_cpp_v3/decorator_node.h"

class RateController : public BT::DecoratorNode {
 public:
  /**
   * @brief RateController constructor
   * @param name name of this node
   * @param conf config of this node
   */
  RateController(const std::string& name, const BT::NodeConfiguration& conf)
  		: BT::DecoratorNode(name, conf), first_time_(false), initialized_(false) {}

  void Initialize()
  {
  	  double hz = 1.0;
	  getInput("hz", hz);
	  period_ = 1.0 / hz;
	  initialized_ = true;
	  first_time_ = true;
	  start_ = std::chrono::high_resolution_clock::now();
  }
  /**
   * @brief Provided ports
   * @return BT::PortsList ports list
   */
  static BT::PortsList providedPorts() {
    return {BT::InputPort<double>("hz", 10.0, "Rate")};
  }

 private:
  /**
   * @brief Node Execution Logic
   * @return BT::NodeStatus node status
   */
  BT::NodeStatus tick()
  {
  	  if (!initialized_)
  		  Initialize();
  
	  auto now = std::chrono::high_resolution_clock::now();
	  auto elapsed = now - start_;
	  auto seconds =
	      std::chrono::duration_cast<std::chrono::duration<float>>(elapsed);
	  if (first_time_ || seconds.count() >= period_) {
	    first_time_ = false;
	    const BT::NodeStatus child_status = child_node_->executeTick();
	    start_ = std::chrono::high_resolution_clock::now();
	    switch (child_status) {
	      case BT::NodeStatus::RUNNING:
	        return BT::NodeStatus::RUNNING;
	      case BT::NodeStatus::SUCCESS:
	        return BT::NodeStatus::SUCCESS;
	      case BT::NodeStatus::FAILURE:
	      default:
	        return BT::NodeStatus::FAILURE;
	    }
	  }
	  return BT::NodeStatus::FAILURE;
}

  bool first_time_;   // first tiem execute child node
  bool initialized_;  // initialization flag
  double period_;     // execute child node period
  std::chrono::time_point<std::chrono::high_resolution_clock>  start_;  // start time of tick
};

port

portslist中没有的key是不能用getInput或getOutput来操作的。

  • 获取port的值

getInput("hz", hz);

  • 设置port的值
1
2
3
task_ = config.blackboard->get<Task>("cur_task");
setOutput<std::string>("type", task_.type_);
setOutput<std::string>("state", task_.state_);
  • 在xml文件中声明ports
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<Action ID="GetPath">
    <input_port name="value_A"/>
</Action>


<Decorator ID="RateController">
    <input_port name="hz" type="double">Rate</input_port>
</Decorator>
<Condition ID="ReachGoalCondition">
    <input_port name="dis_tol" type="float"/>
</Condition>

<SubTree ID="DilemmaSetControlHandler">
    <input_port default="true" name="__shared_blackboard">If false (default), the Subtree has an isolated blackboard and needs port remapping</input_port>
</SubTree>
行为树|路径规划行为树

行为树的优点:

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

缺点:

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

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

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

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

行为树是由控制节点、装饰节点、行为节点组成的一棵树。中间节点一般为控制节点和装饰节，用于控制行为树的执行流程，它们相对固定，一旦确定几乎不会变化。叶子节点由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();

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

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>标签对应的文件。

clang-format的使用|常用工具

统一的代码规范对于整个团队来说十分重要，通过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

除了LLVM和Google外，还有 Chromium, Mozilla, WebKit

  • 批量格式化代码

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编译|ROSROS2

安装 colcon : sudo apt install python3-colcon-common-extensions

  • 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    

ROS2的build没有了ROS1中的devel概念

visual studio code的配置|常用工具

不搜索指定的文件夹

可以使用以下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，放大前两个系数