nav2_costmap_2d: static_layer: add grid value interpretation for inscribed inflated obstacles

[DylanDeCoeyer-Quimesis]

Until now, the special OccupancyGrid value "99" attributed to the inscribed inflated obstacles was mapped to 251 on conversion to Costmap2D, using the default linear relation. This is incorrect since a special value exists in the Costmap2D for inscribed obstacles: 253.

This commit makes sure that the correct value is used in case of inflated obstacle.

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	/
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom simulation
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

@DylanDeCoeyer-Quimesis I need much more detailed explanation of this, possibly with a diagram to meaningfully review it. I believe you that something may benefit from this, but I need to understand in detail like you do with references 😉

This touches a very old and very established piece of software, so some explanation for myself and future readers is important

[DylanDeCoeyer-Quimesis]

@SteveMacenski sure! The class Costmap2DPublisher is using a cost translation table when converting Costmap2D to OccupancyGrid. This conversion table associates the value nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE (253) to 99.

To recover the original Costmap2D from the OccupancyGrid on the subscription side, the subscribers must use the same (reciprocal) conversion table. However, in the case of the StaticLayer subscription, the special value attributed to the inscribed inflated obstacle was ignored. The usual scaling factor was applied:

double scale = static_cast<double>(value) / lethal_threshold_; // for inscr. inflated obst.: 99 / 100 = 0.99
return scale * LETHAL_OBSTACLE; // for inscr. inflated obst.: 0.99 * 254 = 251.46 => 251

So, this costmap layer initially supposed to copy the incoming costmap was transforming every inscribed inflated obstacle cells (253) into non lethal obstacle cells with cost of 251.

If this is the expected behavior for any historical reason, then I'll have at least to make the StaticLayer::interpretValue method virtual to let inheriting classes customize the behavior of this method and add INSCRIBED_INFLATED_OBSTACLE.

[SteveMacenski]

Thanks! LGTM. CI had an unexpected test fail, so running again to make sure it wasn't just a rare one off.

[SteveMacenski]

@DylanDeCoeyer-Quimesis please rebase or pull in main to acquire #5569 to fix CI

[DylanDeCoeyer-Quimesis]

@SteveMacenski there is still an issue with the unit tests, but I don't understand it. It says the number of INSCRIBED_INFLATED_OBSTACLE cells is 28 while 78 was expected by the ASSERT_EQ. But when I modify the ASSERT_EQ to expect 28, it says that the number of cells is 78...

    [costmap_tests-4] /opt/overlay_ws/src/navigation2/nav2_costmap_2d/test/integration/inflation_tests.cpp:465: Failure
    [costmap_tests-4] Expected equality of these values:
    [costmap_tests-4]   countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
    [costmap_tests-4]     Which is: 28
    [costmap_tests-4]   78u
    [costmap_tests-4]     Which is: 78

[SteveMacenski]

I'm not sure - I'd look at printing the map (uncommenting printMap line) and seeing what it looks like to debug further. I see inline in the code it has 28u - not that I think it'll solve it, but I triggered CI again to see what it does