前言
最近在读一篇规划方面论文，需要复现论文中的算法，在论文的代码演示中，作者已经预先定义好了一个地图。为了看看这个算法在其他的地图场景下适用性如何，我打算自己改一改地图。这个算法中使用到的建图工具正好是Octomap，于是这就开启了我这几天和Octomap的“软磨硬泡”之旅。这是我第一次比较深入地进行论文阅读，也是第一次进行实验的复现，所以作为一个“小白”，开启这段旅程着实不太容易，好在经过一段时间摸索终于摸出了一些门路。于是，写下这篇博客，就当是笔记，以备日后查看。
这篇博客将会涵盖Octomap的编译、Octomap地图样例运行、自定义地图这三个部分内容。最终的目的是要生成一个.bt格式的自定义地图文件，这样就可以无缝衔接地去测试算法了。
准备
本次实验是在Ubuntu 20.04上进行的。
为了保证后期Octomap的正常运行，还需要安装一些包。
sudo apt-get install cmake doxygen libqt4-dev libqt4-opengl-dev libqglviewer-dev-qt4这条命令应该在18.04及更老的Ubuntu上可以运行，但是对于Ubuntu 20.04来说，会出一些问题。原因是在Ubuntu 20.04中，Qt4的库已经被遗弃了，改成了Qt5。而在官方文档中也说明了，Qt4、OpenGL和QGLViewer是可视化工具的依赖包。我查了一些资料，应该这个比较可用，后来也试成功了（How to Install Qt4 Libraries in Ubuntu 20.04），有需要的可以一试。
Octomap的编译
Octomap的代码要从其GitHub上下载（Octomap）。
首先，在Ubuntu上进入需要下载到的位置，然后执行下述命令：
git clone https://github.com/OctoMap/octomap.git
这样，在指定路径下就能看到一个名为octomap的文件夹。这里面还包含了两个比较重要的子文件夹，一个名字也是octomap，它是之后我们真正会用到的库；另一个名字是octovis，是一个可视化Octomap的工具。这两个我们之后都会频繁用到。（题外话，tree命令是用来查看文件夹结构的，比较方便）
下面开始编译，按照官方Github上的教程，我们可以在子文件夹中分别编译上述提到的两个文件夹，也可以在最上层的文件夹中一起编译两个包。在此，我选择的是一起编译。
首先确认自己进入了刚刚clone下来的octomap最上层文件夹中，如下图中箭头所示（注意！因为还有个子文件夹叫octomap，所以不要进错了，如果想要两个子文件夹同时编译，需要在最上层文件夹中操作）。
进入上图中箭头所示的文件夹
cd octomap创建一个名为build的文件夹，并进入
mkdir build && cd build下面进行编译，Octomap的库使用CMake编译（关于CMake我也是第一次接触，先挖个坑，之后好好学学，争取把它填上），分别执行以下两条命令（均在build文件夹下执行）
cmake ..
make运行完成后，应该octomap的文件夹下又多出来了bin和lib文件夹
bin文件夹中包含着编译后产生的二进制文件，之后还会到这里来的。到此为止，编译部分应该大功告成！
Octomap地图样例运行
在Octomap官方的API文档中，介绍了一个快速尝试Octomap的方法，我们可以来尝试一下。这个样例位于octomap/octomap/src中，名字是simple_example.cpp，会用C++代码的形式生成一个简单的Octomap三维物体。
首先，我们先找到simple_example.cpp的位置
用文本编辑器、vscode或其他你喜欢的方式打开它（关于代码的一些解读见注释）
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
using namespace std;
using namespace octomap;
void print_query_info(point3d query, OcTreeNode* node) {
if (node != NULL) {
cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
}
else
cout << "occupancy probability at " << query << ":\t is unknown" << endl;
}
int main(int /*argc*/, char** /*argv*/) {
cout << endl;
cout << "generating example map" << endl;
OcTree tree (0.1); // create empty tree with resolution 0.1
// 以下部分用于建立一个边长为2的正方体
for (int x=-20; x<20; x++) {
for (int y=-20; y<20; y++) {
for (int z=-20; z<20; z++) {
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true); // integrate 'occupied' measurement
}
}
}
// 以下部分建立了一个边长为1.2的正方体，并在x、y、z三个方向上都做了相应的平移
// 之后，再把和上面建立边长为2的正方体相交的部分给去掉了
for (int x=-30; x<30; x++) {
for (int y=-30; y<30; y++) {
for (int z=-30; z<30; z++) {
point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
tree.updateNode(endpoint, false); // integrate 'free' measurement
}
}
}
cout << endl;
cout << "performing some queries:" << endl;
point3d query (0., 0., 0.);
OcTreeNode* result = tree.search (query);
print_query_info(query, result);
query = point3d(-1.,-1.,-1.);
result = tree.search (query);
print_query_info(query, result);
query = point3d(1.,1.,1.);
result = tree.search (query);
print_query_info(query, result);
cout << endl;
tree.writeBinary("simple_tree.bt");
cout << "wrote example file simple_tree.bt" << endl << endl;
cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
}看完了代码，我们可以来尝试得到它的运行结果。由于是C++程序，我们不能像python那样直接运行，需要先进行编译。鉴于刚刚我们已经完成过Octomap库的编译了，已经获得了这个样例程序相应的二进制文件，我们可以直接尝试运行一下。
下面进入最上层的octomap文件夹中的bin文件夹（这和我们开始自己建立的build文件夹同级），如下图：
我们可以在里面看到一个对应的名为simple_example的二进制文件，利用如下命令运行它
./simple_example
此时，bin文件夹中会多出一个名为simple_tree.bt的地图文件，这就是我们通过simple_example.cpp生成的自定义地图。
我们可以通过可视化工具octovis来查看这个样例地图长什么样
./octovis simple_tree.bt
自定义地图
看完了官方样例，我们就可以自己手工生成一些简单的地图试一试。只要模仿simple_example.cpp来编写代码即可，这里提供一份如下图所示的地图的代码供参考。
代码如下：
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
using namespace std;
using namespace octomap;
void print_query_info(point3d query, OcTreeNode* node) {
if (node != NULL) {
cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
}
else
cout << "occupancy probability at " << query << ":\t is unknown" << endl;
}
int main(int /*argc*/, char** /*argv*/) {
cout << endl;
cout << "generating example map" << endl;
OcTree tree (0.1); // create empty tree with resolution 0.1 [http://octomap.github.io/octomap/doc/classoctomap_1_1OcTree.html#a08375ca02aff5117437ab2a417c8e062]
// insert some measurements of occupied cells
// 四周的墙
for (int y=0; y<200; y++){
int x = 0;
for (int z=-6; z<7; z++){
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true);// integrate 'occupied' measurement
}
}
for (int x=0; x<200; x++){
int y = 200;
for (int z=-6; z<7; z++){
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true);// integrate 'occupied' measurement
}
}
for (int y=0; y<200; y++){
int x = 200;
for (int z=-6; z<7; z++){
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true);// integrate 'occupied' measurement
}
}
for (int x=0; x<200; x++){
int y = 0;
for (int z=-6; z<7; z++){
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true);// integrate 'occupied' measurement
}
}
// 中间的厚墙障碍
for (int x=80; x<120; x++){
for (int y=0; y<150; y++){
for (int z=-6; z<7; z++){
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true);// integrate 'occupied' measurement
}
}
}
cout << endl;
tree.writeBinary("test_map1.bt");
cout << "wrote example file test_map1.bt" << endl << endl;
cout << "now you can use octovis to visualize: ./octovis test_map1.bt" << endl;
cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
}为了简单期间，可以在simple_example.cpp同级文件夹下新建一个cpp文件用来保存上面这段代码，我们不妨将其命名为test_map1.cpp。
为了能够顺利通过CMake编译，使其变为二进制文件，我们还需要对相应的CMakeLists.txt进行修改。
打开上图的octomap/octomap/src文件夹下的CMakeLists.txt，并添加如下命令（其中，ADD_EXECUTABLE含义是，使用指定的源文件test_map1.cpp来生成目标可执行文件test_map1；TARGET_LINK_LIBRARIES含义是，为test_map1连接库octomap的头文件路径）
ADD_EXECUTABLE(test_map1 test_map1.cpp)
TARGET_LINK_LIBRARIES(test_map1 octomap)
保存并退出。
进入octomap/build文件夹，在文件夹中执行make命令。然后进入octomap/bin中，就可以看到，新编译出来的test_map1。
然后运行
./octovis test_map1.bt就可以看到我们新生成的自定义地图了
