Costmap2D index checking

[athackst]

Added some index checking to prevent segfaults

---

This change is 

[athackst]

1. We were seg faulting before we added the guards. To me, not having the robot crash far outstrips any minimal computational cost associated with this. As a base library that many people use, this should really be the priority. If there is a more efficient way of doing the check, we can modify to that instead. We considered changing the type to a std::vector instead, but the delta would be very large.

2. Sure, I can add a throw for the sets.

[mikeferguson]

I'm also thinking getCost and setCost should not include checks -- and am also concerned about runtime implications -- here's why:

• If you look at the existing codebase, pretty much everywhere we call setCost() or getCost() we call worldToMap() first -- so we'd be double checking everything in "normal" operation.

• You should probably be using one of the worldToMap functions that includes a check to get your indices before calling getCost/setCost (I am presuming here that you are using costmap_2d in a custom setting, if that's not the case, then we should track down where the proper worldToMap call is missing in this codebase).

• There are three different worldToMap functions -- one that just does the calculation, one that does it with bounds checking, and one that does it with checking and constraining the returned values automatically to the range of the cost map. This allows users to decide what level of checking they want to incur cost for.

I would suggest that the docs for getCost() and setCost() should be updated to note this design aspect. And yes, it is a somewhat poorly designed API (in that it is very easy to misuse, as you've found) -- however as you said, fixing the API would be a pretty big delta this late in the game. I'd definitely suggest that we should impress such an update on the navigation2 work (which doesn't end up effecting a large deployed base with such a change right now, and might still have that same API).

[athackst]

+1 to fixing this in navigation2

We are indeed using Costmap in a custom context. We're doing something similar to setConvexPolygonCost except we are doing the "get" version (with very similar logic). We call one of the mapToWorld functions to convert the footprint into costmap space and then call convexFillCells to get all the cells in order to calculate the total cost of the footprint. It's likely the segfault is occuring during some odd corner case where convexFillCells is returning a cell that's out of the costmap indices. We could put the check outside the getCost function, but it seems to me that a public api should guard itself against segfaults and there shouldn't be a need to translate between world and map frames in order to do that. We could, of course, do the check outside of the getCost function. I would however recommend that index checks inside of setConvexPolygonCost remain. If there is interest, I could PR in our getConvexPolygonCost function as well, modifying to do the check internally instead of in getCost.