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 10db46b7..6bfbf267 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -129,6 +129,9 @@ class AStarPlanner : public PlannerMethodBase /// Cached neighbors of each NavCel, and NavCels around each vertex. std::vector> neighbors_; std::vector> vertex_cels_; + // 1 = the vertex is on the border of the map (the corner of a hole, its outer edge): no step + // through it from one NavCel to another that only shares it (that would cut the hole's corner) + std::vector border_vertex_; /// Distance between the centroids of neighbor NavCels (mean): the path's spacing. double cel_spacing_ {0.1}; 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 c305609d..a71c7c20 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -527,18 +527,49 @@ void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) centroids_[c] = Eigen::Vector3f{cc.x(), cc.y(), cc.z()}; } + // Degenerate NavCels (a repeated vertex: no area) are left out of the graph: they have no + // neighbours. A map can have many, e.g. NavCels a filter removed by collapsing them onto one + // vertex; in vertex_cels_ they would make every one of them a neighbour of every other + // (quadratic in their number). + auto degenerate = [&map](NavCelId c) { + const auto & v = map.navcels[c].v; + return v[0] == v[1] || v[1] == v[2] || v[0] == v[2]; + }; + vertex_cels_.assign(V, {}); for (NavCelId c = 0; c < static_cast(N); ++c) { + if (degenerate(c)) {continue;} for (const auto v : map.navcels[c].v) { if (v < V) {vertex_cels_[v].push_back(c);} } } + // A vertex is on the border when some edge at it belongs to one NavCel only: the NavCels + // around it do not close a fan + border_vertex_.assign(V, 0); + std::vector ends; + for (std::size_t v = 0; v < V; ++v) { + ends.clear(); + for (const auto c : vertex_cels_[v]) { + for (const auto w : map.navcels[c].v) { + if (w != v) {ends.push_back(w);} + } + } + std::sort(ends.begin(), ends.end()); + for (std::size_t i = 0; i < ends.size(); ) { + std::size_t j = i; + while (j < ends.size() && ends[j] == ends[i]) {++j;} + if (j - i == 1) {border_vertex_[v] = 1; break;} + i = j; + } + } + // Neighbors through a shared vertex; two shared vertices are a shared edge neighbors_.assign(N, {}); double spacing_sum = 0.0; std::size_t spacing_count = 0; for (NavCelId c = 0; c < static_cast(N); ++c) { + if (degenerate(c)) {continue;} auto & out = neighbors_[c]; for (const auto v : map.navcels[c].v) { if (v >= V) {continue;} @@ -933,8 +964,11 @@ std::vector AStarPlanner::a_star_path( (centroids_[v] - centroids_[cid_start]).norm() < kEscapeDistance; if (!traversable(v) && !escaping) {continue;} - // Only a vertex in common: no cutting a corner, every NavCel around it must be passable + // Only a vertex in common: no cutting a corner, every NavCel around it must be passable, + // and the vertex must not be on the border of the map (the corner of a hole: the step would + // go through it) if (nb.shared_vertex != kNoVertex) { + if (border_vertex_[nb.shared_vertex]) {continue;} const auto & around = vertex_cels_[nb.shared_vertex]; const bool clear = std::all_of( around.begin(), around.end(), [&](NavCelId c) { diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp index 7cdeaaf3..2a4974a4 100644 --- a/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp +++ b/planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp @@ -600,8 +600,33 @@ class OpenPlanner : public easynav::navmap::AStarPlanner using easynav::navmap::AStarPlanner::a_star_path; using easynav::navmap::AStarPlanner::choose_path; using easynav::navmap::AStarPlanner::last_search_stats_; + using easynav::navmap::AStarPlanner::ensure_graph_cache; + using easynav::navmap::AStarPlanner::neighbors_; }; +// A copy of \p src with the NavCels where \p keep is true, \p degenerate more NavCels on its +// vertex 0 (no area) and a free "obstacles" layer +template +::navmap::NavMap remade(const ::navmap::NavMap & src, Keep keep, std::size_t degenerate = 0) +{ + ::navmap::NavMap out; + out.positions = src.positions; + const auto s = out.create_surface("map"); + for (std::size_t c = 0; c < src.navcels.size(); ++c) { + const auto cid = static_cast<::navmap::NavCelId>(c); + const auto q = src.navcel_centroid(cid); + if (!keep(q.x(), q.y())) {continue;} + const auto & v = src.navcels[c].v; + out.add_navcel_to_surface(s, out.add_navcel(v[0], v[1], v[2])); + } + for (std::size_t k = 0; k < degenerate; ++k) { + out.add_navcel_to_surface(s, out.add_navcel(0, 0, 0)); + } + out.add_layer("obstacles", "", "", 0); + out.rebuild_geometry_accels(); + return out; +} + geometry_msgs::msg::Pose at(double x, double y) { geometry_msgs::msg::Pose p; @@ -886,3 +911,46 @@ TEST_F(NavMapPlannerTest, WithHysteresisAnObstacleOfAFloatCostLayerOnTheFollowed ASSERT_FALSE(chosen.empty()); EXPECT_NEAR(chosen.back().position.y, 3.3, 1e-9) << "the followed path crosses the wall"; } + +TEST_F(NavMapPlannerTest, HolesTouchingAtACornerAreAWall) +{ + // A diagonal line of cells without NavCels (holes: e.g. buildings cut out of the map), each + // touching the next only at a corner, across the map. The free NavCels on both sides share + // those corners, but a robot cannot go through a point + auto planner = std::make_shared(); + planner->initialize(node_, "planner_holes"); + const auto & grid = nav_state_.get<::navmap::NavMap>("map.navmap"); + auto cell = [](float m) {return static_cast(std::floor(m / kResolution));}; + const auto rising = remade(grid, [&](float x, float y) {return cell(x) != cell(y);}); + EXPECT_TRUE(planner->a_star_path(rising, at(2.55, 0.55), at(0.55, 2.55)).empty()); + EXPECT_FALSE(planner->a_star_path(rising, at(2.55, 0.55), at(3.45, 1.55)).empty()) + << "on the same side there is a path"; + + // the other way (against the mesh's split diagonal) + const auto falling = remade( + grid, [&](float x, float y) {return cell(x) != kCells - 1 - cell(y);}); + EXPECT_TRUE(planner->a_star_path(falling, at(0.55, 0.55), at(3.45, 3.45)).empty()); +} + +TEST_F(NavMapPlannerTest, DegenerateNavCelsAreLeftOutOfTheGraph) +{ + // NavCels with no area all on one vertex (what a filter that removes NavCels may leave): they + // are nobody's neighbours, or each would be a neighbour of all the others (quadratic) + const std::size_t kDegenerate = 2000; + const auto & grid = nav_state_.get<::navmap::NavMap>("map.navmap"); + const auto map = remade(grid, [](float, float) {return true;}, kDegenerate); + const std::size_t first = map.navcels.size() - kDegenerate; + auto planner = std::make_shared(); + planner->initialize(node_, "planner_degenerate"); + planner->ensure_graph_cache(map); + for (std::size_t c = 0; c < map.navcels.size(); ++c) { + if (c >= first) { + EXPECT_TRUE(planner->neighbors_[c].empty()) << "degenerate NavCel " << c; + continue; + } + for (const auto & n : planner->neighbors_[c]) { + EXPECT_LT(static_cast(n.cid), first) << "NavCel " << c; + } + } + EXPECT_FALSE(planner->a_star_path(map, at(0.05, 0.05), at(3.45, 3.45)).empty()); +}