Fix bug where global planner lethal cost is always 1 unit smaller than expected

[siferati]

Description

This is a fix to an issue I reported on upstream over 2 years ago:

• [Bug] [Global Planner] Lethal cost is always 1 unit smaller than user setting ros-planning/navigation#1093

I found this issue again while working on the route deviation layer.
We set lethal_cost_ of the global planner to be 253 (i.e. costmap_2d::INSCRIBED_INFLATED_OBSTACLE).
This is correct -- if the robot center is at a cell of cost 253, the robot is in collision.

The problem is that the global planner treats this cost as if it was the costmap_2d::LETHAL_OBSTACLE cost, and so assumes that lethal_cost_ - 1 is also in collision (i.e. costmap_2d::INSCRIBED_INFLATED_OBSTACLE).

The whole point of this param being dynamically reconfigured is that we don't have to be tied to the value of costmap_2d::LETHAL_OBSTACLE and so we also shouldn't assume that lethal_cost_ - 1 is lethal too.

So if you happen to have an aggressive inflation factor that causes most cells to have the highest possible free cost (i.e. 252), then the potential field will look like in the image, where every cell outside the route is not expanded because we set lethal_cost_ to costmap_2d::INSCRIBED_INFLATED_OBSTACLE and so costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1 is also treated as lethal.

This will result in the global planner failing to find any valid paths as soon as the robot center touches the gray area.