costmap_2d::StaticLayer-plugin of layered-costmap does not respect the maps coordinate frame in updateBounds() calculation

[bitmeal]

This issue is filed for documentation purposes and helping other users find out about the problem and classifying their observations. A pull request solving the issue has been made: #900

scenario

The image below illustrates the following scenario:

• a map is served from a map-server and used in the global_costmap and local_costmap
• the global_costmap and the local_costmap both use one single costmap_2d::StaticLayer exclusively
• global_costmap uses the /map-frame (bottom left)
• the local_costmap uses /odom as its global_frame (global_frame: /odom) and is a rolling window round the robot with frame /base_link

To reproduce the following error, it is important that no other layer request another portion of the local costmap to be updated!

The real-world use-case uses an external tracking- and observation-system using cameras, which generates robot poses (as tf), as well as obstacle information as a map-server compatible node. Therefore only costmap_2d::StaticLayers with different resolutions and update rates are used.

The images here are taken from a simulation, using gazebo and a static map served by the map_server-node.

behavior

As can be observed, the local costmap (axis aligned to /odom-frame) is only filled with data in the positive sector of the /odom-frame. The areas of interest are highlighted with orange circles and extensions of the axes are show corresponding to their color (red, green) in the image below.

Expected behavior would be: all areas visible as marked in the global_costmap to be marked in the local_costmap.

Possible observations include:

• updates of the positive costmap-frames sector only
• updates only in specific regions

the error

When updating a layered costmap, costmap_2d::LayeredCostmap asks all layers for the region they want to update in the next cycle. The region is fetched using the updateBounds()-method. updateBounds operates in meters, and specifies an axis-aligned bounding-box (minx_, maxx_, miny_, maxy_) to be updated, in the costmaps frame.

The updateBounds()-method, as implemented by costmap_2d::StaticLayer, assumes the maps origin (x_, y_) and origin + extension (x_ + with_, y_ + width_) in the StaticLayers-frame and uses these expressions as two opposing corners of the axis-aligned bounding-box. (Partial updates use the variables x_, y_, width_, height_ as well, so the Problem is the same and the solution as well.)

The image below illustrates the error:

• blue solid: map as served by map-server, origin at (0,0) in frame /map
• blue dotted: range to be updated, as computed by updateBounds(), under the assumption that the maps frame and the costmaps frame are the same
• green solid: an axis-aligned bounding-box in frame /odom including the whole updated map.

As noted at the top, it is important, that no other layer requests another portion of the map to be updated! The requested bounds from all layers will be evaluated and the overall minimum and maximum values for both axes will be selected. All layers are updated in this global window! If there is any other layer requesting to update the whole costmap, all layers will be updated completely and the problem is not observable!

solution

Solution to the problem is: calculating the axis-aligned bounding-box of the map in the costmaps frame, as shown in green above. This is achieved by transforming all corners of the map into the costmaps frame and selecting the overall minimum and maximum per axis.

A pull request, giving the expected behavior as in the image below, has been made. See #900