ros下各個包中的map的格式

1 map_server

1.1 加載地圖

// 有三種模式

/** Map mode
 *  Default: TRINARY -
 *      value >= occ_th - Occupied (100)
 *      value <= free_th - Free (0)
 *      otherwise - Unknown
 *  SCALE -
 *      alpha < 1.0 - Unknown
 *      value >= occ_th - Occupied (100)
 *      value <= free_th - Free (0)
 *      otherwise - f( (free_th, occ_th) ) = (0, 100)
 *          (linearly map in between values to (0,100)
 *  RAW -
 *      value = value
 */
enum MapMode {TRINARY, SCALE, RAW};


// 默認閾值爲 0.65 0.196
private_nh.param("occupied_thresh", occ_th, 0.65);
private_nh.param("free_thresh", free_th, 0.196);



  // Copy pixel data into the map structure
  pixels = (unsigned char*)(img->pixels);
  for(j = 0; j < resp->map.info.height; j++)
  {
    for (i = 0; i < resp->map.info.width; i++)
    {
      // Compute mean of RGB for this pixel
      p = pixels + j*rowstride + i*n_channels;
      color_sum = 0;
      for(k=0;k<avg_channels;k++)
        color_sum += *(p + (k));
      color_avg = color_sum / (double)avg_channels;

      if (n_channels == 1)
          alpha = 1;
      else
          alpha = *(p+n_channels-1);

      if(negate)
        color_avg = 255 - color_avg;


      // RAW 模式，直接輸出像素值爲柵格值
      if(mode==RAW){
          value = color_avg;
          resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
          continue;
      }


      // If negate is true, we consider blacker pixels free, and whiter
      // pixels occupied.  Otherwise, it's vice versa.
      occ = (255 - color_avg) / 255.0;

      // Apply thresholds to RGB means to determine occupancy values for
      // map.  Note that we invert the graphics-ordering of the pixels to
      // produce a map with cell (0,0) in the lower-left corner.
      if(occ > occ_th)
        value = +100;    // 100 爲障礙物
      else if(occ < free_th)
        value = 0;       // 0 爲 free ，即沒有障礙物
      else if(mode==TRINARY || alpha < 1.0)
        value = -1;      // 如果是TRINARY模式，則其他情況賦值爲 -1 ，代表 未知
      else {             // 如果爲SCALE模式，則其他情況賦值 0-99 之間的值
        double ratio = (occ - free_th) / (occ_th - free_th);
        value = 99 * ratio;
      }

      resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
    }
  }

1.2 保存地圖

      
// map_server/src/map_saver.cpp


  int threshold_occupied_ = 65;
  int threshold_free_ = 25;

     for(unsigned int y = 0; y < map->info.height; y++) {
        for(unsigned int x = 0; x < map->info.width; x++) {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
            fputc(254, out); // 白色
          } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
            fputc(000, out);  // 黑色
          } else { //occ [0.25,0.65]
            fputc(205, out);  // 灰色
          }
        }
      }

 

2 GMapping保存的地圖

  if(!private_nh_.getParam("occ_thresh", occ_thresh_))
    occ_thresh_ = 0.25;


  for(int x=0; x < smap.getMapSizeX(); x++)
  {
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
      /// @todo Sort out the unknown vs. free vs. obstacle thresholding
      GMapping::IntPoint p(x, y);
      double occ=smap.cell(p);
      assert(occ <= 1.0);

      // occ < 0, 地圖填值 -1，代表未知
      if(occ < 0)
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
      // occ > 0.25, 地圖填值 100，佔用，代表障礙物
      else if(occ > occ_thresh_)
      {
        // 註釋裏爲輸出1-100 間的值
        //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
      }
      // 其他情況, 地圖填值 0， free， 代表沒有障礙物
      else
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
    }
  }

 

3 AMCL中的map

/**
 * Convert an OccupancyGrid map message into the internal
 * representation.  This allocates a map_t and returns it.
 */
map_t*
AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
  map_t* map = map_alloc();
  ROS_ASSERT(map);

  map->size_x = map_msg.info.width;
  map->size_y = map_msg.info.height;
  map->scale = map_msg.info.resolution;
  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
  // Convert to player format
  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
  ROS_ASSERT(map->cells);
  for(int i=0;i<map->size_x * map->size_y;i++)
  {
    // 將0，free 轉成 -1
    if(map_msg.data[i] == 0)
      map->cells[i].occ_state = -1;
    // 將100，佔用 轉成 +1
    else if(map_msg.data[i] == 100)
      map->cells[i].occ_state = +1;
    // 將其他情況 轉成 0
    else
      map->cells[i].occ_state = 0;
  }

  return map;
}

 

4 cartographer_ros中的地圖

4.1 默認實現

如果觀測到了，輸出 0-100中間的值，如果沒觀測到，輸出 -1

for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
      const uint32_t packed = pixel_data[y * width + x];
      const unsigned char color = packed >> 16;
      const unsigned char observed = packed >> 8;
      const int value =
          observed == 0
              ? -1
              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
      CHECK_LE(-1, value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }

4.2 轉換成AMCL能用的地圖

std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
  ...
  for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
      const uint32_t packed = pixel_data[y * width + x];
      const unsigned char color = packed >> 16;
      const unsigned char observed = packed >> 8;
 
      int value_temp = ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
      if (value_temp > 100*0.65)
          value_temp = 100;
      else if (value_temp < 100*0.195)
          value_temp =  0;
      else
          value_temp += 0;  
      const int value =
          observed == 0
              ? -1
              : value_temp;
      CHECK_LE(-1, value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }
 
  return occupancy_grid;
}

 

references

1 https://blog.csdn.net/xiaoma_bk/article/details/82963731 --- cartographer amcl 柵格地圖統一