Global costmap cleared on incoming map update (static_layer)

[matlabbe]

I have a mapping node publishing a global map at 1 Hz. This map is received by the costmap static layer, clearing/updating his costmap. However, the layered_costmap_ is updated only inside the map update loop of CostMap2DROS. So until the updateMap() is called, the map is empty if it was resized. When move_base is planning (through navfn), the global costmap can be momentary "empty" if the map was not updated before makePlan() is called. A straight line (through obstacles) is then planned.

A solution without changing code is to increase the update_frequency parameter so that the map is updated before a new goal is sent to move_base. In my tests, I should increase the frequency over 30 Hz to have 90% to the time a good planning (when navfn is not planning in a momentary empty map). This solution is not computationally efficient if a new map is published only at 1 Hz (note that a new goal is sent just after the map is published by the mapping node).

To make sure that the costmap is always updated when a new map is received, and the update_frequency doesn't need to be increased, I added "layered_costmap_->updateMap(0,0,0);" at the end of the "void StaticLayer::incomingUpdate(...)" method. It works well if the layered costmap has only a static_layer and an inflation_layer. I didn't test if an obstacle_layer/footprint_layer is in the stack of used plugins (for which updateMap() may require not null robot position).

Maybe it could have a way so that costmap plugins can notify CostMap2DROS that an update of all layers is required instead of waiting the update in the thread loop.

Cheers

[mikeferguson]

We've done a number of improvements on mutexes over the past two years, but it looks like this one still exists. I think we need to do two more things thought:

• "staticLayer::IncomingMap()" needs to lock the costmap mutex for the duration (that way, it can't do a resize/clear during planning, as makePlan() has locked the cost map). This would probably drastically reduce the occurrence of this sort of problem, but:
• We should also be looking at isCurrent() in makePlan() so we don't plan in between the IncomingMap() and the UpdateMap().

[matlabbe]

For reference, there is a workaround with rtabmap_ros package (added 8 years ago). In global costmap parameters, instead of:

plugins:
   - {name: static_layer, type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

use:

plugins:
   - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}