feat(navigator): keep the contingency margin in the route planner

A mission keeps GF_CONT_H_MARGIN from the fence, and so does the route
of the Return avoidance planner. The fence checks of the mission route
planner did not know the margin: a route they accept could run closer
to the fence and trigger GF_CONT_ACTION on its way. Example: fence
500 m from Home, margin 40 m, rally point 480 m from Home. The planner
took the rally point, and the action triggered 20 m before it.

The planner's segments now keep the margin from the polygons and
circles, and a rally point also has to keep it from GF_MAX_HOR_DIST.

A vehicle that is already closer to the fence than the margin cannot
keep it. Its own segments are then checked against the fence itself:
the Return that GF_CONT_ACTION starts there still finds a route, but
not one that crosses the fence.

Navigator does not use this planner yet, so nothing flies differently.

Assisted-by: Claude:claude-fable-5-2
Signed-off-by: jonas <jonspero@me.com>
This commit is contained in:
jonas committed 2026-11-19 22:47:02 +01:00
1 parent 2b6d876130
commit c10e6fae40
3 files changed
+181 -31

No files matched your search

@@ -79,6 +79,25 @@ bool checkBatch(Geofence &geofence, PathBatch &batch)
return true;
}
// Start a path at the vehicle with the margin it can keep: GF_CONT_H_MARGIN, or 0 if the vehicle is already closer
// to the fence. Example: the Return that GF_CONT_ACTION starts there is checked against the fence itself.
bool startVehiclePath(Geofence &geofence, const Position &vehicle, Geofence::PathCheck &path)
{
path = {{vehicle.lat, vehicle.lon}, {vehicle.lat, vehicle.lon}, 0.f, geofence.getContGeofenceMargin()};
bool keeps_margin = true;
// A path without length tells whether the vehicle keeps the margin, as for the first waypoint of a mission.
if (path.margin > 0.f && !geofence.checkPathBatch(&path, 1, &keeps_margin)) {
return false;
}
if (!keeps_margin) {
path.margin = 0.f;
}
return true;
}
} // namespace
// Polygons and circles only: a vehicle above the altitude limits or too far from Home does not recover, so its
@@ -110,6 +129,9 @@ bool GeofencePathChecker::pruneCandidates(ProjectionReferenceBatch &batch) const
continue;
}
// Segments keep GF_CONT_H_MARGIN from the polygons and circles, like the legs of a mission.
float margin = _geofence.getContGeofenceMargin();
if (reference.source_index < 0) {
// A vehicle in recovery keeps all its candidates unchecked: set bits 0 to count - 1.
if (recovering(anchor)) {
@@ -117,9 +139,19 @@ bool GeofencePathChecker::pruneCandidates(ProjectionReferenceBatch &batch) const
continue;
}
} else if (!_geofence.checkPointAgainstAllGeofences(anchor.lat, anchor.lon, anchor.alt)) {
// Scratch off the stack: a full batch is checked at once, so the next path of the batch is always free.
Geofence::PathCheck &scratch = shared_paths.paths[shared_paths.count];
if (!startVehiclePath(_geofence, anchor, scratch)) {
return false;
}
margin = scratch.margin;
} else if (!_geofence.checkPointAgainstAllGeofences(anchor.lat, anchor.lon, anchor.alt)
|| !_geofence.isCloserThanContMaxDistToHome(anchor.lat, anchor.lon)) {
// A safe point outside the allowed area keeps no candidates: a segment entirely inside an exclusion
// area would cross no boundary.
// area would cross no boundary. Neither does one closer to GF_MAX_HOR_DIST than the margin.
continue;
}
@@ -130,7 +162,7 @@ bool GeofencePathChecker::pruneCandidates(ProjectionReferenceBatch &batch) const
continue;
}
shared_paths.paths[shared_paths.count] = {{anchor.lat, anchor.lon}, {projection.lat, projection.lon}};
shared_paths.paths[shared_paths.count] = {{anchor.lat, anchor.lon}, {projection.lat, projection.lon}, 0.f, margin};
shared_paths.reference_indices[shared_paths.count] = i;
shared_paths.candidate_indices[shared_paths.count] = j;
++shared_paths.count;
@@ -181,7 +213,13 @@ bool GeofencePathChecker::checkPath(const Position &vehicle, const Position &end
return true;
}
const Geofence::PathCheck path{{vehicle.lat, vehicle.lon}, {end.lat, end.lon}};
Geofence::PathCheck path;
if (!startVehiclePath(_geofence, vehicle, path)) {
return false;
}
path.end = {end.lat, end.lon};
return _geofence.checkPathBatch(&path, 1, &clear);
}
@@ -44,11 +44,14 @@ namespace mission_route
* Checks route segments against the loaded Navigator geofence.
*
* Calls must not overlap because they reuse the same scratch buffer; Navigator calls it from a single task.
* Segments are checked against the polygons and circles. Mission legs passed the altitude and Home limits in the
* mission check, and these limits are convex, so points on the legs are not checked again: plan only on a checked
* mission. Safe points are checked against every limit.
* Segments are checked against the polygons and circles and keep GF_CONT_H_MARGIN from them, like the legs of a
* mission. Mission legs passed the altitude and Home limits in the mission check, and these limits are convex, so
* points on the legs are not checked again: plan only on a checked mission. Safe points are checked against every
* limit.
* When the vehicle breaches a polygon or circle, segments from the vehicle, including every
* checkPath() segment, are not checked so it can recover. Safe point segments still are.
* A vehicle closer to a polygon or circle than the margin cannot keep it: its segments are checked against the
* fence itself over their whole length, for every polygon and circle.
* Planning must still wait for the fence to finish loading.
*/
class GeofencePathChecker final : public PathChecker
@@ -57,6 +57,12 @@ protected:
void TearDown() override { param_reset_all(); }
void setMargin(float margin)
{
ASSERT_EQ(param_set(param_find("GF_CONT_H_MARGIN"), &margin), PX4_OK);
_navigator.updateParams();
}
Position routePosition(float north, float east, float altitude = kAltitude) const
{
const matrix::Vector2d coordinates = position(north, east);
@@ -142,32 +148,39 @@ TEST_F(MissionRouteGeofenceTest, ChecksMoreThanOnePathBatchAndThePartialTail)
ASSERT_TRUE(loadFence(exclusionSquare()));
GeofencePathChecker checker{_fence};
ProjectionReferenceBatch batch{};
batch.count = kReferenceCount;
for (uint8_t i = 0; i < batch.count; ++i) {
const float north = i % 2 == 0 ? 0.f : 60.f;
batch.items[i].position = routePosition(north, 100.f);
addCandidate(batch.items[i], routePosition(north, 500.f), 1);
addCandidate(batch.items[i], routePosition(north, 200.f), 2);
addCandidate(batch.items[i], routePosition(north, 600.f), 3);
}
// With a margin, the check of each vehicle takes the next free path of the batch, also after a full batch.
for (const float margin : {0.f, 40.f}) {
SCOPED_TRACE(margin);
setMargin(margin);
ProjectionReferenceBatch batch{};
batch.count = kReferenceCount;
ASSERT_TRUE(checker.pruneCandidates(batch));
for (uint8_t i = 0; i < batch.count; ++i) {
const float north = i % 2 == 0 ? 0.f : 60.f;
batch.items[i].position = routePosition(north, 100.f);
addCandidate(batch.items[i], routePosition(north, 500.f), 1);
addCandidate(batch.items[i], routePosition(north, 200.f), 2);
addCandidate(batch.items[i], routePosition(north, 600.f), 3);
}
for (uint8_t i = 0; i < batch.count; ++i) {
SCOPED_TRACE(i);
const auto &candidates = batch.items[i].candidate_buffer;
ASSERT_TRUE(checker.pruneCandidates(batch));
if (i % 2 == 0) {
ASSERT_EQ(candidates.count, 1);
EXPECT_EQ(candidates.candidates[0].segment.start.idx, 2);
for (uint8_t i = 0; i < batch.count; ++i) {
SCOPED_TRACE(i);
const auto &candidates = batch.items[i].candidate_buffer;
} else {
ASSERT_EQ(candidates.count, 3);
// The segments at 60 m north pass 10 m from the square: clear without a margin only.
if (i % 2 == 0 || margin > 0.f) {
ASSERT_EQ(candidates.count, 1);
EXPECT_EQ(candidates.candidates[0].segment.start.idx, 2);
for (uint8_t j = 0; j < candidates.count; ++j) {
EXPECT_EQ(candidates.candidates[j].segment.start.idx, j + 1);
} else {
ASSERT_EQ(candidates.count, 3);
for (uint8_t j = 0; j < candidates.count; ++j) {
EXPECT_EQ(candidates.candidates[j].segment.start.idx, j + 1);
}
}
}
}
@@ -336,13 +349,20 @@ TEST_F(MissionRouteGeofenceTest, VehicleCandidatesInPruneAgreeWithCheckPath)
struct FenceCase {
const char *name;
FencePoints points;
float margin;
};
// With the margin, each fence case also has spots closer to the fence than that.
const FencePoints inclusion = polygon(true, {{-250.f, -250.f}, {250.f, -250.f}, {250.f, 400.f}, {-250.f, 400.f}});
const FenceCase fence_cases[] {
{"exclusion polygon", exclusionSquare()},
{"inclusion polygon", polygon(true, {{-250.f, -250.f}, {250.f, -250.f}, {250.f, 400.f}, {-250.f, 400.f}})},
{"exclusion circle", circle(false, {0.f, 300.f}, 60.f)},
{"inclusion circle", circle(true, {0.f, 150.f}, 300.f)},
{"exclusion polygon", exclusionSquare(), 0.f},
{"inclusion polygon", inclusion, 0.f},
{"exclusion circle", circle(false, {0.f, 300.f}, 60.f), 0.f},
{"inclusion circle", circle(true, {0.f, 150.f}, 300.f), 0.f},
{"exclusion polygon with a margin", exclusionSquare(), 160.f},
{"inclusion polygon with a margin", inclusion, 160.f},
{"exclusion circle with a margin", circle(false, {0.f, 300.f}, 60.f), 160.f},
{"inclusion circle with a margin", circle(true, {0.f, 150.f}, 300.f), 160.f},
};
// Each fence case has spots outside the allowed area, where the vehicle recovers.
const Position spots[] {routePosition(0.f, 100.f), routePosition(100.f, 100.f), routePosition(0.f, 300.f),
@@ -354,6 +374,7 @@ TEST_F(MissionRouteGeofenceTest, VehicleCandidatesInPruneAgreeWithCheckPath)
for (const FenceCase &fence_case : fence_cases) {
SCOPED_TRACE(fence_case.name);
ASSERT_TRUE(loadFence(fence_case.points));
setMargin(fence_case.margin);
for (size_t spot = 0; spot < 5; ++spot) {
SCOPED_TRACE(::testing::Message() << "vehicle at spot " << spot);
@@ -589,3 +610,91 @@ TEST_F(MissionRouteGeofenceTest, CurrentAltitudeHopToLandingIsChecked)
EXPECT_EQ(planner.planMissionResumeJoin(rtlRequest(vehicle, 1), join), mission_route::FailureReason::kNoValidPath);
EXPECT_FALSE(join.valid());
}
// A segment keeps GF_CONT_H_MARGIN from the fence, like a mission leg. Both segments pass north of the exclusion
// square: one 50 m from it, the other 25 m.
TEST_F(MissionRouteGeofenceTest, SegmentsKeepTheContingencyMargin)
{
ASSERT_TRUE(loadFence(exclusionSquare()));
const Position vehicle = routePosition(100.f, 100.f);
const Position far_end = routePosition(100.f, 500.f);
const Position near_end = routePosition(60.f, 500.f);
GeofencePathChecker checker{_fence};
for (const float margin : {0.f, 40.f}) {
SCOPED_TRACE(margin);
setMargin(margin);
ProjectionReferenceBatch batch{};
batch.count = 1;
batch.items[0].position = vehicle;
addCandidate(batch.items[0], far_end, 1);
addCandidate(batch.items[0], near_end, 2);
ASSERT_TRUE(checker.pruneCandidates(batch));
ASSERT_EQ(batch.items[0].candidate_buffer.count, margin > 0.f ? 1 : 2);
EXPECT_EQ(batch.items[0].candidate_buffer.candidates[0].segment.start.idx, 1);
bool clear = false;
ASSERT_TRUE(checker.checkPath(vehicle, far_end, clear));
EXPECT_TRUE(clear);
ASSERT_TRUE(checker.checkPath(vehicle, near_end, clear));
EXPECT_EQ(clear, margin <= 0.f);
}
}
// A safe point closer to the fence than the margin keeps no candidates, also for GF_MAX_HOR_DIST.
TEST_F(MissionRouteGeofenceTest, SafePointWithinTheContingencyMarginKeepsNoCandidates)
{
const auto candidates_kept = [this](const Position & anchor, const Position & projection, int32_t source_index = 0) {
GeofencePathChecker checker{_fence};
ProjectionReferenceBatch batch{};
batch.count = 1;
batch.items[0].position = anchor;
batch.items[0].source_index = source_index;
addCandidate(batch.items[0], projection);
EXPECT_TRUE(checker.pruneCandidates(batch));
return batch.items[0].candidate_buffer.count;
};
// 30 m west of the exclusion square, with a segment that leads away from it.
ASSERT_TRUE(loadFence(exclusionSquare()));
EXPECT_EQ(candidates_kept(routePosition(0.f, 220.f), routePosition(0.f, 100.f)), 1);
setMargin(40.f);
EXPECT_EQ(candidates_kept(routePosition(0.f, 220.f), routePosition(0.f, 100.f)), 0);
EXPECT_EQ(candidates_kept(routePosition(0.f, 200.f), routePosition(0.f, 100.f)), 1);
// 280 m from Home with a limit of 300 m.
ASSERT_TRUE(loadFence({}));
const float maximum_horizontal = 300.f;
ASSERT_EQ(param_set(param_find("GF_MAX_HOR_DIST"), &maximum_horizontal), PX4_OK);
_navigator.updateParams();
EXPECT_EQ(candidates_kept(routePosition(0.f, 280.f), routePosition(0.f, 200.f)), 0);
EXPECT_EQ(candidates_kept(routePosition(0.f, 250.f), routePosition(0.f, 200.f)), 1);
// A vehicle there keeps its candidate: the Return that GF_CONT_ACTION starts at the limit needs a route.
EXPECT_EQ(candidates_kept(routePosition(0.f, 280.f), routePosition(0.f, 200.f), -1), 1);
}
// A vehicle closer to the fence than the margin cannot keep it: its segments are checked against the fence itself.
// The Return that GF_CONT_ACTION starts there gets a route, but not one through the exclusion square.
TEST_F(MissionRouteGeofenceTest, VehicleWithinTheContingencyMarginIsCheckedAgainstTheFence)
{
ASSERT_TRUE(loadFence(exclusionSquare()));
setMargin(40.f);
const Position vehicle = routePosition(0.f, 220.f); // 30 m west of the square
const Position away = routePosition(0.f, 100.f);
const Position across = routePosition(0.f, 500.f);
GeofencePathChecker checker{_fence};
ProjectionReferenceBatch batch{};
batch.count = 1;
batch.items[0].position = vehicle;
addCandidate(batch.items[0], across, 1);
addCandidate(batch.items[0], away, 2);
ASSERT_TRUE(checker.pruneCandidates(batch));
ASSERT_EQ(batch.items[0].candidate_buffer.count, 1);
EXPECT_EQ(batch.items[0].candidate_buffer.candidates[0].segment.start.idx, 2);
bool clear = false;
ASSERT_TRUE(checker.checkPath(vehicle, away, clear));
EXPECT_TRUE(clear);
ASSERT_TRUE(checker.checkPath(vehicle, across, clear));
EXPECT_FALSE(clear);
}