elevation mapping算法1之Ubuntu18.04+ROS-melodic编译安装elevation mapping高程图及示例运行

0 引言

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

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

• config：这个子文件夹包含了一些机器人、高程图和可视化等一些.yaml的配置文件。

• doc：这个子文件夹包含了elevation mapping高程图的一些说明图片的文档等。

• include：这个子文件夹包含了elevation mapping高程图的头文件，这些头文件定义了elevation mapping高程图中使用的各种数据类型、函数和类等。

• src：这个子文件夹包含了elevation mapping高程图的源代码，包括数据下载和处理模块、数据处理和分析模块、数字高程模型生成和输出模块、用户接口模块等。

• test：这个子文件夹包含了elevation mapping高程图的测试代码和测试数据。

• launch：这个子文件夹包含了一些与elevation mapping高程图相关的roslaunch启动脚本。

• rviz：这个子文件夹包含了一些与elevation mapping高程图rviz可视化的模板。

• scripts：这个子文件夹包含了一些与elevation mapping高程图的一些python代码脚本。

这些子工程提供了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/srcgit clone https://github.com/anybotics/elevation_mapping.git
git clone https://github.com/ANYbotics/kindr_ros.gitcd ..
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 ofthe following names:kindrConfig.cmakekindr-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 tfFile "/opt/ros/melodic/lib/python2.7/dist-packages/tf/__init__.py", line 30, in <module>from tf2_ros import TransformException as Exception, ConnectivityException, LookupException, ExtrapolationExceptionFile "/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，当然还有pose的topic和帧率的输入，手边暂时没带里程计pose和点云的数据包，就不试了，后续有空再补。
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:

• https://github.com/ANYbotics/elevation_mapping
• https://blog.csdn.net/BIT_HXZ/article/details/127297351

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