grid_map（一）：grid_map学习
0.前言
作为一个学感知的人，学了一年半，没有一个拿出手的感知建图程序，自认为自己是非常惭愧的，于是希望多挖掘grid_map这个强大package的功能；这个系列自己一定要更新到自己学明白呀呀呀呀。
github代码：https://github.com/ANYbotics/grid_map
https://github.com/ANYbotics/grid_map
1.grid map概述
它是一个具有ROS接口的C++库，用于管理具有多个数据层multiple data layers的二维网格地图two-dimensional grid maps。它专为移动机器人测绘而设计，用于存储高程elevation、方差variance、颜色color、摩擦系数friction coefficient、立足点质量foothold quality、表面法线surface normal、可穿越性traversability等数据。它用于以机器人为中心的高程测绘软件包中，该软件包专为崎岖地形导航rough terrain navigation而设计。
特征：
- Multi-layered多层：为通用2.5维(2.5-dimensional)网格映射而开发，支持任意数量的层(any number of layers)。
- Efficient map re-positioning高效地图重新定位：数据存储作为二维循环缓冲区(two-dimensional circular buffer)实现。这允许地图位置(map's position)的非破坏性移动（例如跟随机器人），而无需复制内存中的数据。
- 基于Eigen：栅格地图数据存储为Eigen数据类型。用户可以将可用的Eigen算法(Eigen algorithms)直接应用于地图数据，以实现多功能(versatile)和高效的数据操作。
- Convenience functions方便的功能：几种辅助方法允许方便且内存安全的单元数据访问。例如，实现了矩形、圆形、多边形区域和直线的迭代器函数(iterator functions)。
- ROS接口：栅格地图（grid map）可以和ROS消息类型相互转换，如PointCloud2、OccupencyGrid、GridCells和我们的自定义栅格地图（grid map）消息。转换包提供与costmap_2d、PCL和OctoMap数据类型的兼容性。
- OpenCV接口：网格贴图可以无缝地从OpenCV图像类型转换为OpenCV图像类型，以利用OpenCV提供的工具。
- 可视化：grid_map_rviz_plugin 在rviz中将栅格贴图渲染为三维曲面图（高度图）。此外，grid_map_visualization软件包有助于将栅格地图可视化为点云、占用栅格、栅格单元等。
- filters 过滤器：grid_map_filters 提供了一系列过滤器，用于将栅格地图作为一系列过滤器进行处理。解析数学表达式允许灵活地设置强大的计算，如阈值thresholding、法向量（normal vectors）、平滑（smoothening）、方差（variance）、修复（inpainting）和矩阵核卷积（matrix kernel convolutions）。
作者：
Author: Péter Fankhauser
Affiliation: ANYboticsThis projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab).
This work is conducted as part of ANYmal Research, a community to advance legged robotics.
grid_map_rviz_plugin_example
2.grid_map安装与编译
2.1 安装
sudo apt-get install ros-$ROS_DISTRO-grid-map####安装成功
正在设置 ros-melodic-grid-map-visualization (1.6.4-2bionic.20211014.180708) ...
正在设置 ros-melodic-grid-map-loader (1.6.4-2bionic.20211014.180838) ...
正在设置 ros-melodic-grid-map-rviz-plugin (1.6.4-2bionic.20211216.191104) ...
正在设置 ros-melodic-grid-map-filters (1.6.4-2bionic.20211014.180708) ...
正在设置 ros-melodic-grid-map-octomap (1.6.4-2bionic.20210504.142440) ...
正在设置 ros-melodic-grid-map-demos (1.6.4-2bionic.20211216.191922) ...
正在设置 ros-melodic-grid-map (1.6.4-2bionic.20211216.192643) ...2.2 编译与测试
2.2.1 依赖
线性代数库（the linear algebra library ）Eigen：
sudo apt-get install libeigen3-dev其他依赖库：
##可能已经安装了
##没有安装就
#####sudo apt-get install ros-$ROS_DISTRO-roscpp
#####sudo apt-get install ros-$ROS_DISTRO-filters
roscpp, tf, filters, sensor_msgs, nav_msgs, and cv_bridge2.2.2 编译
cd grid_map_ws/src
git clone https://github.com/anybotics/grid_map.git
cd ../
catkin_make为了最大限度地提高性能，请确保以发布(Release)模式构建
catkin_make -DCMAKE_BUILD_TYPE=Release2.2.3 编译报错1
fatal error: filters/filter_base.hpp: 没有那个文件或目录
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp:11:10: fatal error: filters/filter_base.hpp: 没有那个文件或目录
#include
^~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
将对应文件中的
文件地址：/grid_map_ws/src/grid_map/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp:11:10
//修改方法
// #include
#include 参考链接：https://github.com/ANYbotics/grid_map/issues/292https://github.com/ANYbotics/grid_map/issues/292
2.2.4 编译报错2
error: ‘Rect2f’ is not a member of ‘cv’
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_cv/src/GridMapCvProcessing.cpp:85:7: error: ‘Rect2f’ is not a member of ‘cv’
cv::Rect2f boundingBox;
^~~~~~
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_cv/src/GridMapCvProcessing.cpp:85:7: note: suggested alternative: ‘Rect’
cv::Rect2f boundingBox;
^~~~~~
Rect
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_cv/src/GridMapCvProcessing.cpp:102:7: error: ‘boundingBox’ was not declared in this scope
boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();
^~~~~~~~~~~
试着按照终端中“suggested alternative: ‘Rect’”修改。
// cv::Rect2f boundingBox;
cv::Rect boundingBox;修改后报错如下，继续按照提示修改：
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_cv/src/GridMapCvProcessing.cpp:103:91: error: ‘class cv::RotatedRect’ has no member named ‘boundingRect2f’; did you mean ‘boundingRect’?
boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();
^~~~~~~~~~~~~~
boundingRect
// boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();
boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect();2.2.5 编译报错3
参考编译报错1修改
这里有较多.hpp文件需要修改
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp:14:10: fatal error: filters/filter_base.hpp: 没有那个文件或目录
#include
^~~~~~~~~~~~~~~~~~~~~~~~~
这个文件夹下面的几乎都要进行修改
grid_map_ws/src/grid_map/grid_map_filters/include/grid_map_filters2.2.6 编译报错4
fatal error: filters/filter_chain.hpp: 没有那个文件
/home/elfoot/learning/grid_map_ws/src/grid_map/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp:17:10: fatal error: filters/filter_chain.hpp: 没有那个文件或目录
#include
^~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
对应文件：
// #include
#include 其他需要一样修改的文件：
grid_map_ws/src/grid_map/grid_map_demos/src/normal_filter_comparison_node.cpp2.2.7 编译测试
##使用catkin_make
catkin_make run_tests_grid_map_core run_tests_grid_map_ros
##使用catkin_build
catkin build grid_map --no-deps --verbose --catkin-make-args run_tests测试没有问题：
-- run_tests.py: execute commands
/home/elfoot/learning/grid_map_ws/devel/lib/grid_map_ros/grid_map_ros-test --gtest_output=xml:/home/elfoot/learning/grid_map_ws/build/test_results/grid_map_ros/gtest-grid_map_ros-test.xml
[==========] Running 7 tests from 4 test cases.
[----------] Global test environment set-up.
[----------] 1 test from RosMessageConversion
[ RUN ] RosMessageConversion.roundTrip
[ OK ] RosMessageConversion.roundTrip (0 ms)
[----------] 1 test from RosMessageConversion (0 ms total)
[----------] 2 tests from RosbagHandling
[ RUN ] RosbagHandling.saveLoad
[ OK ] RosbagHandling.saveLoad (43 ms)
[ RUN ] RosbagHandling.saveLoadWithTime
[ OK ] RosbagHandling.saveLoadWithTime (2 ms)
[----------] 2 tests from RosbagHandling (45 ms total)
[----------] 2 tests from OccupancyGridConversion
[ RUN ] OccupancyGridConversion.withMove
[ OK ] OccupancyGridConversion.withMove (0 ms)
[ RUN ] OccupancyGridConversion.roundTrip
[ OK ] OccupancyGridConversion.roundTrip (1 ms)
[----------] 2 tests from OccupancyGridConversion (1 ms total)
[----------] 2 tests from ImageConversion
[ RUN ] ImageConversion.roundTripBGRA8
Segmentation fault (core dumped)
-- run_tests.py: verify result "/home/elfoot/learning/grid_map_ws/build/test_results/grid_map_ros/gtest-grid_map_ros-test.xml"
Cannot find results, writing failure results to '/home/elfoot/learning/grid_map_ws/build/test_results/grid_map_ros/MISSING-gtest-grid_map_ros-test.xml'
[100%] Built target run_tests_grid_map_ros_gtest_grid_map_ros-test
Scanning dependencies of target run_tests_grid_map_ros_gtest
[100%] Built target run_tests_grid_map_ros_gtest
Scanning dependencies of target run_tests_grid_map_ros
[100%] Built target run_tests_grid_map_ros
3 使用演示
grid_map_demos包包括一些演示节点
3.1 simple_demo
该节点创建一个栅格地图(a grid map)，向其中添加数据并且发布出来；最后用rviz查看结果。
source devel/setup.bash
roslaunch grid_map_demos simple_demo.launch
3.2 tutorials_demo
是grid_map库功能的拓展演示
roslaunch grid_map_demos tutorial_demo.launch
3.3 iterators_demo
演示grid_map的迭代功能
roslaunch grid_map_demos iterators_demo.launch
视频链接：https://www.bilibili.com/video/BV1r34y1q7Hm/
https://www.bilibili.com/video/BV1r34y1q7Hm/
3.4 image_to_gridmap_demo
转换图片到栅格地图
roslaunch grid_map_demos image_to_gridmap_demo.launch出现报错：
OpenCV Error: The function/feature is not implemented (Unknown/unsupported array type) in getMat_, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matrix.cpp, line 1289
terminate called after throwing an instance of 'cv::Exception'
what(): /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matrix.cpp:1289: error: (-213) Unknown/unsupported array type in function getMat_
[image_to_gridmap_demo-3] process has died [pid 29170, exit code -6, cmd /home/elfoot/learning/grid_map_ws/devel/lib/grid_map_demos/image_to_gridmap_demo __name:=image_to_gridmap_demo __log:=/home/elfoot/.ros/log/809e4ca0-6f05-11ec-b31f-08beac26819a/image_to_gridmap_demo-3.log].
log file: /home/elfoot/.ros/log/809e4ca0-6f05-11ec-b31f-08beac26819a/image_to_gridmap_demo-3*.log
修改CMakeLists.txt中的OpenCV_DIR，涉及三处，报错变化
add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(OpenCV_DIR "/usr/local/opencv3/share/OpenCV") ######add
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${OCTOMAP_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS} ######add
)
target_link_libraries(
image_to_gridmap_demo
${catkin_LIBRARIES}
${OpenCV_LIBRARIES} ######add
)OpenCV Error: Assertion failed (0 <= i && i < (int)vv.size()) in getMat_, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matrix.cpp, line 1244
terminate called after throwing an instance of 'cv::Exception'
what(): /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matrix.cpp:1244: error: (-215) 0 <= i && i < (int)vv.size() in function getMat_
先记个思路：可能是opencv编译安装时留下的问题，稍后有时间解决
------更多示例见github：https://github.com/ANYbotics/grid_maphttps://github.com/ANYbotics/grid_map
@meng