cartographer amcl 栅格地图统一

一、缘由

 1.occupancy_grid_node_main.cc节点将子图转化为栅格地图发出去

cartographer运行过程中动态发布的nav_msgs/OccupancyGrid格式的topic可作为地图用于navigation。

occupancy_grid_node_main.cc 发布出的栅格地图 值 -1  【0-100】 的int value

实测可以直接使用，但在运动中全局路径规划靠近障碍物

2.amcl接收网格地图后对数据做了处理：

/**
 * 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;isize_x * map->size_y;i++)
  {
    if(map_msg.data[i] == 0)
      map->cells[i].occ_state = -1;
    else if(map_msg.data[i] == 100)
      map->cells[i].occ_state = +1;
    else
      map->cells[i].occ_state = 0;
  }

  return map;
}

3.map_server 地图保存

      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) { //occ [0,0.1)
            fputc(254, out);
          } else if (map->data[i] == +100) { //occ (0.65,1]
            fputc(000, out);
          } else { //occ [0.1,0.65]
            fputc(205, out);
          }
        }
      }

4.map_server中地图加载

      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;
      else if(occ < free_th)
        value = 0;
      else if(mode==TRINARY || alpha < 1.0)
        value = -1;
      else {
        double ratio = (occ - free_th) / (occ_th - free_th);
        value = 99 * ratio;
      }

默认参数：

resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

二、修改

 即对OccupancyGrid中数据为100的网格视为障碍物，将数据为0的网格视为未知，其余均视为可通行。然而cartographer中对于OccupancyGrid的处理略有区别，其发布的网格地图几乎没有100或0的数据，即发布的地图没有障碍物，导致amcl粒子检测不到障碍物无法收敛。故若想将该动态地图的topic用于amcl，需要在cartographer发布前做一定处理。

    cartographer发布OccupancyGrid的程序位于occupancy_grid_node_main.cc中。关注CreateOccupancyGridMsg这一函数的调用。该函数将地图数据具体封装成nav_msgs/OccupancyGrid格式。其实现位于msg_conversion.cc中，故做出相应修改：

std::unique_ptr 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;
}

      修改完毕后，该动态地图可用于navigation中的amcl定位。

      应注意在amcl中使用map的topic应相应配置好参数，如use_map_topic等。

  同时若应用于navigation时应配置好全局的costmap，在costmap_common_params.yaml中将subscribe-_to_updates（static_layer）打开。