ROS——SLAM实时更新显示栅格地图之nav_msgs::OccupancyGrid

---

效果图

  废话不说，直接上最终效果图～看完图你应该就知道是不是你想要的东东。这是对Gazebo其中一个部分场景的无人机SLAM效果图。黑色块为障碍物，颜色较深的灰色块为危险区，颜色再浅一点的是可行区域，最浅的是未知区域（最外围的区域）；
Rviz实时SLAM重建效果图如下：

Gazebo场景图如下：

---

OccupancyGrid概述

官方数据类型解释如下：

nav_msgs/OccupancyGrid Message
File: nav_msgs/OccupancyGrid.msg
Raw Message Definition

# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.

Header header 

#MetaData for the map
MapMetaData info

# The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data

消息定义格式如下：

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

---

OccupancyGrid使用注意事项

1.向Rviz发送的数据中一定要包含frame_id

map.header.frame_id="grid";

2.栅格地图初始化大小为

map.info.width      = 20;           // 宽
map.info.height     = 20;           // 高

3.栅格地图分辨率对应栅格地图中一小格的长和宽

map.info.resolution = 0.5;

4.栅格地图中每一个小格的坐标对应一位数组中的一个数据。
例如：
实际地图中某点坐标为(x,y)，对应栅格地图中坐标为[x*map.info.width+y]。

5.栅格地图中每个小格的颜色代表的是该格被占用的概率，颜色越深表明被占用率越高，也就是有更高的可能是障碍物/建筑物。为什么是概率那，因为建立栅格地图要依赖于GPS或其他导航系统，导航系统都是有误差的，哪怕精度再高，因此基于这些位置信息建立出来的栅格地图存在一定的不准确度，就使用概率来表示。概率的范围是[0,100]，如果是未知区域可以将该位置的概率设置为-1。

---

创建工程

1.创建工程所在文件夹

mkdir grid
cd grid
mkdir src

2.创建ROS工程的grid包

catkin_create_pkg grid std_msgs rospy roscpp

3.在创建好的ROS工程内建立新的gridMap.cpp文件，并添加如下程序

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/OccupancyGrid.h"

int main(int argc, char * argv[]) {

  ros::init(argc, argv, "gridMap");

  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/gridMap", 1);
  nav_msgs::OccupancyGrid map;

  map.header.frame_id="grid";
  map.header.stamp = ros::Time::now(); 
  map.info.resolution = 0.5;         // float32
  map.info.width      = 20;           // uint32
  map.info.height     = 20;           // uint32
  
  int p[map.info.width*map.info.height] = {-1};   // [0,100]
  p[10] = 100;
  std::vector<signed char> a(p, p+400);
  map.data = a;

  while (ros::ok())
  {
      pub.publish(map);
  }

  ros::shutdown();
  return 0;
}

4.修改CMakeLists.txt文件，在文件中添加如下程序

add_executable(gridMap
  src/gridMap.cpp
)
add_dependencies(gridMap ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(gridMap
  ${catkin_LIBRARIES}
)

5.编译工程

catkin_make

6.运行工程
6.1.首先运行

roscore

6.2.运行新建的结点

source devel/setup.bash
rosrun grid gridMap

---

在Rviz中实时现实栅格地图

1.打开Rviz

rosrun rviz rviz

2.创建栅格地图 点击左下角添加按钮`Add`，在打开的`Create visualization`栏中选择`Map`，并点击`OK`键确认。 3.打开`Map`下拉列表中的属性信息，在`Topic`话题后面输入/选择`/gridMap`话题，即可看到删咯地图在Rviz中现实的结果。此时虽然会现实栅格地图，但是会出现Error提示，因此修改`Global Option`属性中的`Fixed Frame`对应程序中的`grid`，这时错误提示消失，正常显示删咯地图。 **注释** `Global Option`属性中的`Fixed Frame`对应程序中向Rviz发布消息的消息头中的`frame_id`，即 ```c map.header.frame_id="grid"; ``` `Map`属性中的`Topic`对应程序中向Rviz发布消息的话题名，即 ```c ros::Publisher pub = nh.advertise("/gridMap", 1); ```

---

结束语

至此已经可以在Rviz中显示栅格地图，快去实现你自己的SLAM吧～