Skip to content

feat(geofence): add segment geometry helpers - #28758

Open
JonasPerolini wants to merge 2 commits into
mainfrom
gf-lines
Open

JonasPerolini wants to merge 2 commits into
mainfrom
gf-lines

Conversation

@JonasPerolini

@JonasPerolini JonasPerolini commented Sep 18, 2026

Copy link
Copy Markdown
Contributor

The mission checker validates individual waypoints but misses geofence breaches between them. This stack of 5 PRs adds checks at mission upload to catch those breaches before flight. To test the complete feature, use the last PR in this stack: gf-jump.

Algorithm:

  1. Check that the first waypoint is inside of all GFs (validated)
  2. Check if the line between waypoints crosses any GF, if it does, we are now outside of the validated zone --> breach. Boundary contact counts as a breach to ensure that a path that exists the GF through a vertice is caught.

Note: The implementation is differemt to the GF-aware RTL planner. The RTL planner’s checks are designed to find routes around fences. They use prepared polygons in local coordinates and allow paths to touch corners or follow their edges. Mission validation uses a simpler rule: once the starting point is valid, touching any fence boundary is a breach. This lets us check polygons directly from the existing fence cache. It avoids adding another projected fence (in local coordinates) or changing the RTL planner’s boundary rules.

This PR adds segment intersection and point-to-segment distance helpers for mission geofence checks. No behavior change, only lib.

@github-actions github-actions Bot added kind:feature Request or change that adds new functionality. kind:test Adds or improves tests. scope:navigation Missions, RTL, geofence, takeoff, landing, or navigator behavior. scope:testing Unit, integration, fuzzing, or test data. labels Sep 18, 2026
@JonasPerolini
JonasPerolini added this pull request to stack #28763 September 18, 2026 15:11
@dakejahl

Copy link
Copy Markdown
Contributor

Claude review on behalf of @dakejahl

nit, src/lib/geofence/GeofenceUtilsTest.cpp:96: the intersection table has no non-degenerate, non-collinear pair where a side value is exactly zero and the answer is still false, i.e. an endpoint on the other segment's supporting line but beyond it, with overlapping bounds: {"EndpointOnLineBeyondSegment", {0, 0}, {4, 0}, {5, 0}, {3, 2}, false}. That is the configuration where the inclusive <=/>= in segmentsIntersectInclusive (geofence_utils.cpp:535) accepts the first straddle test and only the second one rejects; the zero-side rows in the table today are all touching, collinear or point cases. One more row pins it.

@JonasPerolini

Copy link
Copy Markdown
Contributor Author

Thanks for the review. Test case added here e63e3be @dakejahl

@github-actions

Copy link
Copy Markdown
Contributor

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 0 byte (0 %)]
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +84  [ = ]       0    .debug_abbrev
  +0.0%     +40  [ = ]       0    .debug_aranges
  +0.1%    +272  [ = ]       0    .debug_frame
  +0.0% +7.81Ki  [ = ]       0    .debug_info
  +0.0% +1.55Ki  [ = ]       0    .debug_line
    [NEW]      +7  [ = ]       0    [Unmapped]
    +0.0% +1.55Ki  [ = ]       0    [section .debug_line]
  +0.1% +2.52Ki  [ = ]       0    .debug_loclists
    [NEW]      +3  [ = ]       0    [Unmapped]
    +0.1% +2.52Ki  [ = ]       0    [section .debug_loclists]
  +0.1%    +358  [ = ]       0    .debug_rnglists
  +0.0%    +414  [ = ]       0    .debug_str
    [NEW]      +1  [ = ]       0    [Unmapped]
    +0.0%    +413  [ = ]       0    [section .debug_str]
  +0.0% +13.0Ki  [ = ]       0    TOTAL

px4_fmu-v6x [Total VM Diff: 8 byte (0 %)]
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%      +8  +0.0%      +8    .text
    +0.0%     +12  +0.0%     +12    [section .text]
   -30.8%      -4 -30.8%      -4    g_nullstring
  +0.0%     +84  [ = ]       0    .debug_abbrev
  +0.0%     +40  [ = ]       0    .debug_aranges
  +0.1%    +272  [ = ]       0    .debug_frame
  +0.0% +7.81Ki  [ = ]       0    .debug_info
  +0.0% +1.54Ki  [ = ]       0    .debug_line
   -20.0%      -1  [ = ]       0    [Unmapped]
    +0.0% +1.55Ki  [ = ]       0    [section .debug_line]
  +0.1% +2.53Ki  [ = ]       0    .debug_loclists
  +0.1%    +358  [ = ]       0    .debug_rnglists
  +0.0%    +450  [ = ]       0    .debug_str
  -0.1%      -8  [ = ]       0    [Unmapped]
  +0.0% +13.1Ki  +0.0%      +8    TOTAL

Updated: 2026-09-22T11:05:16

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

kind:feature Request or change that adds new functionality. kind:test Adds or improves tests. scope:navigation Missions, RTL, geofence, takeoff, landing, or navigator behavior. scope:testing Unit, integration, fuzzing, or test data.

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants