From 9237ff502d61f369df2ff3766c9206abecd9da26 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Thu, 5 Nov 2026 10:24:56 +0100 Subject: [PATCH 1/2] navmap_planner: weighted A*, replan hysteresis, cheaper plans, multi-floor fixes --- planners/easynav_navmap_planner/README.md | 3 + .../easynav_navmap_planner/AStarPlanner.hpp | 49 ++- .../easynav_navmap_planner/AStarPlanner.cpp | 254 ++++++++++++--- .../tests/navmap_planner_terrain_tests.cpp | 80 ++++- .../tests/navmap_planner_tests.cpp | 292 ++++++++++++++++++ 5 files changed, 626 insertions(+), 52 deletions(-) diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 164d67b5..d11f5c55 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -55,6 +55,9 @@ All parameters are declared under the plugin namespace, i.e. | Name | Type | Default | Description | |---|---|---:|---| | `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | +| `.heuristic_weight` | `double` | `1.0` | Weighted A*: the heuristic is multiplied by this weight (values below 1 are taken as 1). `1` keeps A* admissible (shortest paths); a larger weight expands fewer NavCels and may return paths up to that factor longer (weighted A* without re-opening: each NavCel is expanded once). | +| `.replan_hysteresis` | `double` | `0.0` | If above 0, the path being followed is kept while it stays traversable, unless a new one is shorter by this fraction (`0.2`: 20 %); what is left of it (from the robot, in 3D) is published. Against the oscillation of replanning on a map that changes with the robot's position (a level-of-detail map). `0`: always the new path. | +| `.replan_hysteresis_horizon` | `double` | `0.0` | With `replan_hysteresis`: only this much of the followed path (m, along it from the robot) must still be traversable to keep it; the far part of a level-of-detail map is coarse and changes with every re-mesh, so it is checked when the robot gets near. `0`: the whole path. | | `.cost_weight` | `double` | `5.0` | Weight of the cell cost (inflation) against the path length; higher values keep paths farther from obstacles. | | `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | | `.terrain.slope_weight` | `double` | `0.0` | Extra cost per meter on a sloped NavCel: `slope_weight * tan²(slope)`, up, down or across. Gentle slopes cost little and steep ones a lot: with `20`, 5° adds 0.15 per meter, 10° 0.6, 20° 2.6 and 30° 6.7 (the flat costs `cost_factor`, 2). `0` ignores the slope. | diff --git a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index f6c54040..10db46b7 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -79,6 +79,7 @@ class AStarPlanner : public PlannerMethodBase protected: double cost_factor_ {2.0}; ///< Scaling factor applied to cell cost values. + double heuristic_weight_ {1.0}; ///< Weighted A*: the heuristic times this (1 = admissible). double cost_weight_ {5.0}; ///< Weight of the cell cost (inflation) against length. double cost_axial_ {1.0}; ///< Cost multiplier for axial (horizontal/vertical) moves. double cost_diagonal_ {1.0}; ///< Cost multiplier for diagonal moves. @@ -132,12 +133,56 @@ class AStarPlanner : public PlannerMethodBase /// Distance between the centroids of neighbor NavCels (mean): the path's spacing. double cel_spacing_ {0.1}; - /// Cached per-NavCel occupancy / cost values (0..255). + /// Per-NavCel cost values (0..255) of a cost layer that is not uint8 of the map's size (else + /// the layer itself is read, through occ_data_: no copy of the whole map per plan). std::vector occ_; + /// The cost layer's own data during a_star_path(), when it is uint8 of the map's size. + const std::uint8_t * occ_data_ {nullptr}; + /// Cost of NavCel @p c in the plan being made. + std::uint8_t occ(::navmap::NavCelId c) const {return occ_data_ ? occ_data_[c] : occ_[c];} - /// Reusable buffers for A* search cost and parent links. + /// Reusable buffers for A* search cost and parent links. An entry is valid in the current + /// search only if its stamp is epoch_: nothing of size N is reset per plan, so a plan costs + /// what it explores, not the size of the map. std::vector g_; std::vector<::navmap::NavCelId> parent_; + std::vector g_stamp_; + /// Weighted A* only (heuristic_weight > 1): a NavCel is closed in this search if its stamp is + /// epoch_. Sized on the first weighted search, so the admissible A* uses no memory for it. + std::vector closed_stamp_; + std::uint32_t epoch_ {0}; + + /// > 0: the path being followed is kept while it stays traversable, unless a new one is + /// shorter by this fraction (0.2: 20 %). Against the oscillation of replanning on a map that + /// changes with the robot's position (a level-of-detail map). 0 (default): always the new one. + double replan_hysteresis_ {0.0}; + /// With replan_hysteresis: only this much of the followed path (m, along it from the robot) must + /// still be traversable to keep it. On a level-of-detail map the far part is coarse and changes + /// with every re-mesh (its poses may even fall outside the new coarse NavCels): it is checked + /// when the robot gets near. 0 (default): the whole path. + double replan_hysteresis_horizon_ {0.0}; + /// What the last a_star_path() did (for measurements): NavCels taken from the open list + /// (expanded, re-expansions of a NavCel reached again by a cheaper way included) and entries + /// pushed to it. + struct SearchStats + { + std::size_t expanded{0}; + std::size_t pushed{0}; + }; + SearchStats last_search_stats_; + + /// The path being followed (for replan_hysteresis) and its goal. + std::vector followed_; + geometry_msgs::msg::Pose followed_goal_; + + /** + * @brief With replan_hysteresis: the path to follow, the new one or what is left of the + * current one (still traversable on @p map and not much longer). Remembers it. + * Without: @p fresh. + */ + std::vector choose_path( + const ::navmap::NavMap & map, const geometry_msgs::msg::Pose & robot, + const geometry_msgs::msg::Pose & goal, std::vector fresh); /// Which NavCels are within the terrain limits (terrain_ok_), for a new geometry. void build_terrain(const ::navmap::NavMap & map); diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 67f7a83b..4ac93e61 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -70,10 +70,25 @@ void AStarPlanner::on_initialize() easynav::declare_parameter_if_absent(*node, plugin_name + ".cost_factor", 2.0); easynav::declare_parameter_if_absent(*node, plugin_name + ".cost_weight", 5.0); easynav::declare_parameter_if_absent(*node, plugin_name + ".continuous_replan", true); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".replan_hysteresis", replan_hysteresis_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".replan_hysteresis_horizon", replan_hysteresis_horizon_); node->get_parameter(plugin_name + ".cost_factor", cost_factor_); + // Weighted A*: the heuristic is multiplied by this weight (>= 1). 1 keeps A* admissible (optimal + // paths); a larger weight expands fewer NavCels and may return paths up to that factor longer. + easynav::declare_parameter_if_absent(*node, plugin_name + ".heuristic_weight", 1.0); + node->get_parameter(plugin_name + ".heuristic_weight", heuristic_weight_); + if (!(heuristic_weight_ >= 1.0)) {heuristic_weight_ = 1.0;} node->get_parameter(plugin_name + ".cost_weight", cost_weight_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); + node->get_parameter(plugin_name + ".replan_hysteresis", replan_hysteresis_); + if (!(replan_hysteresis_ >= 0.0)) {replan_hysteresis_ = 0.0;} + node->get_parameter( + plugin_name + ".replan_hysteresis_horizon", replan_hysteresis_horizon_); + if (!(replan_hysteresis_horizon_ >= 0.0)) {replan_hysteresis_horizon_ = 0.0;} + followed_.clear(); // Terrain (off by default) const std::string t = plugin_name + ".terrain."; @@ -156,7 +171,8 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = goal; - auto poses = a_star_path(navmap, robot_pose.pose.pose, goal); + auto poses = choose_path( + navmap, robot_pose.pose.pose, goal, a_star_path(navmap, robot_pose.pose.pose, goal)); if (!poses.empty()) { current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; @@ -182,6 +198,100 @@ void AStarPlanner::update(NavState & nav_state) } } +std::vector AStarPlanner::choose_path( + const ::navmap::NavMap & map, const geometry_msgs::msg::Pose & robot, + const geometry_msgs::msg::Pose & goal, std::vector fresh) +{ + auto remember = [this, &goal](std::vector p) { + followed_ = p; + followed_goal_ = goal; + return p; + }; + if (replan_hysteresis_ <= 0.0 || followed_.size() < 2 || !(goal == followed_goal_)) { + return remember(std::move(fresh)); + } + auto length = [](const std::vector & p, std::size_t from) { + double l = 0.0; + for (std::size_t k = from + 1; k < p.size(); ++k) { + l += std::hypot( + p[k].position.x - p[k - 1].position.x, p[k].position.y - p[k - 1].position.y); + } + return l; + }; + // What is left of the followed path: the poses after its segment nearest the robot (the pose + // nearest the robot may be behind it: the poses of a shortcut path are metres apart, and + // going back to it, then forward, then back again is an oscillation). Nearest in 3D: with + // floors over each other the path may pass right under the robot (its end on the floor below, + // the robot on its way down above it); nearest in the plane, that part would be "what is left" + // and the robot would stay over the goal + std::size_t from = 1; + double best = std::numeric_limits::infinity(); + for (std::size_t k = 0; k + 1 < followed_.size(); ++k) { + const auto & a = followed_[k].position; + const auto & b = followed_[k + 1].position; + const double ux = b.x - a.x, uy = b.y - a.y, len2 = ux * ux + uy * uy; + const double t = len2 > 0.0 ? std::clamp( + ((robot.position.x - a.x) * ux + (robot.position.y - a.y) * uy) / len2, 0.0, 1.0) : 0.0; + const double d = std::hypot( + a.x + t * ux - robot.position.x, a.y + t * uy - robot.position.y, + a.z + t * (b.z - a.z) - robot.position.z); + if (d < best) {best = d; from = k + 1;} + } + // Still traversable on this map (it may have changed under it), up to the horizon: the path + // from the robot, sampled along its segments (its poses are metres apart: a segment can cut a + // corner its ends do not), on NavCels below the inscribed cost and within the terrain limits. + // In the first metre the inscribed band is allowed (the robot may be in it, as a plan from there + // allows), an obstacle or a hole is not. + const auto cost = std::dynamic_pointer_cast<::navmap::LayerView>( + map.layers.get(map.has_layer("inflated_obstacles") ? "inflated_obstacles" : "obstacles")); + bool valid = best < 2.0; + ::navmap::NavMap::LocateOpts opts; + const double step = std::clamp(cel_spacing_, 0.05, 0.25); + double along = 0.0; + Eigen::Vector3d prev(robot.position.x, robot.position.y, robot.position.z); + for (std::size_t k = from; k < followed_.size() && valid; ++k) { + const Eigen::Vector3d next( + followed_[k].position.x, followed_[k].position.y, followed_[k].position.z); + const double len = (next - prev).head<2>().norm(); + const int n = std::max(1, static_cast(std::ceil(len / step))); + for (int i = 1; i <= n && valid; ++i) { + const Eigen::Vector3d q = prev + (next - prev) * (static_cast(i) / n); + const double at = along + len * i / n; + if (replan_hysteresis_horizon_ > 0.0 && at > replan_hysteresis_horizon_) { + k = followed_.size(); // the rest is checked when the robot gets near + break; + } + std::size_t sidx = 0; + ::navmap::NavCelId c; + Eigen::Vector3f bary, hit; + if (!map.locate_navcel(q.cast(), sidx, c, bary, &hit, opts)) { + valid = false; + break; + } + opts.hint_cid = c; + const std::uint8_t v = cost && c < cost->size() ? (*cost)[c] : navmap_ros::FREE_SPACE; + const bool near = (q - Eigen::Vector3d(robot.position.x, robot.position.y, + robot.position.z)).head<2>().norm() < 1.0; + valid = (near ? v < navmap_ros::LETHAL_OBSTACLE : v < + navmap_ros::INSCRIBED_INFLATED_OBSTACLE) && + (terrain_ok_.size() != map.navcels.size() || terrain_ok_[c]); + } + along += len; + prev = next; + } + if (!valid) {return remember(std::move(fresh));} + // the new path only if it is clearly shorter (or there is no other) + const double left = std::hypot( + followed_[from].position.x - robot.position.x, + followed_[from].position.y - robot.position.y) + length(followed_, from); + if (!fresh.empty() && length(fresh, 0) < (1.0 - replan_hysteresis_) * left) { + return remember(std::move(fresh)); + } + std::vector kept(followed_.begin() + static_cast(from), + followed_.end()); + return remember(std::move(kept)); +} + void AStarPlanner::clear_path(NavState & nav_state) { current_path_.poses.clear(); @@ -231,8 +341,17 @@ AStarPlanner::path_smoother( Eigen::Vector3f bary; Eigen::Vector3f hit; - // Try full locate with hint (from previous point) + // Try full locate with hint (from previous point). NavMap's search returns the surface nearest + // in height however far: a point by the edge of a floor, over the gap beside a ramp, would be + // found on the floor below and moved there. That one is no good: the closest NavCel instead bool ok = navmap.locate_navcel(pts[i], sidx, cid, bary, &hit, opts); + if (ok) { + const ::navmap::NavCel & tri = navmap.navcels[cid]; + const Eigen::Vector3f on = ::navmap::closest_point_on_triangle( + pts[i], navmap.positions.at(tri.v[0]), navmap.positions.at(tri.v[1]), + navmap.positions.at(tri.v[2])); + ok = std::abs(on.z() - pts[i].z()) < 0.5f; + } if (!ok) { // Fallback to closest triangle (keeps the query on-surface) float sqd = 0.0f; @@ -431,24 +550,17 @@ void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) cel_spacing_ = spacing_count > 0 ? spacing_sum / static_cast(spacing_count) : 0.1; } - if (occ_.size() != N) { - occ_.resize(N); - } - - const double inf = std::numeric_limits::infinity(); - + // Sized for this map once; entries are made valid per search by their stamp (see epoch_) if (g_.size() != N) { - g_.assign(N, inf); - } else { - std::fill(g_.begin(), g_.end(), inf); - } - - if (parent_.size() != N) { + g_.assign(N, std::numeric_limits::infinity()); parent_.assign(N, std::numeric_limits<::navmap::NavCelId>::max()); - } else { - std::fill( - parent_.begin(), parent_.end(), - std::numeric_limits<::navmap::NavCelId>::max()); + g_stamp_.assign(N, 0); + epoch_ = 0; + } + if (++epoch_ == 0) { // wrapped: clear the stamps once + std::fill(g_stamp_.begin(), g_stamp_.end(), 0); + std::fill(closed_stamp_.begin(), closed_stamp_.end(), 0); + epoch_ = 1; } } @@ -495,9 +607,13 @@ std::vector AStarPlanner::shortcut_path( nm.positions.at(t.v[0]), nm.positions.at(t.v[1]), nm.positions.at(t.v[2])}; }; - // NavCel under p, searched from `from` and its neighbors first: consecutive samples of a - // segment are in the same NavCel or a neighbor one. NavMap's search only as a fallback. - auto locate_near = [&](const Eigen::Vector3f & p, NavCelId from, NavCelId & out) -> bool { + // NavCel under (x, y) of p on the surface `from` is on, searched from `from` and its neighbors + // first: consecutive samples of a segment are in the same NavCel or a neighbor one. NavMap's + // search only as a fallback. The height is that of `from`'s plane there, not the segment's + // (a straight segment over a hill is in the air or under the ground) + auto locate_near = [&](const Eigen::Vector3f & p_in, NavCelId from, NavCelId & out) -> bool { + const auto tf = vertices(from); + const Eigen::Vector3f p(p_in.x(), p_in.y(), height_at(p_in, tf[0], tf[1], tf[2])); auto inside = [&](NavCelId c) { const auto t = vertices(c); return in_triangle_xy(p, t[0], t[1], t[2]) && @@ -511,7 +627,12 @@ std::vector AStarPlanner::shortcut_path( opts.hint_cid = from; std::size_t sidx = 0; Eigen::Vector3f bary, hit; - return nm.locate_navcel(p, sidx, out, bary, &hit, opts); + // NavMap's search returns the surface nearest in height, however far: with floors over each + // other, a segment that crosses (in the plane) the gap beside a ramp would be found "on" the + // floor below or above and taken, its samples moved to that floor. Only a surface that + // continues the one of `from` + if (!nm.locate_navcel(p, sidx, out, bary, &hit, opts)) {return false;} + return inside(out); }; // A step through a link (not neighbors in the mesh) is kept as is: no shortcut across it @@ -530,7 +651,7 @@ std::vector AStarPlanner::shortcut_path( } std::uint8_t allowed = 0; for (std::size_t k = i + 1; k < j; ++k) { - allowed = std::max(allowed, occ_[cels[k]]); + allowed = std::max(allowed, occ(cels[k])); } float allowed_slope = 0.0f; for (std::size_t k = i; k <= j; ++k) { @@ -546,13 +667,17 @@ std::vector AStarPlanner::shortcut_path( NavCelId c; if (!locate_near(p, cur, c)) {return false;} if (slope_weight_ > 0.0 && slope_cost_[c] > allowed_slope) {return false;} - const std::uint8_t v = occ_[c]; + const std::uint8_t v = occ(c); if (v >= navmap_ros::INSCRIBED_INFLATED_OBSTACLE || v > allowed || !terrain_ok_[c]) { return false; } cur = c; } - return true; + // and ends on the surface it was followed on: not right under (or over) b, on another floor + NavCelId c; + if (!locate_near(b, cur, c)) {return false;} + const auto t = vertices(c); + return std::abs(height_at(b, t[0], t[1], t[2]) - b.z()) < 0.5f; }; // Greedy: from each kept waypoint, the farthest visible one (exponential, then binary search) @@ -616,6 +741,7 @@ std::vector AStarPlanner::a_star_path( using navmap_ros::LETHAL_OBSTACLE; using navmap_ros::NO_INFORMATION; + last_search_stats_ = SearchStats{}; if (nm.navcels.empty()) {return {};} // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) @@ -655,16 +781,39 @@ std::vector AStarPlanner::a_star_path( return static_cast(d.norm()); }; - // Cache per-NavCel uint8_t cost values (0..255) for the selected layer. - for (NavCelId c = 0; c < static_cast(N); ++c) { - // If the cell has no stored value, assume FREE_SPACE (0). - occ_[c] = nm.layer_get(cost_layer, c, FREE_SPACE); + // Per-NavCel uint8_t cost values (0..255) of the selected layer: the layer itself when it is + // uint8 of the map's size (no copy), else converted once into occ_ (a missing value: FREE_SPACE) + struct ResetOcc + { + const std::uint8_t * & p; + ~ResetOcc() {p = nullptr;} // the layer's data is only valid during this plan + } reset_occ{occ_data_}; + { + const auto view = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get(cost_layer)); + if (view && view->size() == N) { + occ_data_ = view->data().data(); + } else { + occ_data_ = nullptr; + occ_.resize(N); + for (NavCelId c = 0; c < static_cast(N); ++c) { + occ_[c] = nm.layer_get(cost_layer, c, FREE_SPACE); + } + } } + // g of the current search (an entry of another search is +inf) + const double inf_cost = std::numeric_limits::infinity(); + auto g = [&](NavCelId c) -> double {return g_stamp_[c] == epoch_ ? g_[c] : inf_cost;}; + auto set_g = [&](NavCelId c, double v, NavCelId from) { + g_[c] = v; + parent_[c] = from; + g_stamp_[c] = epoch_; + }; // Traversability: block lethal, unknown and inscribed (the robot would touch an obstacle), // as the costmap planner does. auto traversable = [&](NavCelId c) -> bool { - const std::uint8_t v = occ_[c]; + const std::uint8_t v = occ(c); return v < INSCRIBED_INFLATED_OBSTACLE && terrain_ok_[c]; }; // How far from the start inscribed cells are still traversable (m). @@ -672,14 +821,14 @@ std::vector AStarPlanner::a_star_path( // The robot may already be within the inscribed band (e.g. stopped by a reflex): it can // leave it, unless it is on an obstacle or an unknown cell. auto can_start_at = [&](NavCelId c) -> bool { - const std::uint8_t v = occ_[c]; + const std::uint8_t v = occ(c); return (v != LETHAL_OBSTACLE) && (v != NO_INFORMATION); }; // Normalize a uint8_t cost into [0, 1]; FREE_SPACE=0 → 0.0; INSCRIBED=253 → ~1.0. // Returns +inf for non-traversable (lethal/unknown). auto normalized_cost = [&](NavCelId c) -> double { - const std::uint8_t v = occ_[c]; + const std::uint8_t v = occ(c); if (v == LETHAL_OBSTACLE || v == NO_INFORMATION) { return std::numeric_limits::infinity(); } @@ -735,18 +884,34 @@ std::vector AStarPlanner::a_star_path( std::priority_queue, Cmp> open; // Every meter costs at least cost_factor: still admissible, and far fewer expansions - const double h_scale = std::max(1.0, static_cast(cost_factor_)); + const double h_scale = + heuristic_weight_ * std::max(1.0, static_cast(cost_factor_)); auto h = [&](NavCelId a, NavCelId b) -> double { return h_scale * euclid(a, b); }; - g_[cid_start] = 0.0; + set_g(cid_start, 0.0, std::numeric_limits::max()); open.push(Node{cid_start, h(cid_start, cid_goal)}); + last_search_stats_ = SearchStats{0, 1}; + + // Weighted A* (heuristic_weight > 1): the heuristic is no longer consistent, so a NavCel can be + // reached more cheaply after it was expanded. Re-opening it can make the search explode on large + // maps; weighted A* without re-opening keeps the bound (paths at most heuristic_weight times the + // optimal cost), so each NavCel is expanded once and stale queue entries are skipped. With the + // admissible heuristic (weight 1) the search is the classic one, unchanged. + const bool weighted = heuristic_weight_ > 1.0; + if (weighted && closed_stamp_.size() != N) {closed_stamp_.assign(N, 0);} + auto closed = [&](NavCelId c) {return closed_stamp_[c] == epoch_;}; while (!open.empty()) { const auto cur = open.top(); open.pop(); const NavCelId u = cur.cid; + if (weighted) { + if (closed(u)) {continue;} + closed_stamp_[u] = epoch_; + } + ++last_search_stats_.expanded; if (u == cid_goal) {break;} @@ -769,15 +934,16 @@ std::vector AStarPlanner::a_star_path( if (!clear) {continue;} } + if (weighted && closed(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} - const double tentative = g_[u] + sc; - if (tentative < g_[v]) { - g_[v] = tentative; - parent_[v] = u; + const double tentative = g(u) + sc; + if (tentative < g(v)) { + set_g(v, tentative, u); const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); + ++last_search_stats_.pushed; } } @@ -788,22 +954,22 @@ std::vector AStarPlanner::a_star_path( const auto kind = static_cast(l.type); if (kind >= link_type_ok_.size() || !link_type_ok_[kind]) {continue;} const NavCelId v = l.from == u ? l.to : l.from; - if (v >= N || !traversable(v)) {continue;} + if (v >= N || !traversable(v) || (weighted && closed(v))) {continue;} const double ncost = normalized_cost(v); if (!std::isfinite(ncost)) {continue;} // Never shorter than the straight line, so the heuristic stays admissible const double len = std::max(static_cast(l.length), euclid(u, v)); const double cf = std::max(1.0, static_cast(cost_factor_)); - const double tentative = g_[u] + len * (cf + static_cast(cost_weight_) * ncost); - if (tentative < g_[v]) { - g_[v] = tentative; - parent_[v] = u; + const double tentative = g(u) + len * (cf + static_cast(cost_weight_) * ncost); + if (tentative < g(v)) { + set_g(v, tentative, u); open.push(Node{v, tentative + h(v, cid_goal)}); + ++last_search_stats_.pushed; } } } - if (!std::isfinite(g_[cid_goal])) { + if (!std::isfinite(g(cid_goal))) { return {}; } diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp index b037893c..44fb9f72 100644 --- a/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp +++ b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp @@ -20,9 +20,11 @@ #include #include #include +#include #include #include #include +#include #include #include "gtest/gtest.h" @@ -45,21 +47,33 @@ namespace using Height = std::function; -// nx x ny cells of 0.1 m from (x0, y0), at height h, as one surface; cells where keep(x, y) +using SharedVertices = std::map, uint32_t>; + +// nx x ny cells of 0.1 m from (x0, y0), at height h, as one surface; cells where keep(x, y). With +// `shared`, a vertex where another grid with the same `shared` has one (to the mm) is that one: +// the grids are joined there void add_grid( NavMap & nm, int nx, int ny, float x0, float y0, const Height & h, - const std::function & keep = nullptr) + const std::function & keep = nullptr, SharedVertices * shared = nullptr) { const float r = 0.1f; const std::size_t s = nm.create_surface("map"); - const auto base = static_cast(nm.positions.size()); + std::vector ids; for (int j = 0; j <= ny; ++j) { for (int i = 0; i <= nx; ++i) { - const float x = x0 + i * r, y = y0 + j * r; - nm.add_vertex({x, y, h(x, y)}); + const float x = x0 + i * r, y = y0 + j * r, z = h(x, y); + const auto key = std::make_tuple(std::llround(x * 1000), std::llround(y * 1000), + std::llround(z * 1000)); + const auto it = shared ? shared->find(key) : SharedVertices::iterator{}; + if (shared && it != shared->end()) { + ids.push_back(it->second); + continue; + } + ids.push_back(static_cast(nm.add_vertex({x, y, z}))); + if (shared) {(*shared)[key] = ids.back();} } } - auto v = [&](int i, int j) {return base + static_cast(j * (nx + 1) + i);}; + auto v = [&](int i, int j) {return ids[static_cast(j * (nx + 1) + i)];}; for (int j = 0; j < ny; ++j) { for (int i = 0; i < nx; ++i) { if (keep && !keep(x0 + (i + 0.5f) * r, y0 + (j + 0.5f) * r)) {continue;} @@ -353,6 +367,60 @@ TEST_F(TerrainPlannerTest, LinksJoinSurfacesByTheirKind) EXPECT_FALSE(plan(4.45f, 1.05f, 3.0f, 0.55f, 1.05f, 0.0f).poses.empty()); } +TEST_F(TerrainPlannerTest, AShortcutDoesNotCrossAGapOverAnotherFloor) +{ + // Ground floor 6 x 4 m; 2 m above it, a floor shaped as a U: open over x < 5, 1 < y < 3 (the + // gap beside a ramp, the well of a stair). From one arm of the U to the other the way is round + // its base (x > 5): the straight segment across the gap is over the ground floor, 2 m down, and + // not traversable (a shortcut found it "on" the ground floor and put its samples there) + NavMap nm; + add_grid(nm, 60, 40, 0.0f, 0.0f, flat); + add_grid( + nm, 60, 40, 0.0f, 0.0f, [](float, float) {return 2.0f;}, + [](float x, float y) {return !(x < 5.0f && y > 1.0f && y < 3.0f);}); + nm.rebuild_geometry_accels(); + make(); + set_map(nm); + const auto p = plan(0.55f, 0.55f, 2.0f, 0.55f, 3.45f, 2.0f); + ASSERT_FALSE(p.poses.empty()); + double max_x = 0.0; + for (const auto & ps : p.poses) { + EXPECT_NEAR(ps.pose.position.z, 2.0, 0.05) << ps.pose.position.x << " " << ps.pose.position.y; + max_x = std::max(max_x, ps.pose.position.x); + } + EXPECT_GT(max_x, 5.0) << "round the base of the U"; +} + +TEST_F(TerrainPlannerTest, AShortcutDoesNotEndUnderTheGoalOnAnotherFloor) +{ + // Ground floor: y < 1.5, and x < 0.5 up to y = 3; from there, at y in [2, 3] (a gap between), a + // ramp up to 1 m at x = 2.5, and an upper floor, x in [2.5, 6], over the ground floor. From the + // ground floor to right over it, on the upper one: round and up the ramp. Most of the way is on + // the ground floor: a shortcut tried there to the goal walked the ground floor all the way to + // right under it, and took it (the multi-floor garage, paper/RESULTADOS.md) + NavMap nm; + SharedVertices shared; + add_grid( + nm, 60, 30, 0.0f, 0.0f, flat, [](float x, float y) {return x < 0.5f || y < 1.5f;}, &shared); + add_grid(nm, 20, 10, 0.5f, 2.0f, [](float x, float) { + return (x - 0.5f) / 2.0f; + }, nullptr, &shared); + add_grid(nm, 35, 30, 2.5f, 0.0f, [](float, float) {return 1.0f;}, nullptr, &shared); + nm.rebuild_geometry_accels(); + make(); + set_map(nm); + const auto p = plan(5.95f, 0.15f, 0.0f, 3.05f, 1.05f, 1.0f); + ASSERT_FALSE(p.poses.empty()); + double max_y = 0.0; + for (std::size_t k = 1; k < p.poses.size(); ++k) { + const auto & a = p.poses[k - 1].pose.position; + const auto & b = p.poses[k].pose.position; + EXPECT_LT(std::abs(b.z - a.z), 0.2) << b.x << " " << b.y << " " << b.z; + max_y = std::max(max_y, b.y); + } + EXPECT_GT(max_y, 2.0) << "up the ramp"; +} + TEST_F(TerrainPlannerTest, NewLimitsAndNewTerrainApplyOnTheNextPlan) { NavMap nm; diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp index 4a7e6a57..878f7376 100644 --- a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp +++ b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp @@ -267,6 +267,57 @@ TEST_F(NavMapPlannerTest, CostWeightIsAParameterWithADefault) EXPECT_DOUBLE_EQ(node_->get_parameter("planner.cost_weight").as_double(), 5.0); } +TEST_F(NavMapPlannerTest, HeuristicWeightDefaultsToAdmissibleAStar) +{ + EXPECT_DOUBLE_EQ(node_->get_parameter("planner.heuristic_weight").as_double(), 1.0); +} + +// A weighted heuristic still finds a path to a reachable goal, and a weight below 1 is taken as 1. +// Weighted A* (without re-opening) keeps its bound: the path costs at most `weight` times the +// optimal one; with no cost but the length, it is at most `weight` times as long. +TEST_F(NavMapPlannerTest, WeightedPathIsWithinTheWeightOfTheOptimalOne) +{ + auto plan_length = [&](double w) { + node_->set_parameter(rclcpp::Parameter("planner.heuristic_weight", w)); + planner_ = std::make_shared(); + planner_->initialize(node_, "planner"); + // a wall with an opening and no other cost: the cost of a path is proportional to its length + set_robot(0.55, 0.55); + inscribe([](float x, float y) {return x > 1.8f && x < 2.2f && (y < 1.5f || y > 2.5f);}); + set_goal(3.06, 3.04); + planner_->update(nav_state_); + const auto & poses = path().poses; + double len = 0.0; + for (std::size_t k = 1; k < poses.size(); ++k) { + len += std::hypot(poses[k].pose.position.x - poses[k - 1].pose.position.x, + poses[k].pose.position.y - poses[k - 1].pose.position.y); + } + return len; + }; + const double optimal = plan_length(1.0); + ASSERT_GT(optimal, 0.0); + for (double w : {1.25, 2.0, 4.0}) { + EXPECT_LE(plan_length(w), w * optimal + 1e-6) << "weight " << w; + } +} + +TEST_F(NavMapPlannerTest, WeightedHeuristicStillReachesTheGoal) +{ + for (double w : {0.5, 1.5, 3.0}) { + node_->set_parameter(rclcpp::Parameter("planner.heuristic_weight", w)); + planner_ = std::make_shared(); + planner_->initialize(node_, "planner"); + set_robot(0.55, 2.0); + inflate(costly_box); + set_goal(3.45, 2.0); + planner_->update(nav_state_); + ASSERT_FALSE(path().poses.empty()) << "weight " << w; + const auto & last = path().poses.back().pose.position; + EXPECT_NEAR(last.x, 3.45, 1e-6) << "weight " << w; + EXPECT_NEAR(last.y, 2.0, 1e-6) << "weight " << w; + } +} + // A costly (not inscribed) band on the straight line: the path goes around it. TEST_F(NavMapPlannerTest, InflationCostKeepsThePathAwayFromObstacles) { @@ -539,3 +590,244 @@ TEST_F(NavMapPlannerTest, AChangedMapWithTheSameSizeIsNotPlannedOnTheOldGraph) ASSERT_FALSE(p.poses.empty()); EXPECT_GT(p.poses.front().pose.position.x, 10.0); } + +namespace +{ +// The planner with its protected parts reachable +class OpenPlanner : public easynav::navmap::AStarPlanner +{ +public: + using easynav::navmap::AStarPlanner::a_star_path; + using easynav::navmap::AStarPlanner::choose_path; + using easynav::navmap::AStarPlanner::last_search_stats_; +}; + +geometry_msgs::msg::Pose at(double x, double y) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.orientation.w = 1.0; + return p; +} + +// A straight polyline from (x0, y) to (x1, y), a pose every 0.1 m +geometry_msgs::msg::Pose at(double x, double y, double z) +{ + auto p = at(x, y); + p.position.z = z; + return p; +} + +std::vector straight(double x0, double x1, double y) +{ + std::vector out; + for (double x = x0; x <= x1 + 1e-9; x += 0.1) { + out.push_back(at(x, y)); + } + return out; +} +} // namespace + +TEST_F(NavMapPlannerTest, ManyPlansInARowAreTheSameAsTheFirstPlanOfANewPlanner) +{ + // The search buffers are reused from plan to plan (no reset of the whole map): what a plan + // left behind must never change the next one + wall_off(2.0, 2.0, 4); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_reused"); + const std::vector> goals = { + {3.45, 3.45}, {0.55, 3.45}, {2.0, 2.0} /* inside the ring: unreachable */, {3.45, 0.55}, + {3.45, 3.45}}; + for (int round = 0; round < 3; ++round) { + for (const auto & [gx, gy] : goals) { + const auto reused = planner->a_star_path(map, at(0.55, 0.55), at(gx, gy)); + auto fresh_planner = std::make_shared(); + fresh_planner->initialize(node_, "planner_fresh_" + std::to_string(round)); + const auto fresh = fresh_planner->a_star_path(map, at(0.55, 0.55), at(gx, gy)); + ASSERT_EQ(reused.size(), fresh.size()) << gx << ", " << gy; + for (std::size_t k = 0; k < fresh.size(); ++k) { + ASSERT_EQ(reused[k].position.x, fresh[k].position.x); + ASSERT_EQ(reused[k].position.y, fresh[k].position.y); + } + } + } +} + +TEST_F(NavMapPlannerTest, ACostLayerThatIsNotUint8IsStillRead) +{ + // The planner reads a uint8 layer of the map's size directly; any other is converted. The float + // layer is "inflated_obstacles", which the planner prefers to "obstacles": a layer with a name of + // its own, because one that changes type under the same name (float "obstacles" over the uint8 + // one the map in the NavState has) makes NavMap::operator= of a map of the same geometry + // dereference a null layer. + auto navmap = nav_state_.get<::navmap::NavMap>("map.navmap"); + for (std::size_t c = 0; c < navmap.navcels.size(); ++c) { + const auto cid = static_cast<::navmap::NavCelId>(c); + const auto p = navmap.navcel_centroid(cid); + // a wall across x = 2 m, as float costs + navmap.layer_set( + "inflated_obstacles", cid, std::abs(p.x() - 2.0f) < 0.15f ? 254.0f : 0.0f); + } + nav_state_.set("map.navmap", navmap); + set_goal(3.45, 0.55); + planner_->update(nav_state_); + EXPECT_TRUE(path().poses.empty()) << "the float wall blocks"; +} + +TEST_F(NavMapPlannerTest, WithHysteresisTheFollowedPathIsKeptUnlessANewOneIsClearlyShorter) +{ + node_->declare_parameter("planner_h.replan_hysteresis", 0.2); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_h"); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + const auto goal = at(3.5, 1.0); + // the path being followed: 3 m along y = 1 + auto followed = planner->choose_path(map, at(0.5, 1.0), goal, straight(0.5, 3.5, 1.0)); + ASSERT_EQ(followed.size(), 31u); + // a new one 5 % shorter: kept the followed one (what is left from the robot) + auto almost = straight(0.6, 3.5, 1.2); + auto kept = planner->choose_path(map, at(0.6, 1.0), goal, almost); + ASSERT_FALSE(kept.empty()); + EXPECT_NEAR(kept.front().position.y, 1.0, 1e-9) << "the followed path, not the new one"; + EXPECT_NEAR(kept.front().position.x, 0.6, 1e-9); + // a new one 40 % shorter: taken + auto much = straight(1.8, 3.5, 0.5); + auto taken = planner->choose_path(map, at(0.6, 1.0), goal, much); + EXPECT_NEAR(taken.front().position.y, 0.5, 1e-9); + // another goal: the new path whatever its length + auto other = planner->choose_path(map, at(0.6, 1.0), at(3.5, 3.5), straight(0.6, 3.5, 3.0)); + EXPECT_NEAR(other.front().position.y, 3.0, 1e-9); +} + +TEST_F(NavMapPlannerTest, WithHysteresisABlockedFollowedPathIsDropped) +{ + node_->declare_parameter("planner_hb.replan_hysteresis", 0.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_hb"); + const auto goal = at(3.5, 1.0); + planner->choose_path( + nav_state_.get<::navmap::NavMap>("map.navmap"), at(0.5, 1.0), goal, straight(0.5, 3.5, 1.0)); + // an obstacle on the followed path (x = 2.5 m): it is no longer traversable + vertical_wall(2.5); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + auto longer = straight(0.5, 3.5, 3.3); // even a much longer path replaces it + longer.insert(longer.begin(), at(0.5, 1.0)); + const auto chosen = planner->choose_path(map, at(0.5, 1.0), goal, longer); + ASSERT_FALSE(chosen.empty()); + EXPECT_NEAR(chosen.back().position.y, 3.3, 1e-9); +} + +TEST_F(NavMapPlannerTest, WithoutHysteresisTheNewPathIsAlwaysTaken) +{ + auto planner = std::make_shared(); + planner->initialize(node_, "planner_noh"); // replan_hysteresis: 0 by default + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + const auto goal = at(3.5, 1.0); + planner->choose_path(map, at(0.5, 1.0), goal, straight(0.5, 3.5, 1.0)); + const auto chosen = planner->choose_path(map, at(0.6, 1.0), goal, straight(0.6, 3.5, 1.2)); + EXPECT_NEAR(chosen.front().position.y, 1.2, 1e-9); +} + +TEST_F(NavMapPlannerTest, WithHysteresisWhatIsLeftOfTheFollowedPathNeverStartsBehindTheRobot) +{ + // The poses of a shortcut path are metres apart: the one nearest the robot can be behind it. + // The rest of the path starts after the segment the robot is on, or the robot would go back to + // that pose, forward, back again... (found by a benchmark: a 0.2 m oscillation for ever) + node_->declare_parameter("planner_hs.replan_hysteresis", 0.2); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_hs"); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + const auto goal = at(3.5, 1.0); + planner->choose_path(map, at(0.5, 1.0), goal, {at(0.5, 1.0), at(2.0, 1.0), at(3.5, 1.0)}); + // the robot 0.6 m along: the pose at 0.5 m is the nearest, but behind + const auto kept = planner->choose_path(map, at(1.1, 1.0), goal, {}); + ASSERT_FALSE(kept.empty()); + EXPECT_NEAR(kept.front().position.x, 2.0, 1e-9); + EXPECT_EQ(kept.size(), 2u); +} + +TEST_F(NavMapPlannerTest, WithAHorizonOnlyTheNearPartOfTheFollowedPathMustStillBeTraversable) +{ + // A level-of-detail map changes far from the robot with every re-mesh: the far part of the + // followed path is checked when the robot gets near, not now + node_->declare_parameter("planner_hz.replan_hysteresis", 0.2); + node_->declare_parameter("planner_hz.replan_hysteresis_horizon", 1.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_hz"); + const auto goal = at(3.5, 1.0); + planner->choose_path( + nav_state_.get<::navmap::NavMap>("map.navmap"), at(0.5, 1.0), goal, straight(0.5, 3.5, 1.0)); + vertical_wall(2.5); // 2 m ahead: beyond the horizon + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + auto longer = straight(0.5, 3.5, 3.3); + longer.insert(longer.begin(), at(0.5, 1.0)); + const auto kept = planner->choose_path(map, at(0.5, 1.0), goal, longer); + ASSERT_FALSE(kept.empty()); + EXPECT_NEAR(kept.back().position.y, 1.0, 1e-9) << "the followed path, blocked only far"; + // the robot 1.3 m from the wall (beyond the first metre, within the horizon): dropped + const auto near = planner->choose_path(map, at(1.2, 1.0), goal, longer); + EXPECT_NEAR(near.back().position.y, 3.3, 1e-9); +} + +TEST_F(NavMapPlannerTest, WithHysteresisASegmentOfTheFollowedPathThroughAnObstacleDropsIt) +{ + // The poses of the followed path are clear, the segment between two of them is not (an obstacle + // appeared between them): the followed path is not kept, even within the first metre + node_->declare_parameter("planner_hg.replan_hysteresis", 0.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_hg"); + const auto goal = at(3.5, 1.0); + planner->choose_path( + nav_state_.get<::navmap::NavMap>("map.navmap"), at(0.5, 1.0), goal, + {at(0.5, 1.0), at(3.5, 1.0)}); + vertical_wall(1.2); // 0.7 m ahead, between the two poses + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + auto longer = straight(0.5, 3.5, 3.3); + longer.insert(longer.begin(), at(0.5, 1.0)); + const auto chosen = planner->choose_path(map, at(0.5, 1.0), goal, longer); + ASSERT_FALSE(chosen.empty()); + EXPECT_NEAR(chosen.back().position.y, 3.3, 1e-9); +} + +TEST_F(NavMapPlannerTest, TheSearchStatsCountWhatTheLastPlanExpandedAndGrowWithTheDistance) +{ + auto planner = std::make_shared(); + planner->initialize(node_, "planner_stats"); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + ASSERT_FALSE(planner->a_star_path(map, at(0.5, 0.5), at(1.0, 0.5)).empty()); + const auto near = planner->last_search_stats_; + EXPECT_GT(near.expanded, 0u); + EXPECT_GE(near.pushed, near.expanded); // every expanded entry was pushed once + ASSERT_FALSE(planner->a_star_path(map, at(0.5, 0.5), at(3.5, 3.5)).empty()); + const auto far = planner->last_search_stats_; + EXPECT_GT(far.expanded, near.expanded); + // a plan that does not search (an empty map) leaves them at zero, not at the previous plan's + ::navmap::NavMap empty; + EXPECT_TRUE(planner->a_star_path(empty, at(0.5, 0.5), at(3.5, 3.5)).empty()); + EXPECT_EQ(planner->last_search_stats_.expanded, 0u); + EXPECT_EQ(planner->last_search_stats_.pushed, 0u); +} + +TEST_F(NavMapPlannerTest, WithHysteresisWhatIsLeftOfTheFollowedPathIsFoundInHeightToo) +{ + // A garage: the followed path goes along this floor and later passes right under the robot + // (its end on the floor 2.3 m below). The rest of the path is what follows the part the robot + // is on (0.1 m away in the plane), not the part under it (0 m away in the plane, 2.3 m in + // height): found in the plane, the robot would be "at" that part and stay over the goal + node_->declare_parameter("planner_h3.replan_hysteresis", 0.2); + node_->declare_parameter("planner_h3.replan_hysteresis_horizon", 1.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_h3"); + const auto & map = nav_state_.get<::navmap::NavMap>("map.navmap"); + const auto goal = at(0.5, 1.1, -2.3); + planner->choose_path( + map, at(0.5, 1.0), goal, + {at(0.5, 1.0), at(3.5, 1.0), at(3.5, 1.0, -2.3), at(1.1, 1.1, -2.3), goal}); + const auto kept = planner->choose_path(map, at(1.1, 1.1), goal, {}); + ASSERT_FALSE(kept.empty()) << "the followed path is kept from the part on this floor"; + EXPECT_NEAR(kept.front().position.x, 3.5, 1e-9); + EXPECT_NEAR(kept.front().position.z, 0.0, 1e-9); + EXPECT_EQ(kept.size(), 4u); +} From 44f57417742bfa6655f92d1603f004504bf1fb90 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Thu, 5 Nov 2026 12:06:45 +0100 Subject: [PATCH 2/2] navmap_planner: fix stale closed stamps and float cost layers in the hysteresis; named constants; tests --- .../easynav_navmap_planner/AStarPlanner.cpp | 63 +++++++++++-------- .../tests/navmap_planner_tests.cpp | 55 ++++++++++++++++ 2 files changed, 91 insertions(+), 27 deletions(-) diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 4ac93e61..c305609d 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -39,6 +39,13 @@ namespace easynav namespace navmap { +// How far from the start (m) the inscribed band is still traversable: the robot may be in it. +constexpr double kEscapeDistance = 1.0; +// How far (m) the robot may be from the path it follows for it to be kept (replan hysteresis). +constexpr double kMaxPathOffset = 2.0; +// A point is on a surface if the surface is within this height (m) of it: floors over each other. +constexpr float kSurfaceHeightTolerance = 0.5f; + static double compute_path_length(const nav_msgs::msg::Path & path) { double total_length = 0.0; @@ -218,12 +225,8 @@ std::vector AStarPlanner::choose_path( } return l; }; - // What is left of the followed path: the poses after its segment nearest the robot (the pose - // nearest the robot may be behind it: the poses of a shortcut path are metres apart, and - // going back to it, then forward, then back again is an oscillation). Nearest in 3D: with - // floors over each other the path may pass right under the robot (its end on the floor below, - // the robot on its way down above it); nearest in the plane, that part would be "what is left" - // and the robot would stay over the goal + // What is left of the path: the poses after its segment nearest the robot, in 3D (a path may + // pass right under the robot on floors over each other). Nearest pose could be behind it. std::size_t from = 1; double best = std::numeric_limits::infinity(); for (std::size_t k = 0; k + 1 < followed_.size(); ++k) { @@ -237,14 +240,18 @@ std::vector AStarPlanner::choose_path( a.z + t * (b.z - a.z) - robot.position.z); if (d < best) {best = d; from = k + 1;} } - // Still traversable on this map (it may have changed under it), up to the horizon: the path - // from the robot, sampled along its segments (its poses are metres apart: a segment can cut a - // corner its ends do not), on NavCels below the inscribed cost and within the terrain limits. - // In the first metre the inscribed band is allowed (the robot may be in it, as a plan from there - // allows), an obstacle or a hole is not. + // Still traversable on the current map, up to the horizon: sampled along the path from the robot, + // below the inscribed cost (kEscapeDistance around the robot: below lethal) and in the terrain + // limits. The cost is read as a_star_path does (any layer type). + const std::string cost_layer = + map.has_layer("inflated_obstacles") ? "inflated_obstacles" : "obstacles"; const auto cost = std::dynamic_pointer_cast<::navmap::LayerView>( - map.layers.get(map.has_layer("inflated_obstacles") ? "inflated_obstacles" : "obstacles")); - bool valid = best < 2.0; + map.layers.get(cost_layer)); + auto cost_at = [&](::navmap::NavCelId c) -> std::uint8_t { + return cost && c < cost->size() ? (*cost)[c] : + map.layer_get(cost_layer, c, navmap_ros::FREE_SPACE); + }; + bool valid = best < kMaxPathOffset; ::navmap::NavMap::LocateOpts opts; const double step = std::clamp(cel_spacing_, 0.05, 0.25); double along = 0.0; @@ -269,9 +276,9 @@ std::vector AStarPlanner::choose_path( break; } opts.hint_cid = c; - const std::uint8_t v = cost && c < cost->size() ? (*cost)[c] : navmap_ros::FREE_SPACE; + const std::uint8_t v = cost_at(c); const bool near = (q - Eigen::Vector3d(robot.position.x, robot.position.y, - robot.position.z)).head<2>().norm() < 1.0; + robot.position.z)).head<2>().norm() < kEscapeDistance; valid = (near ? v < navmap_ros::LETHAL_OBSTACLE : v < navmap_ros::INSCRIBED_INFLATED_OBSTACLE) && (terrain_ok_.size() != map.navcels.size() || terrain_ok_[c]); @@ -280,13 +287,19 @@ std::vector AStarPlanner::choose_path( prev = next; } if (!valid) {return remember(std::move(fresh));} - // the new path only if it is clearly shorter (or there is no other) + // The new path only if it is clearly shorter. If A* found none (fresh is empty) but this one is + // still valid up to the horizon, keep it; it is checked every cycle and replaced as soon as it + // is no longer valid. const double left = std::hypot( followed_[from].position.x - robot.position.x, followed_[from].position.y - robot.position.y) + length(followed_, from); if (!fresh.empty() && length(fresh, 0) < (1.0 - replan_hysteresis_) * left) { return remember(std::move(fresh)); } + if (fresh.empty()) { + RCLCPP_DEBUG(get_node()->get_logger(), "A* found no path: keeping the followed one"); + } + // It starts at followed_[from], ahead of the robot; the first segment was checked above std::vector kept(followed_.begin() + static_cast(from), followed_.end()); return remember(std::move(kept)); @@ -350,7 +363,7 @@ AStarPlanner::path_smoother( const Eigen::Vector3f on = ::navmap::closest_point_on_triangle( pts[i], navmap.positions.at(tri.v[0]), navmap.positions.at(tri.v[1]), navmap.positions.at(tri.v[2])); - ok = std::abs(on.z() - pts[i].z()) < 0.5f; + ok = std::abs(on.z() - pts[i].z()) < kSurfaceHeightTolerance; } if (!ok) { // Fallback to closest triangle (keeps the query on-surface) @@ -555,6 +568,7 @@ void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) g_.assign(N, std::numeric_limits::infinity()); parent_.assign(N, std::numeric_limits<::navmap::NavCelId>::max()); g_stamp_.assign(N, 0); + closed_stamp_.clear(); // epoch_ restarts: old stamps would read as closed (sized when used) epoch_ = 0; } if (++epoch_ == 0) { // wrapped: clear the stamps once @@ -617,7 +631,7 @@ std::vector AStarPlanner::shortcut_path( auto inside = [&](NavCelId c) { const auto t = vertices(c); return in_triangle_xy(p, t[0], t[1], t[2]) && - std::abs(height_at(p, t[0], t[1], t[2]) - p.z()) < 0.5f; + std::abs(height_at(p, t[0], t[1], t[2]) - p.z()) < kSurfaceHeightTolerance; }; if (inside(from)) {out = from; return true;} for (const auto & nb : neighbors_[from]) { @@ -677,7 +691,7 @@ std::vector AStarPlanner::shortcut_path( NavCelId c; if (!locate_near(b, cur, c)) {return false;} const auto t = vertices(c); - return std::abs(height_at(b, t[0], t[1], t[2]) - b.z()) < 0.5f; + return std::abs(height_at(b, t[0], t[1], t[2]) - b.z()) < kSurfaceHeightTolerance; }; // Greedy: from each kept waypoint, the farthest visible one (exponential, then binary search) @@ -816,10 +830,8 @@ std::vector AStarPlanner::a_star_path( const std::uint8_t v = occ(c); return v < INSCRIBED_INFLATED_OBSTACLE && terrain_ok_[c]; }; - // How far from the start inscribed cells are still traversable (m). - constexpr double kEscapeDistance = 1.0; // The robot may already be within the inscribed band (e.g. stopped by a reflex): it can - // leave it, unless it is on an obstacle or an unknown cell. + // leave it (within kEscapeDistance), unless it is on an obstacle or an unknown cell. auto can_start_at = [&](NavCelId c) -> bool { const std::uint8_t v = occ(c); return (v != LETHAL_OBSTACLE) && (v != NO_INFORMATION); @@ -894,11 +906,8 @@ std::vector AStarPlanner::a_star_path( open.push(Node{cid_start, h(cid_start, cid_goal)}); last_search_stats_ = SearchStats{0, 1}; - // Weighted A* (heuristic_weight > 1): the heuristic is no longer consistent, so a NavCel can be - // reached more cheaply after it was expanded. Re-opening it can make the search explode on large - // maps; weighted A* without re-opening keeps the bound (paths at most heuristic_weight times the - // optimal cost), so each NavCel is expanded once and stale queue entries are skipped. With the - // admissible heuristic (weight 1) the search is the classic one, unchanged. + // Weighted A* (heuristic_weight > 1) without re-opening: each NavCel is expanded once (paths at + // most heuristic_weight times the optimal cost). With weight 1 the search is the classic one. const bool weighted = heuristic_weight_ > 1.0; if (weighted && closed_stamp_.size() != N) {closed_stamp_.assign(N, 0);} auto closed = [&](NavCelId c) {return closed_stamp_[c] == epoch_;}; diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp index 878f7376..7cdeaaf3 100644 --- a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp +++ b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp @@ -831,3 +831,58 @@ TEST_F(NavMapPlannerTest, WithHysteresisWhatIsLeftOfTheFollowedPathIsFoundInHeig EXPECT_NEAR(kept.front().position.z, 0.0, 1e-9); EXPECT_EQ(kept.size(), 4u); } + +TEST_F(NavMapPlannerTest, WeightedAStarStillPlansWhenTheMapChangesSizeAndComesBack) +{ + // A level-of-detail map changes size all the time. A plan that returns before searching (here, + // the goal is on an obstacle) must not leave the closed stamps of the first map to the third + node_->declare_parameter("planner_ws.heuristic_weight", 1.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_ws"); + const auto big = nav_state_.get<::navmap::NavMap>("map.navmap"); // 40 x 40 cells + const auto start = at(0.55, 0.55); + const auto goal = at(3.45, 0.55); + const auto first = planner->a_star_path(big, start, goal); + ASSERT_FALSE(first.empty()); + + auto grid = grid_; + grid.info.width = 30; + grid.info.height = 30; + grid.data.assign(30 * 30, 0); + grid.data[5 * 30 + 25] = 100; // the cell of (2.55, 0.55) + auto small = navmap_ros::from_occupancy_grid(grid); + for (std::size_t c = 0; c < small.navcels.size(); ++c) { + const auto cid = static_cast<::navmap::NavCelId>(c); + small.layer_set( + "obstacles", cid, small.layer_get("occupancy", cid, 0)); + } + EXPECT_TRUE(planner->a_star_path(small, start, at(2.55, 0.55)).empty()); + + const auto again = planner->a_star_path(big, start, goal); + ASSERT_FALSE(again.empty()) << "NavCels of the first search read as closed"; + EXPECT_EQ(again.size(), first.size()); +} + +TEST_F(NavMapPlannerTest, WithHysteresisAnObstacleOfAFloatCostLayerOnTheFollowedPathDropsIt) +{ + node_->declare_parameter("planner_hf.replan_hysteresis", 0.5); + auto planner = std::make_shared(); + planner->initialize(node_, "planner_hf"); + const auto goal = at(3.5, 1.0); + planner->choose_path( + nav_state_.get<::navmap::NavMap>("map.navmap"), at(0.5, 1.0), goal, straight(0.5, 3.5, 1.0)); + + // a wall across x = 2.5 m in a float layer, the type the planner converts instead of reading + auto walled = nav_state_.get<::navmap::NavMap>("map.navmap"); + for (std::size_t c = 0; c < walled.navcels.size(); ++c) { + const auto cid = static_cast<::navmap::NavCelId>(c); + const auto p = walled.navcel_centroid(cid); + walled.layer_set( + "inflated_obstacles", cid, std::abs(p.x() - 2.5f) < 0.15f ? 254.0f : 0.0f); + } + auto longer = straight(0.5, 3.5, 3.3); + longer.insert(longer.begin(), at(0.5, 1.0)); + const auto chosen = planner->choose_path(walled, at(0.5, 1.0), goal, longer); + ASSERT_FALSE(chosen.empty()); + EXPECT_NEAR(chosen.back().position.y, 3.3, 1e-9) << "the followed path crosses the wall"; +}