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

亦凉 2022-04-13 15:07 462阅读 0赞

### 文章目录 ###

*  效果图
 *  OccupancyGrid概述
 *  OccupancyGrid使用注意事项
 *  创建工程
 *  在Rviz中实时现实栅格地图
 *  结束语

--------------------

# 效果图 #

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

--------------------

# OccupancyGrid概述 #

[官方数据类型解释][Link 1]如下：

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2][] 2.创建栅格地图 点击左下角添加按钮\`Add\`，在打开的\`Create visualization\`栏中选择\`Map\`，并点击\`OK\`键确认。 ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 3][] 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); \`\`\` ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 4][]

--------------------

# 结束语 #

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

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: /images/20220413/83399ac82c0745e1b6390c48e4786431.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]: /images/20220413/abaaa2d2225f4f4884d2e99a2d1ab93b.png
[Link 1]: http://docs.ros.org/kinetic/api/nav_msgs/html/msg/OccupancyGrid.html
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20181207200025824.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 3]: https://img-blog.csdnimg.cn/20181207150823101.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 4]: /images/20220413/4b0fc801819a4408bccab59072f18fdc.png