[jmguerreroh]

Split out of #177, as suggested.

What

Changes to easynav_navmap_planner, motivated by planning on level-of-detail NavMaps (fine near the robot, coarse far away) and on buildings with several floors:

• Weighted A* (heuristic_weight, default 1.0, which keeps A* admissible and the previous behaviour). A larger weight expands fewer NavCels at the cost of paths up to that factor longer; each NavCel is expanded once.
• Replan hysteresis (replan_hysteresis, replan_hysteresis_horizon, both 0.0 by default = off). Keeps the path being followed while it stays traversable, unless a new one is shorter by a given fraction. This avoids the oscillation of replanning on a map that changes with the robot's position. The horizon limits the check to the part of the path near the robot, because the far part of a LOD map is coarse and changes with every re-mesh.
• Cheaper plans: epoch-stamped search buffers instead of clearing them on every plan.
• Multi-floor fixes: surface-continuity checks so the path does not jump between floors that lie over each other.

All new parameters are documented in the README. With the defaults, behaviour is unchanged.

Tests

New tests in navmap_planner_tests.cpp and navmap_planner_terrain_tests.cpp. One existing test was rewritten to use a float inflated_obstacles layer.

Dependencies

This package reads cost layers whose type may not be uint8. The planner tests need the NavMap::operator= fix from EasyNavigation/NavMap#42; without it a test can crash on a retyped layer.

[fmrico]

Thanks for splitting this out of #177. The direction looks good, and with the defaults the behaviour doesn't change. I especially like the epoch-stamped buffers and reading the uint8 layer without copying it; those help even without LOD maps. Weighted A* without re-opening is also the right choice for large maps.

I found a couple of issues before we can merge:

Bugs

1. Stale closed_stamp_ after a map size change. When N changes, ensure_graph_cache reallocates g_, parent_ and g_stamp_ and resets epoch_ to 0, but it leaves closed_stamp_ alone. The weighted search only reassigns it when closed_stamp_.size() != N. So this sequence breaks: a weighted plan on a map of size N1, a re-mesh to N2, then back to N1. epoch_ starts again at 1 while closed_stamp_ still holds the old stamps, and NavCels show up as closed without being expanded. LOD maps change size all the time, so this will happen. Fix: closed_stamp_.clear() in the same branch that reallocates g_.

2. The hysteresis check ignores obstacles on non-uint8 cost layers. choose_path uses dynamic_pointer_cast<LayerView<std::uint8_t>>. When the cast returns null, every cell counts as FREE_SPACE. With a float inflated_obstacles layer (the case the rewritten test uses), the followed path is kept even when a new obstacle blocks it. It should read costs the same way a_star_path does (layer_get<std::uint8_t>).

Minor

• Please replace the magic numbers with named constants, or parameters where it makes sense: best < 2.0, the 1 m near band, and the 0.5f height tolerance. The 0.5f appears in three places (path_smoother, locate_near, end of the shortcut check), so a single shared constant would help.
• When fresh is empty and the old path is still valid up to the horizon, the old path is kept, even though A* just found no path. Please add at least a comment or a debug log for this case.
• The kept path starts at followed_[from], not at the robot. Can you confirm the controller handles that first segment well?
• Some of the comments are long paragraphs (choose_path, a_star_path). Could you shorten them to one or two lines each?

Before merging

• The description still contains the NavMap#<NUMBER> placeholder. Please link the actual PR. That fix needs to be merged first, or the CI here may fail.
• Please add tests for: (a) weighted A* while the map alternates between sizes, which covers bug 1; (b) hysteresis with a float cost layer and an obstacle on the followed path, which covers bug 2.

[codecov]

Codecov Report

❌ Patch coverage is 96.25668% with 14 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...lanner/src/easynav_navmap_planner/AStarPlanner.cpp	91.03%	6 Missing and 7 partials ⚠️
...map_planner/tests/navmap_planner_terrain_tests.cpp	97.61%	0 Missing and 1 partial ⚠️

📢 Thoughts on this report? Let us know!

[jmguerreroh]

Thanks for the careful review. Pushed a commit that addresses everything:

Bugs

• Stale closed_stamp_: it is now cleared in the same branch of ensure_graph_cache that reallocates g_, so the old stamps cannot survive the epoch_ restart.
• Hysteresis with non-uint8 cost layers: choose_path now reads the cost like a_star_path does (the uint8 view when there is one, layer_get<std::uint8_t> otherwise), so an obstacle in a float inflated_obstacles layer blocks the followed path.

Tests (both fail without the fix and pass with it)

• WeightedAStarStillPlansWhenTheMapChangesSizeAndComesBack: weighted plan on a map of size N1, a plan on a map of size N2 that returns before searching (goal on an obstacle), then N1 again.
• WithHysteresisAnObstacleOfAFloatCostLayerOnTheFollowedPathDropsIt.

Minor

• Magic numbers: kMaxPathOffset (2.0 m) and kSurfaceHeightTolerance (0.5f, now shared by the three places that used it). The 1 m near band reuses kEscapeDistance, which already had the same meaning in a_star_path and is now file-level.
• When A* finds no path but the followed one is still valid up to the horizon, it is kept; I added a comment and a debug log for that case.
• The kept path starts at followed_[from], ahead of the robot, and the segment robot -> followed_[from] is checked (the sampling starts at the robot). A fresh path does not start exactly at the robot either, but at the centroid of its NavCel. From reading RegulatedPurePursuitController, it takes the pose nearest the robot and looks forward to the first pose at lookahead distance, so a first pose ahead of the robot is fine. I have not run it against the controller.
• Shortened the long comments in choose_path and a_star_path.

The package passes all its tests with these changes, lint included.

Regarding the dependency on NavMap: EasyNavigation/NavMap#42, which needs to be merged first.

[fmrico]

LGTM!!! Merging 🚀