Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,9 @@ class AStarPlanner : public PlannerMethodBase
/// Cached neighbors of each NavCel, and NavCels around each vertex.
std::vector<std::vector<Neighbor>> neighbors_;
std::vector<std::vector<::navmap::NavCelId>> 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<std::uint8_t> border_vertex_;

/// Distance between the centroids of neighbor NavCels (mean): the path's spacing.
double cel_spacing_ {0.1};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<NavCelId>(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<std::uint32_t> 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<NavCelId>(N); ++c) {
if (degenerate(c)) {continue;}
auto & out = neighbors_[c];
for (const auto v : map.navcels[c].v) {
if (v >= V) {continue;}
Expand Down Expand Up @@ -933,8 +964,11 @@ std::vector<geometry_msgs::msg::Pose> 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) {
Expand Down
68 changes: 68 additions & 0 deletions planners/easynav_navmap_planner/tests/navmap_planner_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<typename Keep>
::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<std::uint8_t>("obstacles", "", "", 0);
out.rebuild_geometry_accels();
return out;
}

geometry_msgs::msg::Pose at(double x, double y)
{
geometry_msgs::msg::Pose p;
Expand Down Expand Up @@ -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<OpenPlanner>();
planner->initialize(node_, "planner_holes");
const auto & grid = nav_state_.get<::navmap::NavMap>("map.navmap");
auto cell = [](float m) {return static_cast<int>(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<OpenPlanner>();
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<std::size_t>(n.cid), first) << "NavCel " << c;
}
}
EXPECT_FALSE(planner->a_star_path(map, at(0.05, 0.05), at(3.45, 3.45)).empty());
}