0
$\begingroup$

Rosanswers logo

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.


Originally posted by Darkproduct on ROS Answers with karma: 100 on 2020-05-03

Post score: 0


Original comments

Comment by artemiialessandrini on 2020-05-03:
Maybe this bug?

Try to roll back to previous commits

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

$\endgroup$

1 Answer 1

1
$\begingroup$

Rosanswers logo

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

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


Originally posted by stevemacenski with karma: 8272 on 2020-05-03

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Darkproduct on 2020-05-04:
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.

Comment by Darkproduct on 2020-05-04:
Maybe you are right. I've read issue #792 where the problem starts, and that is in kinetic. I'll lock more into this.

Comment by Darkproduct on 2020-05-04:
I confirmed that the map can be updated, but not resized, by moving obstacles in the FOV, which works fine.

Comment by Darkproduct on 2020-05-05:
This solved my problem. I had version 1.14.5 installed and updated it to 1.14.7. Now everything looks fine.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.