cmp函数

cmp 函数的特点：

1. 返回值为 bool 类型，用来表示当前的排序是否正确
2. 参数为两个相同类型的变量，且类型与要排序的容器模板类型相同

   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

   // 从小到大排列 bool cmp( const Data& d1, const Data& d2) { // return(d1.dist < d2.dist); // 由于重载了 <= ，可以直接这么写 return(d1 <= d2); } vector<Data> vec; Data temp; for ( int i = 0; i < 8; ++i ) { temp.dist = rand() %50; temp.confidence = 100; vec.push_back( temp ); } for(int i=0; i<8; i++) { cout << vec.at(i).dist << " "; } cout << endl; // 第三个参数可以是一个函数指针，一般使用cmp函数 stable_sort( vec.begin(), vec.end(), cmp ); for(int i=0; i<8; i++) { cout << vec.at(i).dist << " "; }

std::accumulate 函数

用来计算特定范围内（包括连续的部分和初始值）所有元素的和，除此之外，还可以用指定的二进制操作来计算特定范围内的元素结果，需要 #include <numeric>。 三个形参：头两个形参指定要累加的元素范围，第三个形参则是累加的初值。

1
2
3

vector<int> v;
// 给 v 赋值
cout << std::accumulate(v.begin(), v.end(), 0) << endl;

当不正确关闭Gazebo时，再次启动Gazebo会遇到sever无法启动的问题。

1

Exception [Master.cc:50] Unable to start server[bind: Address already in use]. There is probably another Gazebo process running.

报错： Unable to convert from SDF version 1.7 to 1.6
把world文件的第一行改成 <sdf version = '1.6'>

文件使用了OBJ meshes，这是从Gazebo 7.4.0才可支持，使用gazebo -v发现我的gazebo版本是 7.0

报错 [joint_state_publisher-3] process has died 或者 UnicodeEncodeError: ‘ascii’ codec can’t encode characters in position xxx ordinal

去掉urdf、xacro、launch文件中的中文注释，或者改为英文，而且第一行不能有中文。

1
2
3
4

<plugin name='front_right_middle_sonar_sensor' filename='libgazebo_ros_range.so'>
  <topicName>/prius/front_sonar/right_middle_range</topicName>
  <frameName>front_right_middle_sonar_link</frameName>
</plugin>

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

<material name="blue">
  <color rgba="0.0 0.0 0.8 1.0"/>
</material>

<material name="red">
  <color rgba="0.8 0.0 0.0 1.0"/>
</material>

<material name="white">
  <color rgba="1.0 1.0 1.0 1.0"/>
</material>

<material name="yellow">
  <color rgba="0.8 0.8 0.0 1.0"/>
</material>

1
2
3
4
5
6

<link name="dummy">
   </link>
  <joint name="dummy_joint" type="fixed">
     <parent link="dummy"/>
     <child link="base_link"/>
   </joint>

1
2

[spawn_entity.py-5] [ERROR] [1715655287.539237215] [spawn_entity]: Service %s/spawn_entity unavailable. Was Gazebo started with GazeboRosFactory?
[spawn_entity.py-5] [ERROR] [1715655287.540067060] [spawn_entity]: Spawn service failed. Exiting.

1

[ERROR] [gzserver-1]: process has died [pid 239512, exit code 255, cmd 'gzserver -slibgazebo_ros_init.so -slibgazebo_ros_factory.so -slibgazebo_ros_force_system.so'].

1
2

reload(sys) 
sys.setdefaultencoding("utf-8")

1
2
3
4

#coding=utf8
import sys
reload(sys)
sys.setdefaultencoding('utf8')

• origin 远程服务器
• origin/master 远程分支
• master 本地分支

origin并不特别,就像分支名 master 在git中没有任何特殊意义一样.当执行git init时，master会作为初始分支的默认名字，因此使得master分支名被广泛使用。而origin是执行git clone时的默认服务器名称，当然可以通过指令git clone -o cat，使得默认服务器名称为cat，而默认远程分支为cat/master

从远程分支check out一个本地分支,该本地分支被称为追踪分支(tracking branch),被追踪的分支被称为上游分支(upstream branch),追踪分支可以理解为是和远程分支有直接关联的本地分支. 如果我们在追踪分支时执行git pull，git会自动知道需要获取和merge的分支的服务器.

拉取远程指定分支

1
2
3
4
5
6

git remote add origin https://github.com/user/user.github.com.git
# 或者是 git仓库链接
# git remote add origin git@github.com:XXXX/nothing2.git

# develop为远程仓库的分支名
git fetch origin develop

在本地创建分支develop并切换到该分支

1

git checkout -b develop(本地分支名称)  origin/develop (远程分支名称)

远程分支上的内容都拉取到本地

1
2

# develop 为远程分支名称
git pull origin develop (远程分支名称)

查看分支和合并

git branch -a list both remote-tracking and local branches

git branch -vv 会列出所有的分支信息,包含追踪分支的关联关系

常见的情况是，我自己有一个分支me，还要拉取另一个人的分支，两人共用部分文件，合并到分支master。在自己分支修改文件结束后，提交到自己的分支。然后切换到master分支，git checkout master。由于不知道另一个人是否更新了代码，所以先执行git pull更新一下，然后git merge me，这就合并了我的分支，再git push origin master。

