Fixed bug causing global planner to plan to wrong cell

[siferati]

Description

Fixed a bug that was causing the global planner to compute paths to the wrong cell. This allowed the global planner to return paths that were in collision. You can see in the picture below how the global plan clearly ends inside the inscribed region of the costmap (i.e. collision!)

When the param old_navfn_behavior was set to false, the worldToMap function was subtracting 0.5 from the resulting cell coordinates, which is wrong. Although it makes sense to add 0.5 when converting from map to world (in order to return the center of the cell rather than the corner), the opposite is not true!

You can also confirm how in Costmap2D::mapToWorld we add 0.5 to get the center of the cell, but we don't subtract 0.5 when doing the opposite operation in Costmap2D::worldToMap.

https://github.com/ros-planning/navigation/blob/47c9c629fc93e6c6bbcd3109eb2d5b55efce400d/costmap_2d/src/costmap_2d.cpp#L202-L206

https://github.com/ros-planning/navigation/blob/47c9c629fc93e6c6bbcd3109eb2d5b55efce400d/costmap_2d/src/costmap_2d.cpp#L208-L220