文章目录

0 引言
1 安装依赖
- 1.1 grid map
- 1.2 Eigen
- 1.3 kindr
- 1.4 Point Cloud Library (PCL)
2 编译和问题解决
3 运行示例
- 3.1 turtlesim3_waffle_demo
- 3.2 simple_demo 和 Ground Truth Demo

0 引言

苏黎世开源的elevation mapping指的是苏黎世联邦理工学院（ETH Zurich）开发并开源的一个数字高程模型（Digital Elevation Model，DEM）处理工具，名为elevation，它可以用于处理和分析高程数据，生成高质量的数字高程模型，支持多种高程数据格式和多种高程插值方法。如下图所示，这个高程图项目让机器人在爬楼梯等方面更加稳定。

请添加图片描述

苏黎世开源的elevation mapping工程包含两个子工程elevation_mapping和elevation_mapping_demos，以下是每个子工程的简要介绍：

data：这个子文件夹包含了一些测试数据和示例数据，这些数据可用于测试和演示elevation mapping工具的功能和性能。
doc：这个子文件夹包含了elevation mapping工具的文档和说明，包括使用指南、API文档、开发者文档等等。
include：这个子文件夹包含了elevation mapping工具的头文件，这些头文件定义了elevation mapping工具中使用的各种数据类型、函数和类等等。
lib：这个子文件夹包含了elevation mapping工具所依赖的第三方库和工具，例如GDAL、Boost、zlib等等。
src：这个子文件夹包含了elevation mapping工具的源代码，包括数据下载和处理模块、数据处理和分析模块、数字高程模型生成和输出模块、用户接口模块等等。
test：这个子文件夹包含了elevation mapping工具的测试代码和测试数据，用于测试elevation mapping工具的正确性和稳定性。
tools：这个子文件夹包含了一些与elevation mapping工具相关的辅助工具，例如数据转换工具、可视化工具等等。

这些子工程提供了elevation mapping库的核心功能和一些demo演示功能，使得elevation mapping工具更加完善和易于使用。

👉 elevation mapping github：https://github.com/ANYbotics/elevation_mapping
·
本文系统环境：

Ubuntu18.04
ROS-melodic
OpenCV3.2.0
Eigen 3.3.4
PCL 1.8.1

1 安装依赖

1.1 grid map

首先安装 grid map 栅格地图库：

# sudo apt-get install ros-$ROS_DISTRO-grid-map
sudo apt-get install ros-melodic-grid-map

1.2 Eigen

elevation mapping 工程需要线性代数库eigen：

sudo apt-get install libeigen3-dev

1.3 kindr

直接源码安装即可：

git clone https://github.com/ANYbotics/kindr.git
cd kindr
mkdir build && cd build
cmake .. -DUSE_CMAKE=true
sudo make install

1.4 Point Cloud Library (PCL)