有时会出现冲突 ，这是由于两人修改了同一个文件，冲突的提示

1
2
3

Auto-merging readme.txt
CONFLICT (content): Merge conflict in readme.txt
Automatic merge failed; fix conflicts and then commit the result.

将user分支合并到master分支

1
2
3
4
5
6
7
8
9

# 当前是user分支
git add test.yaml 
git commit -m ":wrench:"
git push origin user
# 切换到 master 分支
git checkout master
git merge user
git push origin master
git checkout user

删除本地分支

git branch -d user

1
2
3
4
5
6

user@user:~/dev_ws/$ git branch -d user
error: The branch 'user' is not fully merged.
If you are sure you want to delete it, run 'git branch -D user'.

user@user:~/dev_ws/$ git branch -D user
Deleted branch user (was cf64937b)

删除远程分支, 千万小心使用 ！

1

git push origin --delete remoteBranchName

难题 1

1
2
3
4
5
6
7

# 执行 git status 的结果
On branch master
Your branch and 'origin/master' have diverged,
and have 5 and 21 different commits each, respectively.
  (use "git pull" to merge the remote branch into yours)

nothing to commit, working tree clean

这发生在多人可能操作 master 分支的时候，我自己的分支提交完了，切换到master分支，要执行合并时，出现这个问题，并不是文件冲突的问题。按如下方法解决：

如果不需要保留本地的修改，只要执行下面两步：

1
2

git fetch origin
git reset --hard origin/master

1
2
3
4

git rebase origin/master
git pull --rebase
# 提交到远程仓库
git push origin master

本地文件修改后，执行git pull出现冲突

执行git pull出现下面提示

1
2

Please commit your changes or stash them before you merge.
Aborting

1. 放弃本地修改的改法 (这种方法会丢弃本地修改的代码，而且不可找回！)

1
2

git reset --hard
git pull

将最新的主分支更新到个人分支

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

# 当前在个人分支
git add .    # 将本地修改文件加入暂存区
git commit -m "修改内容"    # 提交日志

git checkout master      # 切换到主分支（建议操作到这里的时候利用git branch
git pull         # 将本地主分支代码更新
git checkout self-branch    # 切换到自分支

git merge master # 将主分支代码合并更新到自己分支

git push    # 提交到自分支远程端

git checkout master   # 切换到主分支

git merge self-branch   # 将自己代码合并更新到本地主分支master

git push   # 将本地代码推到远程主分支master上

一次合并冲突的解决

我在自己的分支me开发，主分支比我领先很多。有一次，我从git上下载了主分支的压缩包，然后手动覆盖了一部分文件以更新，这其实是不正确的操作。

第二天我先切换到主分支，更新到最新，然后切换到me分支，提交到远程后，合并主分支，结果出现的冲突比我自己修改的多很多，因为把我昨天手动覆盖的那些也包括了。这种情况下，只能手动解决冲突，但是大部分文件一定是用主分支，如果一个个打开会很麻烦。

用VSCode打开工程，分支管理里会显示文件冲突，对使用主分支版本的文件，右键选择就可以，而且能同时选多个文件。解决完之后，选择暂存文件，之后可以push了。

Navigation2中引入了行为树来组织机器人的工作流程和动作执行。

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

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

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

行为树并不执行特定的功能代码，它只是把各个业务逻辑代码组织起来。通过不同的组织方式和少量代码的修改可以实现截然不同的功能

Model

Sequence树节点的特点是它的children必须全部返回SUCCESS才认为执行成功，有一个child返回FAILURE就导致这个树节点执行失败。

• Before ticking the first child, the node status becomes RUNNING.

• If a child returns SUCCESS, it ticks the next child.

• action

动作节点通常实现服务客户端和动作客户端，也可以是一些简单的执行程序。他们通过向Planner server，Controller server，Recovery server发送请求来启动相应的功能程序。action通常作为行为树中的叶子节点，负责具体行为和功能的实现。但这些具体的功能代码并没有在叶子节点中而是在对应的服务端。

• condition

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

• control

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

• decorator

decorator是节点装饰器。它只能有一个子节点。负责将子节点的结果进行修饰。比如将子节点的结果进行反向，约束子节点的执行次数等等。

ControlNode至少有1个子节点，DecoratorNode只有1个子节点，Subtree没有子节点

黑板

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

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

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

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

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

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

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

TOF

优点：

• 测量距离较远，不受表面灰度和特征影响，达百米，可用于无人驾驶

• 深度距离计算，不会随距离的变化而变化，稳定在厘米级

• 不像双目相机、结构光需要经过算法处理才能输出三维数据，TOF可以直接输出被测物体三维数据

• 相比于结构技术的静态场景，TOF更适合动态场景

缺点：

• 精度较差，很难达毫米级，因其对时间测量设备要求高，这点比不上结构光的近距离(1m内)应用，限制在近距离高精度领域的应用

• 室外强光照基本不能使用

双目

代表：图漾, Astro Dabai, ZED

结构光

代表: OAK, 奥比中光, Realsense