fix bounds calculation of StaticLayer-plugin for maps served in different frame than the costmap

[bitmeal]

This pull request addresses issue #899 .

The costmap_2d::StaticLayer-plugin does not respect the maps coordinate frame, and assumes it to be the same as the costmaps global_frame, when calculating the portion of the map to update.

This pull request fixes the issue and transforms the maps bounds in the frame of the costmap and calculates the corresponding axis-aligned bounding-box to server as the portion/bounds to be updated.

For further details, see #899

[bitmeal]

The test(s) (which I did not run before - sorry!) fail, because the submitted fix expects the global_frame of the costmap and the frame given as frame_id from the map_server to exist! See following line from the tests (link) 23:56:38 [31m[ERROR] [1562622998.312606795]: "frame" passed to lookupTransform argument target_frame does not exist. [0m Furthermore, a transformation between these frames should exist (map_frame_ and global_frame_ in costmap_2d::StaticLayer).

The costmaps global_frame is set to "frame" in each rostest-test_obstacle_tests costmap constructors, whereas the map_server is serving a map with frame_id set to "/map". The tests pass, when the map_servers frame_id is set to be "frame" (equal to the given costmaps frame) - it is failing from a missing transformation/frame.

It is possible to "fix the tests" by providing the frame_id parameter as _frame_id:=frame to the map_server. The problem ist, that the tests should describe and test for expected behavior. Thus - per the defined tests - the expected behavior would be, that the costmap does not interpret the map_servers frame_id and does not transform the map data to the costmaps global_frame_, but simply copies it. There is another problem arising now: handling or not-handling of the map_servers frame_id, the globla_frame_ and their relations/transformations, is inconsistent in costmap_d2::StaticLayer, as it differs from using rolling_window: true to rolling_window: false; when rolling_window: true the map data is transformed to the costmaps global_frame_. From the current implementation, no expected behavior can be deduced!

I will file another issue regarding the above-mentioned problem and hope for a discussion, on what the expected behavior should be. This should allow to define the expected behavior in the tests and provide a more robust implementation of costmap_2d::StaticLayer.

Linked Issue: #903

[bitmeal]

Any chance to get (at least) some discussion on this?

[bitmeal]

Bump again, as this is not trivial in my opinion. Could we get some discussion on this and the linked Issues #899 and #903 ? For visibility: @mikeferguson and @DLu maybe?