占据栅格图 Octomap | Mian的笔记

占据栅格图octomap

在传统分阶段（感知-建图-规划-优化）任务中，环境建图是非常重要的一环。octomap是一个开源的三维环境建模库，其基于八叉树octree实现，可以将三维点云数据转换为三维地图

以Ubuntu 20.04 + ROS noetic为例，安装过程如下

sudo apt-get update
sudo apt-get install ros-noetic-octomap ros-noetic-octomap-mapping ros-noetic-octomap-server
# rviz可视化插件
sudo apt-get install ros-noetic-octomap-rviz-plugins
# 离线查看工具，可直接查看地图.bt文件
sudo apt-get install octovis

确保机器人有能提供三维点云数据的传感器

octomap在使用时是作为一个ROS节点而存在，其输入为点云+定位信息，输出占据栅格+可视化标记。最基本的启动方式为roslaunch octomap_server octomap_mapping.launch，可以启动rviz添加octomap，选择/octomap_full进行查看

一般可以通过launch文件启动，便于配置参数

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    
    <!-- ================= 核心坐标系 ================= -->
    <!-- frame_id: 世界坐标系 (地图的根)，通常是 VINS 输出的 "world" 或 "map" -->
    <param name="frame_id" type="string" value="world" />
    
    <!-- base_frame_id: 机器人的基座，通常是 "body_link" 或 "base_link" -->
    <!-- 用于滤除地面或机器人自身的点云 (可选) -->
    <param name="base_frame_id" type="string" value="body_link" />

    <!-- ================= 建图参数 ================= -->
    <!-- resolution: 格子大小 (米)。0.05 代表 5cm -->
    <param name="resolution" value="0.05" />

    <!-- max_range: 传感器最大有效距离 -->
    <!-- D435 建议设为 4.0 - 5.0。超过这个距离的点云不可信，不用于建图 -->
    <param name="sensor_model/max_range" value="5.0" />
    
    <!-- ================= 动态更新逻辑 ================= -->
    <!-- latch: false 表示地图会实时刷新。如果门开了，旧的障碍物会消失。 -->
    <!-- latch: true 表示只增不减。适合完全静态的环境。 -->
    <param name="latch" value="false" /> 

    <!-- 概率参数 (通常用默认即可) -->
    <!-- hit: 击中障碍物增加的概率; miss: 光线穿过减少的概率 -->
    <param name="sensor_model/hit" value="0.7" />
    <param name="sensor_model/miss" value="0.4" />
    
    <!-- ================= 地面滤除 (可选) ================= -->
    <!-- 如果不想要把地板建成障碍物，开启 filter_ground -->
    <param name="filter_ground" value="false" />

    <!-- ================= 数据输入 ================= -->
    <!-- cloud_in: 你的点云话题名字 -->
    <remap from="cloud_in" to="/camera/depth/color/points" />
  
  </node>
</launch>

使用D435 的点云

d435虽然本身只能输出RGB-D图像，但是其内置的驱动可以通过计算输出点云数据，且该点云可以附带颜色，操作方式如下

1	roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

但是显然，由深度相机所得到的点云缺点很明显，首先是距离近，距离如果太远了相机的精度下降，其内容不可信，如果强行纳入计算则原处的点可能波动很大；其次是频率低，该点云的输出频率依赖于相机图像的频率；最后，由于依赖于深度图像，当面对如纯白墙面或者昏暗环境等深度相机不起作用的情况，也无法输出点云。可见，通过这种方式得到点云肯定是比不上由激光雷达得到点云。

占据栅格图的使用

除了直接在rviz中订阅地图来查看生成的占据栅格图，要如何使用地图来进行路径规划？

二维地图

数据结构

二维占据地图projected_map，表示二维地图上障碍物的位置，是octomap_server默认发布的话题，在CPP中需要#include <nav_msgs/OccupancyGrid.h>。

其数据类型为nav_msgs::OccupancyGrid，其具体数据结构有以下三部分

header
info
data

其中：

header.frame_id
表示地图坐标系，通常这里是 map。你工程里 octomap_server 也把 frame_id 设成了 map，见 src/astar_nav/launch/octomap.launch:5。
info.resolution
每个栅格边长，单位米/格。
info.width, info.height
地图宽高，单位是格子数，不是米。
info.origin
栅格 (0,0) 在世界坐标里的原点位姿。通常可理解为地图左下角对应的世界坐标。
data
一个一维 int8[] 数组，长度等于 width * height。

数组索引通常按行展开，访问方式就是： index = y * width + x。其中值为-1~100的整数，-1代表未知，0代表空闲，100表示占据，数字越大表示该位置被占用的概率越大

数据使用

订阅话题并设置回调函数
首先需要订阅话题/projected_map，并使用回调函数将数据传给同类型的类内变量raw_map；同时在回调函数中进行地图膨胀，而膨胀后的地图可以用std::vector<int8_t> inflated_data_表示。

地图膨胀
为了让无人机与障碍物保持安全距离，需要对地图进行膨胀。参考代码如下

void Planner::inflateMap(){
	int w = raw_map_.info.width;
	int h = raw_map_.info.height;
    double res = raw_map_.info.resolution;
    int radius = static_cast<int>(std::ceil(safety_dist_ / res));

    inflated_data_ = raw_map_.data;

    for (int y = 0; y < h; ++y)
    {
        for (int x = 0; x < w; ++x)
        {
            // 原始地图中该格为占据
            if (raw_map_.data[y * w + x] > 50)
            {
                // 将周围 radius 范围内的格子都标记为占据
                for (int dy = -radius; dy <= radius; ++dy)
                {
                    for (int dx = -radius; dx <= radius; ++dx)
                    {
                        int nx = x + dx;
                        int ny = y + dy;
                        if (nx >= 0 && nx < w && ny >= 0 && ny < h)
                        {
                            if (dx * dx + dy * dy <= radius * radius)
                                inflated_data_[ny * w + nx] = 100;
                        }
                    }
                }
            }
        }
    }
}

路径规划
使用得到的膨胀后的二维地图进行路径规划

三维地图

参考

ROS中的三维地图表示：OctoMap详解及应用-CSDN博客
 nav_msgs::OccupancyGrid数据格式详解-CSDN博客