【Octomap】入门与自定义地图的实现

最新推荐文章于 2024-05-18 16:25:44 发布

HauserDong

最新推荐文章于 2024-05-18 16:25:44 发布

阅读量2.1k

点赞数 14

文章标签： ubuntu linux

本文链接：https://blog.csdn.net/weixin_46737655/article/details/126367512

版权

前言

最近在读一篇规划方面论文，需要复现论文中的算法，在论文的代码演示中，作者已经预先定义好了一个地图。为了看看这个算法在其他的地图场景下适用性如何，我打算自己改一改地图。这个算法中使用到的建图工具正好是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

就可以看到我们新生成的自定义地图了

HauserDong

关注

14
点赞
踩
36

收藏

觉得还不错? 一键收藏
1
评论
【Octomap】入门与自定义地图的实现

Octomap浅入门：涉及Octomap的入门使用教程以及利用Octomap自建.bt的个性化地图
复制链接

扫一扫