Moving the robot tears costmap apart

edit

I use cartographer as SLAM to generate a map based on laser information. To perform automated exploration, I need to extract information from the generated map. I use the transform node in cartographer to get the nav_msgs::OccupancyGrid.

In my exploration node, I have a costmap_2d::Costmap2DROS object to hold the map information. The parameter file is here:

global_costmap:
  global_frame: "map"
  robot_base_frame: "base_link"
  transform_tolerance: 0.2
  update_frequency: 5
  publish_frequency: 1
  rolling_window: false
  always_send_full_costmap: false

  plugins:
    - {name: cartographer_layer, type: "costmap_2d::StaticLayer"}
    # - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
    # - {name: inflation, type: "costmap_2d::InflationLayer"}

  cartographer_layer:
    map_topic: "/map"
    first_map_only: false
    # there is no /map_updates topic from cartographer
    subscribe_to_updates: false
    track_unknown_space: true
    use_maximum: false
    unknown_cost_value: -1
    trinary_costmap: true

The map looks good in the beginning: (this is only the costmap without the cartographer map)

But as soon as the robot starts rotating/moving, the map looks like this: (The good map in the background is the "/map" topic from cartographer)

I thought this might be a tf issue. But I'm not sure. The Map is published in the correct frame and my tf tree looks fine too.

I've used a raw costmap2d before and used this code to update my map. There everything worked as intended.

void IndoorExplorer::updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{    
  unsigned int size_in_cells_x = msg->info.width;
  unsigned int size_in_cells_y = msg->info.height;
  double resolution            = msg->info.resolution;
  double origin_x              = msg->info.origin.position.x;
  double origin_y              = msg->info.origin.position.y;

  debug_msg("Received full new map, resizing to:", size_in_cells_x, ", ", size_in_cells_y);
  costmap->resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x, origin_y);

  // lock as we are accessing raw underlying map
  auto* mutex = costmap->getMutex();
  std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);

  // fill map with data
  unsigned char* costmap_data = costmap->getCharMap();
  size_t costmap_size         = costmap->getSizeInCellsX() * costmap->getSizeInCellsY();
  debug_msg("Full map update,", costmap_size, "values");
  for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i)
  {
    unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
    costmap_data[i]         = cost_translation_table__[cell_cost];
  }
  debug_msg("Map updated, written", costmap_size, "values");
}

So what is wrong with the update function of Costmap2DROS?

I'm currently hacking my way through the code, but I've no clue whats going on there.

Edit: This is the last time, I'll edit this post, because it looks like there are no incrementel updates. Everytime I click edit, I loos everything that was not in my original post. This is the worst.