ROS2 Nav2 Costmap2D not returning LETHAL_OBSTACLE when occupied cells are completely inside footprint

edit

The idea is that if you're checking collision regularly (as you would if you're concerned about running into stuff, e.g. everyone) then there would never be a point where there are costs within the inside of the footprint there those costs did not also cross the footprint's boundary line. Even if you're processing new sensor data so things can pop up, they can't pop up in the footprint if your robot is occupying that space.

Additionally, the costmaps commonly have a "clear under footprint" parameter enabled. This will do what it sounds like: clear all costs under the robot footprint since we know its freespace, since the robot is occupying it. So even if we did check the volume, in most places and applications, the volume would be free!

Checking the bounding line is much cheaper computationally than checking the volume of the robot, so this is a performance optimization. But, if its truly necessary and you're willing to take the computational hit, it would be a reasonable contribution to add a function that seeds the center of the robot and Breadth First Search's the interior volume of the footprint and have that be an option for applications using the utility.