Use costmap's worldToMapEnforceBounds instead of worldToMap when checking pose

[corot]

Now getFootprintCells stops adding cells when part of the footprint is outside the map, so the subsequent cost calculation will be wrong. This makes poses like this valid:

getFootprintCells returns only 19 cells that are all clean. With this PR, getFootprintCells will crop the footprint to match map borders and return all the footprint cells within the map.

BONUS Publish padded footprint for visualization

[corot]

The format check fails... fails but cannot be help unless I reformat completely the footprint helper class

[dorezyuk]

Not sure if this the clamping to world bounds is the right idea.

A footprint entirely outside of the map (in our image displaced to the "south") would then "degenerate" to a line on the map border. Or in more general way: The clamping results in a projection (of the offending polygon parts) onto the map border.. Checking these cells for obstacles would not tell much.

In fact, I've done the same "mistake" already and it did not work well (for us at least). In my case I've resorted to a geometry lib (like boost::geometry, geos, etc) to get the overlap between the polygon and the map. Maybe we could try the same here?

[corot]

In fact, I've done the same "mistake" already and it did not work well (for us at least). In my case I've resorted to a graphic processing lib (like boost::geometry, goes, etc) to get the overlap between the polygon and the map. Maybe we could try the same here?

Yes please, could you contribute it?

Or a simpler solution could be to call worldToMap for both points of each segment but only worldToMapEnforceBounds if 1 of the points is out of the map. If both are out, we skip the segment. Sounds reasonable?

[dorezyuk]

Let me cook up something next week - should be relatively easy :)

[dorezyuk]

I've added a branch with the cropping (https://github.com/magazino/move_base_flex/tree/dima/footprint). However, this got me thinking if we really like to return "free/occupied" for footprints partly outside of the map. Honestly, I would rather return an empty cell's vector so we can inform the user that the footprint is outside. @corot @spuetz What do you think?

[corot]

I've added a branch with the cropping (https://github.com/magazino/move_base_flex/tree/dima/footprint). However, this got me thinking if we really like to return "free/occupied" for footprints partly outside of the map. Honestly, I would rather return an empty cell's vector so we can inform the user that the footprint is outside. @corot @spuetz What do you think?

+1 for OUTSIDE when partially outside (and change the comment, as now says "robot is completely outside the map"). The status are sorted from less to more ominous, and being out of the map, even partially, sounds bad enough

[corot]

Closed in favor of #272