修改cartographer动态发布的网格地图

      cartographer运行过程中动态发布的nav_msgs/OccupancyGrid格式的topic可作为地图用于navigation。但是amcl接收网格地图后对数据做了处理：

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++)
  {
    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;
}

       即对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<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 > 65)
          value_temp = 100;
      else if (value_temp < 10)
          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）打开。