nav_msgs::OccupancyGrid数据格式详解

nav_msgs::OccupancyGrid数据格式详解

可以先查看一下ros官网关于nav_msgs::OccupancyGrid数据格式的解释，我应用的时候，感觉还有一些不清楚的地方，所以我自己做了个实验，大家可以参考一下我对下面参数的理解，如有问题请告知。

主要是涉及实际地图与栅格地图单位的问题，实际地图的单位一般为m，栅格地图单位为一格，有些参数没讲清楚。

以比例尺为0.5,地图栅格大小为20×20，地图实际大小为10m×10m的地图举例说明：
nav_msgs::OccupancyGrid组成包括三个部分：

std_msgs/Header header
nav_msgs/MapMetaData info   
int8[] data      一维数组，数组中的数据为对应栅格的占用概率，范围[1,100],概率越大，
                 说明该点越有可能是障碍点，-1表示位置区域，数组元素的序列号计算为：
                 实际地图中某点坐标为(x,y)[单位m]，先转换为栅格右上角为原点的坐标，
                 即mx=(x-map.info.origin.x)/map.info.resolution,my=(y-map.info.origin.y)/map.info.resolution
                 对应栅格地图中坐标为[my*map.info.height+mx]。

下面重点讲一下nav_msgs/MapMetaData info数据格式，包括下面几个部分：

time map_load_time   //地图加载所需时间
float32 resolution   //比例尺，代表一个栅格在实际地图中的大小，比例尺为0.5,说明一个栅格长度为0.5m
uint32 width         //地图宽度，这里指的是栅格数量，不是指实际长度，也就是20,不是10
uint32 height        //同地图宽度
geometry_msgs/Pose origin  //地图原点所在位置,单位m

简单讲一下std_msgs/Header header数据格式：

uint32 seq       //帧序列号，会不断增长
time stamp     //时间戳，做数据融合时会用到
string frame_id  //地图参考的坐标系，比如我代码里参考的坐标系是真实地图下的odom坐标系

geometry_msgs/Pose origin 数据格式也有必要做一下说明，包含两个数据：

geometry_msgs/Point position    //栅格地图原点在真实地图下的坐标，x，y，z，
                                  指栅格点对应的实际位置坐标，单位为m，比如我设置的的
                                  原点为-10,0,则栅格原点在真实地图（-10,0）位置，即栅格
                                  地图整体向x轴负方向移动10m，由于比例尺为0.5,实际移动了20个栅格
geometry_msgs/Quaternion orientation  //坐标轴朝向，四元数表示，这个不讲，真实地图和栅格地图一样。

再说明一下rviz中地图的放置方法，栅格地图中有geometry_msgs/Pose origin数据，代表栅格地图原点在string frame_id （地图参考坐标系）下的坐标和朝向。将地图原点放到参考系对应坐标，地图坐标系/map旋转到对应朝向，其他点按栅格地图的位置关系放置。这里要分清给的坐标是相对与栅格地图/map坐标系下的坐标还是相对于真实地图/odom下的坐标。

下面是我的测试代码：

#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::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/gridMap1", 1);
  nav_msgs::OccupancyGrid map;

  map.header.frame_id="odom";
  map.header.stamp = ros::Time::now(); 
  map.info.resolution = 0.5;         // float32
  map.info.width      = 20;           // uint32
  map.info.height     = 20;           // uint32
  map.info.origin.position.x = -10.0;
  map.info.origin.position.y = 0.0;
  map.info.origin.position.z = 0.0;
  map.info.origin.orientation.x = 0.0;
  map.info.origin.orientation.y = 0.0;
  map.info.origin.orientation.z = 0.0;
  map.info.origin.orientation.w = 0.0;


  
  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;
}