关于ROS的OccupancyGrid数据对应的地图颜色

最新推荐文章于 2025-02-24 21:30:00 发布

原创最新推荐文章于 2025-02-24 21:30:00 发布 · 5.3k 阅读

39 ·

CC 4.0 BY-SA版权

SLAM 同时被 2 个专栏收录

24 篇文章

订阅专栏

ROS

5 篇文章

订阅专栏

本文探讨了ROS中的2D占用栅格地图（OccupancyGrid）及其颜色表示。介绍了OccupancyGrid消息类型的关键组成部分，包括header、info和data。详细解释了不同值（-128至127）在rviz中对应的颜色，特别是值100（占用）、0（空闲）和-1（未知）的颜色，并展示了16x16地图的完整颜色序列。同时，提供了查看地图数据的命令及实验代码。

最近在搞基于ROS的2D占用栅格地图，这类地图可用于机器人定位，它在机器人导航和避障方面很常用。它的消息类型是nav_msgs/OccupancyGrid，通过命令：rosmsg show nav_msgs/OccupancyGrid 可以看到nav_msgs/OccupancyGrid消息包含如下信息：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
int8[] data

对于一个nav_msgs/OccupancyGrid类型的map_msg，它主要由map_msg.header, map_msg.info, map_msg.data 三个主要信息，map_msg.header包含了发布地图的序列号（seq），时间戳（stamp）和帧id信息；map_msg.info包含了发布的地图的加载时间，分辨率，宽度，高度，地图起点坐标和方向；map_msg.data 数组包含了地图的元数据信息，可以看到，它是int8类型的，也就是说每个元数据的值范围是-128~127之间，在链接http://wiki.ros.org/map_server 中提到，
在这里插入图片描述
值100表示单元被占用；值0表示空闲，即未占用；-1（无符号类型下的255）表示未知。我们知道，这里不同的值含义不同，它们在rviz可视化界面上显示的颜色也不同，如下图所示

其中的黑色（值100）边界表示占用，图中红色箭头所在的灰色区域表示空闲（值0），而外围区域（深灰色？浅青色？）表示未知（-1）。这里我不禁好奇（好吧，我承认我是闲的。。。），那-128~127之间其他的值是什么颜色的呢，我发现答案可不是由黑到白色的一个过渡，这中间有彩色。在这里插入图片描述
如上图所示，中间R,G,B颜色的坐标轴分别代表x,y,z的正方向。在这个 $16\times16$ 的栅格地图中（分辨率为1m），从下往上，从右往左对应的单元的像素值分别是：-128，-127，…，0，1，…，127。右下角(-128)是红色，左上角(127)是绿色。上图显示的所有颜色就是map_msg.data中的int8类型的数值范围-128~127所代表的所有颜色（虽然我也很懵逼为啥是这么个对应关系）。通过 rostopic echo /grid_map 命令输出地图消息，显示如下：

header: 
  seq: 24
  stamp: 
    secs: 1591798531
    nsecs: 117001314
  frame_id: "map"
info: 
  map_load_time: 
    secs: 0
    nsecs:         0
  resolution: 1.0
  width: 16
  height: 16
  origin: 
    position: 
      x: -8.0
      y: -8.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
data: [-128, -127, -126, -125, -124, -123, -122, -121, -120, -119, -118, -117, -116, -115, -114, -113, -112, -111, -110, -109, -108, -107, -106, -105, -104, -103, -102, -101, -100, -99, -98, -97, -96, -95, -94, -93, -92, -91, -90, -89, -88, -87, -86, -85, -84, -83, -82, -81, -80, -79, -78, -77, -76, -75, -74, -73, -72, -71, -70, -69, -68, -67, -66, -65, -64, -63, -62, -61, -60, -59, -58, -57, -56, -55, -54, -53, -52, -51, -50, -49, -48, -47, -46, -45, -44, -43, -42, -41, -40, -39, -38, -37, -36, -35, -34, -33, -32, -31, -30, -29, -28, -27, -26, -25, -24, -23, -22, -21, -20, -19, -18, -17, -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127]

最后，奉上实验代码：

# include <iostream>
# include <ros/ros.h>
# include <nav_msgs/OccupancyGrid.h>


static ros::Publisher grid_pub;
static nav_msgs::OccupancyGrid gmap;
double grid_resolution = 1.0;
int grid_width = 16, grid_height = 16;
double origin_x, origin_y;
std::vector<double> logits(grid_height*grid_width, 0);
void grid_map_init()
{
  origin_x = -1.0*grid_resolution*grid_width/2.0;
  origin_y = -1.0*grid_resolution*grid_height/2.0;

  gmap.header.frame_id = "map";
  gmap.header.stamp = ros::Time::now();
  gmap.info.origin.position.x = origin_x;
  gmap.info.origin.position.y = origin_y; 
  gmap.info.origin.orientation.z = 0.0; 
  gmap.info.origin.orientation.w = 0.0; 
  gmap.info.resolution = grid_resolution;
  gmap.info.width = grid_width;
  gmap.info.height = grid_height;
  
  int v = -128;
  int8_t p[gmap.info.width * gmap.info.height];
  for (int i = 0; i < gmap.info.height; ++i)
    for (int j = 0; j < gmap.info.width; ++j)
      p[i*gmap.info.height + j] = v++;
  std::vector<int8_t> a(p, p+gmap.info.width * gmap.info.height);
  gmap.data = a;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_occupancy");
    ros::NodeHandle nh;
    
    grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("/grid_map", 1);
    grid_map_init();
    ros::Rate loop_rate(2);
    while (ros::ok())
    {
        grid_pub.publish(gmap);
        loop_rate.sleep();
    }
    
    return 0;
}