Ask Your Question

Moving the robot tears costmap apart

asked 2020-05-03 06:27:51 -0500

Darkproduct gravatar image

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_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

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

    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) Gazebo image Generated 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) Generated map after moving

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. Tf frames in rviz Tf tree

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.

edit retag flag offensive close merge delete



Maybe this bug?

Try to roll back to previous commits

Or fetch latest commits, seems like it's fixed.

artemiialessandrini gravatar image artemiialessandrini  ( 2020-05-03 22:43:08 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-05-03 22:37:45 -0500

I believe what you're seeing is this:

Resolved in There has been releases since then ( so I think it just needs to be synced to apt. In the mean-time, build from source.

edit flag offensive delete link more


I'm not sure if that's the issue, because I've installed the kinetic version of the costmap2d package. ros-kinetic-costmap-2d/now 1.14.5-1xenial-20191214-114303+0000 amd64

I believe I use this function.

Darkproduct gravatar image Darkproduct  ( 2020-05-04 04:46:24 -0500 )edit

Maybe you are right. I've read issue #792 where the problem starts, and that is in kinetic. I'll lock more into this.

Darkproduct gravatar image Darkproduct  ( 2020-05-04 05:41:07 -0500 )edit

I confirmed that the map can be updated, but not resized, by moving obstacles in the FOV, which works fine.

Darkproduct gravatar image Darkproduct  ( 2020-05-04 05:48:35 -0500 )edit

This solved my problem. I had version 1.14.5 installed and updated it to 1.14.7. Now everything looks fine.

Darkproduct gravatar image Darkproduct  ( 2020-05-05 06:09:22 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2020-05-03 05:11:31 -0500

Seen: 69 times

Last updated: May 03 '20