nav2_smac_planner: Fix incorrect conversion between costmap and worlrd coordinates

[FrGrQuim]

The Smac planner computes paths using costmap coordinates (cell indexes).
Therefore, conversions between world coordinates and costmap coordinates are required.
The issue was that the world-to-costmap conversion used a floor approximation, while the costmap-to-world conversion behaved as if a ceil approximation had been used.
As a result, the planner's output was often shifted in the negative direction.
This commit corrects the inconsistency to ensure proper alignment between costmap and world coordinates.

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4735
Primary OS tested on	Ubuntu
Robotic platform tested on	/
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• I fix the bad conversion cotsmap_cell_index->Worlds in SmacPlanner

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

[SteveMacenski]

This was changed in July to supposedly fix the issue in reverse: 137b93b (and that test that was added is now failing in this PR 😉 )

What distribution did you test this against? Main / Rolling / Jazzy is -, but humble / iron is + due to backport conflicts, so it seems like humble/iron are outliers and inconsistent with the current state of the main branches.

I'm therefore wondering if this change is actually correct. I think perhaps you need to invert that in Humble / Iron that you're using and the main change is incorrect. Or, do you think the previous PR to change it that's in Main/Rolling/Jazzy is wrong? Its worth pausing and having that conversation since your issue reports using iron but this PR is against main. I'm wondering if you inverted them in iron to fix it, but then submitted the inversion in main assuming that they were the same (and they're not; apparently).

Please test and let me know :-) If the right answer is to fix Iron / Humble, submitting PRs with the compliment of this change there should be easy for me to merge. I don't typically accept new PRs for EOL distributions (Iron), but I will in this case since this is presumably a mathematical error

[DylanDeCoeyer-Quimesis]

@SteveMacenski

I notice that the Iron version of the test function does not contain any assert, while the main/jazzy version does. So, it probably explains why the Iron/Humble versions pass the tests.

Regarding the change suggested by @FrGrQuim, I believe that it would make sense (whatever the version) only if mx and my are integers. In this case, the first cell (0, 0) should have the coordinates (0.5, 0.5) * costmap_resolution (i.e. the center of the cell), and not (-0.5, -0.5) * costmap_resolution as it is the case currently.

However, mx and my are float (continuous map coords) in the getWorldCoords and the reciprocal Costmap2D::worldToMapContinuous functions. So, this -0.5/+0.5 added in those two functions does not make sense to me. For example, if mx = 0.1 (i.e. 10% of the first cell), origin is (0.0, 0.0), and resolution is 1, I would expect the world coordinate to be x = origin_x + mx * resolution = 0.1. No 0.5 should be involved in this calculation, since there is no conversion from "discrete world" to "continuous world".

Conclusion
The 0.5 offset should be removed from getWorldCoords and Costmap2D::worldToMapContinuous.

[SteveMacenski]

After analyzing the implementatinos of worldToMap, I'm in agreement that worldToMapContinuous should not have the + 0.5f. That was introduced in #4255. @BriceRenaudeau can you review that again and let us know if you agree that this was added in error? I don't see that any other worldToMap implementation uses the 0.5 mechanic other than that one added, so that seems suspect.

---

On mapToWorld: the worldToMap in general will floor any value when converting to an int or unsigned int, so adding in the +0.5 cells helps deal with that w.r.t. getting the world dimension later. If we have continuous coordinates that were changed in Brice's PR from April, 2024 then the conversion to world again after planning needs to also adjust in its implementation.

I think that's what @HovorunBh was attempting to fix in #4506 about 2-3 months later, but changed the sign because that visually fixed the issue. @HovorunBh can you let us know about your thinking/testing for that? Was it just a visual comparison (and would just removing the 0.5 term also resolve)?

Assuming @BriceRenaudeau and @HovorunBh agree - I think that's a sensible move to remove from both terms. This looks to possibly be an issue that is the result of continuous coordinate updates, when we previously didn't have that feature, and we missed a spot + added another spot to compensate.

[BriceRenaudeau]

@SteveMacenski, You are right, this is clearly a mistake. I might have confused with mapToWorld...

[SteveMacenski]

@FrGrQuim or @DylanDeCoeyer-Quimesis - mind opening a PR to adjust respectively?

@BriceRenaudeau thanks for the fast feedback!

[HovorunBh]

My fix in #4506 was indeed based on visual testing and observed misalignments in planned paths. Additionally, I noticed an inconsistency in how +0.5 was handled between mapToWorld and worldToMap, which led to the argument in my PR:

When we convert world to map coordinates we add 0.5
Therefore when we convert map to world I think we should subtract 0.5. Otherwise we end up in the next costmap cell

I didn't go any deeper trying to verify whether simply removing the +0.5f from worldToMapContinuous would have had the same effect. But your explanation makes sense, I think it is a good idea and it will resolve both issues!

[DylanDeCoeyer-Quimesis]

@FrGrQuim or @DylanDeCoeyer-Quimesis - mind opening a PR to adjust respectively?

@BriceRenaudeau thanks for the fast feedback!

Done! #5049

[SteveMacenski]

OK! Thanks everyone for your quick feedback and confirmation! Closing this one as #5049 supersedes