本文是安装了pcl1.8大版本，可先下载下载pcl 1.8.1 版本（点击Source code(zip）

先安装pcl1.8.1所需的依赖库：

sudo apt-get update  
sudo apt-get install git build-essential linux-libc-dev
sudo apt-get install cmake cmake-gui
sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev
sudo apt-get install mpi-default-dev openmpi-bin openmpi-common 
sudo apt-get install libflann1.9 libflann-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libboost-all-dev
sudo apt-get install libvtk7.1-qt libvtk7.1
sudo apt-get install libqhull* libgtest-dev
sudo apt-get install freeglut3-dev pkg-config
sudo apt-get install libxmu-dev libxi-dev
sudo apt-get install mono-complete
sudo apt-get install openjdk-8-jdk openjdk-8-jre

然后解压下载的pcl1.8.1源码，并进入到pcl1.8.1文件夹中：

mkdir build
cd build
cmake ..
make -j4
sudo make install

测试pcl是否安装成功的话，可用pcl_viewer xxx.pcd：
如果没有pcd文件，可去点云库PCL(Point Cloud Library)的学习资源汇总下载rabbit.pcd

pcl_viewer rabbit.pcd

请添加图片描述

2 编译和问题解决

为了最大限度地提高性能，请确保以发布(Release)模式构建：

mkdir -p ~/ele_map_ws/src
cd ~/ele_map_ws/src

git clone https://github.com/anybotics/elevation_mapping.git
git clone https://github.com/ANYbotics/kindr_ros.git

cd ..
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

catkin build 编译后出现如下错误，kindr_ros 库的原因：

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "kindr" with any of
  the following names:

    kindrConfig.cmake
    kindr-config.cmake

解决办法：可以修改catkin build为 catkin_make 来进行编译

cd ~/ele_map_ws
rm -rf build devel logs
catkin_make -DCMAKE_BUILD_TYPE=Release

重新执行catkin_make -DCMAKE_BUILD_TYPE=Release，发现编译通过：

[ 94%] Linking CXX shared library ~/ele_map_ws/src/ele_map/devel/lib/libelevation_mapping_library.so
[ 94%] Built target elevation_mapping_library
Scanning dependencies of target elevation_mapping
[ 97%] Building CXX object elevation_mapping/elevation_mapping/CMakeFiles/elevation_mapping.dir/src/elevation_mapping_node.cpp.o
[100%] Linking CXX executable ~/ele_map_ws/src/ele_map/devel/lib/elevation_mapping/elevation_mapping
[100%] Built target elevation_mapping

3 运行示例

3.1 turtlesim3_waffle_demo

turtlesim3_waffle_demo是elevation mapping高程图功能库利用搭载realsense仿真相机的turtlebot3仿真机器人不断运动生成高程图的示例，但首先要安装turtlebot3仿真机器人：

## 安装turtlebot3 仿真机器人库
sudo apt install ros-melodic-turtlebot3*

安装需要一段时间，安装完毕后，如下命令，启动两个终端，来演示turtlesim3_waffle_demo

# 终端1，激活环境
source ~/ele_map_ws/devel/setup.bash
roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch

# 终端2，启动并控制仿真机器人
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

在第二个终端，用 s 键控制小车暂停，而 w a d x 分别对应控制小车前进左转右转后退

启动后，发现rviz并没有生成高程图，而且发现终端1报错：

import rospkg
ModuleNotFoundError: No module named 'rospkg'

很明显是缺少rospkg库，直接执行pip install rospkg 安装，但是确定已经安装了，还是出现这个问题；
思考片刻后，针对python3，也安装rospkg，直接执行pip3 install rospkg 安装，这次安装后，问题不再出现。

重新启动后，又出现新的问题：

"~/ele_map_ws/src/elevation_mapping/elevation_mapping_demos/scripts/tf_to_pose_publisher.py", line 5, in <module>
    import tf
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf/__init__.py", line 30, in <module>
    from tf2_ros import TransformException as Exception, ConnectivityException, LookupException, ExtrapolationException
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf2_ros/__init__.py", line 38, in <module>
    from tf2_py import *
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf2_py/__init__.py", line 38, in <module>
    from ._tf2 import *
ImportError: dynamic module does not define module export function (PyInit__tf2)

...

[ERROR] [1690378396.561700432, 1.420000000]: Could not get pose information from robot for time 0.273000. Buffer empty?
[ERROR] [1690378414.259410433, 18.109000000]: Could not get pose information from robot for time 17.450000. Buffer empty?
...

可以定位到是由于~/ele_map_ws/src/elevation_mapping/elevation_mapping_demos/scripts/tf_to_pose_publisher.py该python脚本出现的问题，再联想上一个问题，针对python3，安装pip3 install rospkg才解决问题，打开tf_to_pose_publisher.py脚本，发现默认用的是python3，但ROS melodic默认用的是python2，而Ubuntu20.04的ROS noetic默认用的是python3。

所以直接修改/tf_to_pose_publisher.py脚本中的第一行#!/usr/bin/env python3为#!/usr/bin/env python2.

重新启动后，问题成功解决；

如下所示在rviz中即可看到控制的小车在运动，仿真realsense相机朝前方实时画面，右边在运动到走道附近也会生成elevation mapping高程图：

请添加图片描述

3.2 simple_demo 和 Ground Truth Demo

Ground Truth Demo是elevation mapping高程图功能的真值示例演示，但首先要编译point_cloud_io库：

cd ~/ele_map_ws/src 
git clone https://github.com/ANYbotics/point_cloud_io.git
cd .. 
catkin_make -DCMAKE_BUILD_TYPE=Release

编译时point_cloud_io遇到新问题：

~/ele_map_ws/src/point_cloud_io/src/Read.cpp:11:10: fatal error: filesystem: No such file or directory

解决办法：
首先修改~/ele_map_ws/src/point_cloud_io/src/Read.cpp文件：

// 修改1： #include <filesystem> 替换成 #include <experimental/filesystem>
// #include <filesystem>
#include <experimental/filesystem>

// 修改2：两处std::filesystem 替换成 std::experimental::filesystem
bool Read::readFile(const std::string& filePath, const std::string& pointCloudFrameId) {
  // if (std::filesystem::path(filePath).extension() == ".ply") {
  if (std::experimental::filesystem::path(filePath).extension() == ".ply") {
    // Load .ply file.
    pcl::PointCloud<pcl::PointXYZRGBNormal> pointCloud;
    if (pcl::io::loadPLYFile(filePath, pointCloud) != 0) {
      return false;
    }

    // Define PointCloud2 message.
    pcl::toROSMsg(pointCloud, *pointCloudMessage_);
  }
#ifdef HAVE_VTK
  // else if (std::filesystem::path(filePath).extension() == ".vtk") {
  else if (std::experimental::filesystem::path(filePath).extension() == ".vtk") {
    // Load .vtk file.
    pcl::PolygonMesh polygonMesh;
    pcl::io::loadPolygonFileVTK(filePath, polygonMesh);

CMakeLists.txt 中添加-lstdc++fs


target_link_libraries(read
  ${catkin_LIBRARIES}
  -lstdc++fs
)

重新编译不再报错，编译完成后，如下，启动两个终端，来演示simple_demo 和Ground Truth Demo

# 终端1，激活环境
source ~/ele_map_ws/devel/setup.bash
roslaunch elevation_mapping_demos simple_demo.launch

或者Ground Truth Demo

# 终端2，激活环境
source ~/ele_map_ws/devel/setup.bash
roslaunch elevation_mapping_demos ground_truth_demo.launch

启动后，都提示 [ WARN] [1690391331.761879154] : Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead的问题，其实就是配置文件中的点云topic需要更改成数据中的点云topic，手边暂时没数据就不试了，后续有空再补。
simple_demo 和Ground Truth Demo分别在以下两个文件中修改：

~/ele_map_ws/src/elevation_mapping/elevation_mapping_demos/config/robots/simple_demo_robot.yaml
~/ele_map_ws/src/elevation_mapping/elevation_mapping_demos/config/robots/ground_truth_demo.yaml

请添加图片描述

至此，成功编译安装elevation mapping高程图，并安装turtlebot3机器人仿真库，搭配turtlebot3机器人运行示例熟悉elevation mapping，同时也熟悉了一个Ground Truth Demo，进一步熟悉elevation mapping.

Reference: