Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity
49,599 Commits 879 Branches 164 Tags
dijkstra-geometry-unittests
Commit Graph

29323 Commits

Author SHA1 Message Date
Balduin 74113b5330 refactor: remove SegSegResult enum and replace by bool 
it was only used to test against ::Proper so no functionality is lost.
Tests are adapted accordingly
 2026-05-08 10:41:09 +02:00
Balduin 0a5361dd44 geofence_utils: classify vertex hits via wedge, drop sort+sub-segment scan 
Replace the per-sub-segment midpoint scan in lineSegmentIntersectsPolygon
with a wedge classification per vertex hit: a polygon vertex on the open
segment is a real boundary crossing iff the two segment endpoints fall on
different sides of its interior wedge. The insertion sort is gone; the
remaining sample check collapses to start/midpoint/end.

Also update the segmentsIntersect tests for the SegSegResult enum
introduced in the previous commit, and add direct orient2d and
collinear-disjoint cases.
 2026-05-07 18:45:28 +02:00
Balduin 6883add606 geofence_utils: rewrite based on orient2d 2026-05-07 18:15:18 +02:00
Balduin 8ebfaa1451 add failing test back 2026-05-07 17:55:13 +02:00
Balduin 139b4af1aa tests (claude) 2026-05-07 17:21:48 +02:00
Balduin 45ba43ee1c update call sites 2026-05-07 17:21:00 +02:00
Balduin 49b7ddbe89 wip: distinguish inclusion and exclusion zones in line-polygon intersection 
inclusion zones -> inside allowed, outside not
exclusion zones -> outside allowed, inside not

TODO:
 - update call sites
 - tests
 2026-05-07 17:20:02 +02:00
Balduin 763ebb5641 touching and parallel tests 2026-05-07 17:13:07 +02:00
Balduin 1dbee24ab3 touching and parallel segment tests 2026-05-07 17:05:45 +02:00
Balduin 6a59e4c8e1 Add tests certifying touching and parallel case 2026-05-07 16:57:30 +02:00
Balduin f4e30d7e0a inequality hack 2026-05-07 16:43:04 +02:00
Balduin 9cf751e00f Introduce new tests 2026-05-07 16:21:16 +02:00
Balduin 10bc335b72 refactor(navigator/geofence): move lineSegmentIntersectsPolygon to geofence_utils 2026-05-07 16:21:12 +02:00
RomanBapst d7e749d203 integrate dijkstra lib into avoidance planner 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-05-07 10:39:46 +03:00
RomanBapst 0786191a5c switched to functional unit test to fix compilation error 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-05-07 10:39:17 +03:00
RomanBapst e5b4e9c52a support symmetric cost in order to save memory 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-05-05 10:31:06 +03:00
RomanBapst cce490e6f5 geofence_avoidance_planner: simplify shortest path logic even further 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-05-05 10:31:06 +03:00
RomanBapst e414e15421 geofence_avoidance_planner: save on stack space by simplifying calculations 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-05-05 10:31:06 +03:00
RomanBapst 12b3acd5b4 make search even more efficient by combining two loops 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-30 14:03:07 +03:00
Balduin a3cec7a51d feat(lib): dijkstra path planner library 
- Takes in buffer of all edge costs
 - fills the best_cost buffer for all reachable nodes (unreachable =
   infinite cost)

Usage:
 - Precompute edge costs (polygon expansion, visibility all separate
   from planning algorithm)
 - solve
 - To find best cost from any position: do one more visibility check
   from that position to all reachable nodes, return the one with
   smallest total distance (current position to node to goal)
 - Once waypoint reached, keep looking up next_node until goal reached

Todo:
 - clean up API (raw float buffer really the best? heap allocated array?)
 - make optimised version for symmetric cost? < half the data
 2026-04-29 16:07:20 +02:00
