Ask Your Question
0

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

Comments

1

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
0

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

I believe what you're seeing is this: https://github.com/ros-planning/navig...

Resolved in https://github.com/ros-planning/navig.... There has been releases since then (https://github.com/ros-planning/navig...) so I think it just needs to be synced to apt. In the mean-time, build from source.

edit flag offensive delete link more

Comments

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

2 followers

Stats

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

Seen: 69 times

Last updated: May 03 '20