RomanBapst c3a152fbf2 rtl_direct: send out message to inform that rtl is avoiding geofence 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst 1a7b579342 fix formatting 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst 40bf5b33a2 renamed MOVE_TO_INTERMEDIATE_POINT to AVOID_GEOFENCE 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst 6871caa6f9 invalidate start and destination when resetting the vertices 
- the reference for the projection may change which will cause start and
destination positions to be invalid
- the position of the destination in the graph node may not be correct anymore

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst a77093f0e7 rtl_direct: only update destination if it's invalid or the position actually 
changed

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst cace5b88dd take rtl path into account for remaining rtl time estimation 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-29 14:28:46 +03:00
RomanBapst cac90d75bf geofence utils test: simplify and extend 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-28 14:11:20 +03:00
RomanBapst 2168374b58 geofence utils unit tests: improve test for SymmetricPairIndex 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-28 14:06:57 +03:00
Balduin 3efb9ad3b4 perf(navigator): instrument geofence avoidance planner with perf counters 
Adds five PC_ELAPSED counters around the components expected to dominate
RTL path planning cost so they show up in `perf` output:

  rtl_planner: setup            - update_vertices() (fence-load entry)
  rtl_planner: setup distances  - O(n^2) line-fence checks
  rtl_planner: update start     - O(n) line checks on RTL entry
  rtl_planner: update destination
  rtl_planner: plan path        - Dijkstra in planPath()

Skips the no-work early-return paths (unhealthy state, invalid input) via
perf_cancel so reported numbers reflect actual work done.
 2026-04-28 13:48:18 +03:00
Balduin e5b740cc82 fix(navigator): keep result as member and return by reference to use less stack 
Declaring a local PlannedPath needs an additional 1608B of stack which
we don't typically have.

Making it a member of the planner means it is allocated on the heap.
 2026-04-28 13:48:18 +03:00
Balduin 53b73bb473 fix(geofence_avoidance_planner): fix double-promotion clang-tidy warning 
Cast NAN to double when constructing Vector2d to avoid
-Wdouble-promotion.
 2026-04-27 11:35:03 +02:00
RomanBapst e69e4432ec rtl_direct: reversed changes in rtl time estimate regarding geofence 
avoidance for now, needs more thought

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-27 07:01:23 +03:00
RomanBapst 970d52f502 geofence: removed duplicate call to update_vertices 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-27 06:44:42 +03:00
RomanBapst b8a0b77e22 shorter comment 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-27 06:37:09 +03:00
RomanBapst 21ae5057fd added reference for symmetricPairIndex 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-27 06:33:52 +03:00
RomanBapst 8ec3e2b890 include start into planned path to avoid corner cases when rtl is engaged 
outside of geofence

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-24 17:34:19 +03:00
RomanBapst 6108efbe75 if RTL direct is activated during a geofence violation, we use the last 
recorded valid position as start position for the path planning

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-24 17:32:07 +03:00
RomanBapst ccd3608e50 refactor: instead of computing the distance between the nodes at each iteration, 
precute them in advance. Separate vertices from start and destination
calculations.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-24 14:36:33 +03:00
RomanBapst 5a0b9a44f2 more comments 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-23 11:40:16 +03:00
RomanBapst f401181439 addressed some review comments, fixed bugs 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-23 11:31:08 +03:00
RomanBapst 31792d773d make geofence util functions more readable, added references and more 
unti tests

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-23 11:30:26 +03:00
RomanBapst 22d5617f10 geofence_utils: use shoelace formula to calculate proper inward vector 
at vertice location

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-22 14:43:13 +03:00
RomanBapst e40a491dec some cosmetics 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-21 15:50:08 +03:00
RomanBapst 58a62340d9 fixed radius enlargement bug 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-21 15:49:58 +03:00
RomanBapst 3e1f18e8de added support for circular fences 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-21 15:17:29 +03:00
RomanBapst 05c39e381c geofence_utils: remove unused function 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-20 16:21:16 +03:00
RomanBapst 8c49c202f0 fixed conversion 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-20 16:07:51 +03:00
RomanBapst 66c704684f removed unnecessary include 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-20 14:42:52 +03:00
RomanBapst 1d6d641b2b integrated geofence aware path planner into rtl direct navigation mode 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-20 11:53:45 +03:00
RomanBapst c5c83da12f added a geofence avoidance planner, which returns a path from start to destination 
avoiding geofence breaches

Signed-off-by: RomanBapst <bapstroman@gmail.com>
 2026-04-20 11:48:34 +03:00