UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-25 13:57:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(navigator): Geofence Aware RTL (#27145)

Browse Source 
Plan RTL paths that route around geofence boundaries — both inclusion
and exclusion zones — instead of flying straight through them and
breaching. The planner builds a visibility graph over the margin-inflated
geofence polygons and circles and runs Dijkstra to find the shortest
legal return path, falling back to a straight line to the destination
when no valid path exists.

Highlights:
- New reusable libraries: src/lib/dijkstra (generic shortest path) and
  src/lib/geofence (fixed-point geometry, polygon inflation, bitangent
  visibility); the RTL planner lives in navigator/RTLPlanner.
- Visibility graph keeps only bitangent edges and skips edges that poke
  into a forbidden region, greatly reducing edge-cost computation.
- Corner splitting is limited to sharp convex corners.
- Geometry validation: reject self-intersecting polygons and vertices
  outside the fixed-point range; centimeter fixed-point scaling keeps
  orientation tests exact and avoids drift at large distances.
- Failure handling: a Status enum replaces silent bool returns, and
  failures (unbuildable fence, planner capacity overflow, dataman load
  errors, NaN waypoints, destinations that breach the fence) are
  surfaced to the operator via MAVLink warnings/criticals; the planner
  falls back to a straight-line RTL.
- Destination updates are centralized in RTL::setRtlTypeAndDestination;
  the path is replanned only when the destination or geometry changes.
- VTOLs always use the fixed-wing margin (FW loiter radius).
- kMaxNodes is exposed as a Kconfig option (default 100); the feature is
  disabled on flash-constrained boards (FMUv4 and older, various F4/F7).
- Tests: unit tests for the Dijkstra lib, geofence geometry utils, and
  the avoidance planner, plus a MAVSDK SITL test flying an RTL through a
  geofence.
- Documentation: new "Geofence Awareness" section in the RTL docs.

Co-authored-by: Balduin <balduin@auterion.com>
This commit is contained in:
Roman Bapst
2026-07-09 15:08:27 +03:00
committed by GitHub
parent d448e3f5b0
commit 40e61d2429
47 changed files with 4147 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
boards/ark/fpv/default.px4board
View File

@@ -62,6 +62,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE=n
CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y

1
boards/corvon/743v1/default.px4board
View File

@@ -58,6 +58,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE=n
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y

9
boards/flywoo/gn-f405/default.px4board
View File

@@ -18,6 +18,15 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set
# CONFIG_EKF2_EXTERNAL_VISION is not set
# CONFIG_EKF2_GNSS_YAW is not set
# CONFIG_EKF2_OPTICAL_FLOW is not set
# CONFIG_EKF2_RANGE_FINDER is not set
# CONFIG_EKF2_SIDESLIP is not set
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LOAD_MON=y

1
boards/holybro/kakutef7/default.px4board
View File

@@ -43,6 +43,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
# CONFIG_NAVIGATOR_ADSB is not set
# CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

1
boards/px4/fmu-v2/default.px4board
View File

@@ -44,6 +44,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
# CONFIG_NAVIGATOR_ADSB is not set
# CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

3
boards/px4/fmu-v3/default.px4board
View File

@@ -49,8 +49,8 @@ CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_MODE_MANAGER=y
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
CONFIG_MODULES_FW_MODE_MANAGER=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -68,6 +68,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
# CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y

3
boards/px4/fmu-v4/default.px4board
View File

@@ -50,8 +50,8 @@ CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_MODE_MANAGER=y
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
CONFIG_MODULES_FW_MODE_MANAGER=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -69,6 +69,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
# CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y

BIN
docs/assets/flight_modes/rtl_geofence_exclusion.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 KiB

BIN
docs/assets/flight_modes/rtl_geofence_inclusion.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 7.4 KiB

78
docs/en/flight_modes/return.md
View File

@@ -46,10 +46,10 @@ PX4 provides four alternative approaches for finding an unobstructed path to a s
At high level these are:
- [Home/rally point return](#home_return) (`RTL_TYPE=0`): Ascend to safe altitude and return via a direct path to the closest rally point or home location.
- [Mission landing/rally point return](#mission-landing-rally-point-return-type-rtl-type-1) (`RTL_TYPE=1`): Ascend to a safe altitude, fly direct to the closest destination _other than home_: rally point or start of mission landing.
- [Home/rally point return](#rtl_type_0) (`RTL_TYPE=0`): Ascend to safe altitude and return via a direct path to the closest rally point or home location.
- [Mission landing/rally point return](#rtl_type_1) (`RTL_TYPE=1`): Ascend to a safe altitude, fly direct to the closest destination _other than home_: rally point or start of mission landing.
If no mission landing or rally points are defined, return home via direct path.
- [Mission path return](#mission-path-return-type-rtl-type-2) (`RTL_TYPE=2`): Use mission path and fast-continue to mission landing (if defined).
- [Mission path return](#rtl_type_2) (`RTL_TYPE=2`): Use mission path and fast-continue to mission landing (if defined).
If no mission _landing_ defined, fast-reverse mission to home.
If no _mission_ defined, return direct to home (rally points are ignored).
- [Closest safe destination return](#closest-safe-destination-return-type-rtl-type-3) (`RTL_TYPE=3`): Ascend to a safe altitude and return via direct path to closest destination: home, start of mission landing pattern, or rally point.
@@ -57,16 +57,14 @@ At high level these are:
More detailed explanations for each of the types are provided in the following sections.
<a id="home_return"></a>
### Home/Rally Point Return Type (RTL_TYPE=0)
### Home/Rally Point Return Type (RTL_TYPE=0) {#rtl_type_0}
This is the default return type for a [multicopter](../flight_modes_mc/return.md) (see topic for more information).
In this return type the vehicle:
- Ascends to a safe [minimum return altitude](#minimum-return-altitude) (above any expected obstacles).
- Flies via direct path to the home position or a rally point (whichever is closest)
- Flies to the home position or a rally point (whichever is closest), preferring a [geofence-aware](#geofence_awareness) horizontal path over a direct path where possible.
- On [arrival](#loiter-landing-at-destination) descends to "descent altitude" and waits for a configurable time.
This time may be used to deploy landing gear.
- Lands or waits (this depends on landing parameters),
@@ -77,7 +75,7 @@ In this return type the vehicle:
If no rally points are defined, this is the same as a _Return to Launch_ (RTL)/_Return to Home_ (RTH).
:::
### Mission Landing/Rally Point Return Type (RTL_TYPE=1)
### Mission Landing/Rally Point Return Type (RTL_TYPE=1) {#rtl_type_1}
This is the default return type for a [fixed-wing](../flight_modes_fw/return.md) or [VTOL](../flight_modes_vtol/return.md) vehicle (see topics for more information).
@@ -85,7 +83,7 @@ In this return type the vehicle:
- Ascends to a safe [minimum return altitude](#minimum-return-altitude) (above any expected obstacles) if needed.
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
- Flies via direct constant-altitude path to a rally point or the start of a [mission landing pattern](#mission-landing-pattern) (whichever is closest).
- Flies at constant-altitude to a rally point or the start of a [mission landing pattern](#mission-landing-pattern) (whichever is closest), preferring a [geofence-aware](#geofence_awareness) horizontal path over a direct path where possible.
If no mission landing or rally points are defined the vehicle instead returns home via a direct path.
- If the destination is a mission landing pattern it will follow the pattern to land.
- If the destination is a rally point or home it will [land or wait](#loiter-landing-at-destination) at descent altitude (depending on landing parameters).
@@ -100,7 +98,7 @@ In this return type the vehicle:
Fixed wing vehicles commonly also set [MIS_TKO_LAND_REQ](#MIS_TKO_LAND_REQ) to _require_ a mission landing pattern.
:::
### Mission Path Return Type (RTL_TYPE=2)
### Mission Path Return Type (RTL_TYPE=2) {#rtl_type_2}
This return type uses the mission (if defined) to provide a safe return _path_, and the [mission landing pattern](#mission-landing-pattern) (if defined) to provide landing behaviour.
If there is a mission but no mission landing pattern, the mission is flown _in reverse_.
@@ -110,6 +108,10 @@ Rally points, if any, are ignored.
The behaviour is fairly complex because it depends on the flight mode, and whether a mission and mission landing are defined.
:::
::: warning
This return type does have [geofence awareness](#geofence_awareness) (at any stage).
:::
Mission _with_ landing pattern:
- **Mission mode:**
@@ -145,7 +147,7 @@ For `RTL_TYPE=4`, PX4 currently chooses between continuing to a mission landing
This is only an approximation of the flown path length, because the number if mission items is not indicative of the distance remaining and non-position items are also counted.
:::
### Closest Safe Destination Return Type (RTL_TYPE=3)
### Closest Safe Destination Return Type (RTL_TYPE=3) {#rtl_type_3}
In this return type the vehicle:
@@ -156,12 +158,62 @@ In this return type the vehicle:
By default an MC or VTOL in MC mode will land, and a fixed-wing vehicle circles at the descent altitude.
A VTOL in FW mode aligns its heading to the destination point, transitions to MC mode, and then lands.
## Geofence Awareness {#geofence_awareness}
For most of the return types (including the default home/rally point return type) the return path is chosen to avoid breaching any geofence.
Planning is purely horizontal: the altitude profile is unaffected, and only the lateral path is adjusted to avoid the fence.
If no geofence is set, the vehicle flies a direct path to the destination.
While the return mode is inactive, the autopilot constantly recalculates a [shortest horizontal return path](#shortest-path-calculation) that does not enter any exclusion zones and does not exit any inclusion zones.
If the return mode is triggered while the vehicle is violating any geofence, then the vehicle will first fly directly to the most recent recorded location at which it was not violating the geofence.
If no such point exists, or if the autopilot fails to plan a feasible path (e.g. the destination is located in an exclusion zone), then the vehicle falls back to flying directly to the destination.
::: info
The estimated time for return is based on the current shortest horizontal path to the destination and may change if the geofence is updated.
:::
::: warning
Geofence awareness currently supports a maximum of 99 polygon vertices in total (circles count as 8 vertices each).
If this limit is exceeded, the autopilot falls back to a direct path as described above.
:::
::: warning
There is no absolute guarantee that the vehicle will not breach a geofence on the return path.
Things like path tracking error, wind and other disturbances may cause temporary violation of the geofence.
It is therefore very important to consider this possibility and especially to review the geofence breach action (e.g. [GF_ACTION](../advanced_config/parameter_reference.md#GF_ACTION)).
:::
### RTL-types with Geofence-Awareness
The following table shows which return types currently support geofence awareness:
| Return Type (RTL_TYPE) | Geofence Awareness |
|------------------------|--------------------|
| 0 (home/rally point) | Yes |
| 1 (mission landing) | Yes |
| 2 (mission path) | No |
| 3 (closest safe dest.) | Yes |
| 4 (mission path) | No |
| 5 (rally point only) | Yes |
### Shortest-Path Calculation
For the construction of the shortest path between the starting location and the destination, the autopilot uses the vertices of the geofence polygons as intermediate waypoints.
In order to avoid the path being too close to the polygon boundaries, the autopilot constructs a corresponding set of polygons, which are either enlarged (for exclusion zones) or shrunk (for inclusion zones).
The margin in both images below is 10m.
The figures below show an exclusion zone and an inclusion zone.
![Exclusion Zone](../../assets/flight_modes/rtl_geofence_exclusion.jpg)
![Inclusion Zone](../../assets/flight_modes/rtl_geofence_inclusion.jpg)
## Minimum Return Altitude
For most [return types](#return_types) a vehicle will ascend to a _minimum safe altitude_ before returning (unless already above that altitude), in order to avoid any obstacles between it and the destination.
::: info
The exception is when executing a [mission path return](#mission-path-return-type-rtl-type-2) from _within a mission_.
The exception is when executing a [mission path return](#rtl_type_2) from _within a mission_.
In this case the vehicle follows mission waypoints, which we assume are planned to avoid any obstacles.
:::
@@ -206,7 +258,7 @@ A mission landing pattern is a landing pattern defined as part of a mission plan
This consists of a [MAV_CMD_DO_LAND_START](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START), one or more position waypoints, and a [MAV_CMD_NAV_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LAND) (or [MAV_CMD_NAV_VTOL_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_LAND) for a VTOL Vehicle).
Landing patterns defined in missions are the safest way to automatically land a _fixed-wing_ vehicle on PX4.
For this reason fixed-wing vehicles are configured to use [Mission landing/really point return](#mission-landing-rally-point-return-type-rtl-type-1) by default.
For this reason fixed-wing vehicles are configured to use [Mission landing/really point return](#rtl_type_1) by default.
## Parameters

7
docs/en/flight_modes_fw/return.md
View File

@@ -4,10 +4,10 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it can land.
Fixed-wing vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#mission-landing-rally-point-return-type-rtl-type-1) return type by default, and are expected to always have a mission with a landing pattern.
Fixed-wing vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#rtl_type_1) return type by default, and are expected to always have a mission with a landing pattern.
With this configuration, return mode causes the vehicle to ascend to a minimum safe altitude above obstructions (if needed), fly to the start of the landing pattern defined in the mission plan, and then follow it to land.
Fixed-wing supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including home/rally point return, mission path and closest safe destination.
Fixed-wing supports the [other PX4 return types](../flight_modes/return.md#return_types), including home/rally point return, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -33,7 +33,8 @@ In this return type the vehicle:
- Ascends to a safe minimum return altitude defined by [RTL_RETURN_ALT](#RTL_RETURN_ALT) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
Note that return altitude cannot be configured using the "cone" parameter in fixed-wing vehicles.
- Flies via direct constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
- Flies via a constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- If the destination is a _mission landing pattern_ it will follow the pattern to land.
- If the destination is a rally point or home it will descend to the descent altitude, and then loiter or land (depending on landing parameters).

13
docs/en/flight_modes_mc/return.md
View File

@@ -4,11 +4,11 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it can land.
Multicopters use a [home/rally point return type](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0) by default.
In this return type vehicles ascend to a safe altitude above obstructions if needed, fly directly to the closest safe landing point (a rally point or the home position), descend to the "descent altitude", wait briefly, and then land.
Multicopters use a [home/rally point return type](../flight_modes/return.md#home_return) by default.
In this return type vehicles ascend to a safe altitude above obstructions if needed, fly to the closest safe landing point (a rally point or the home position) via the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness), descend to the "descent altitude", wait briefly, and then land.
The return altitude, descent altitude, and landing delay are normally set to conservative "safe" values, but can be changed if needed.
Multicopter supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including mission landing, mission path and closest safe destination.
Multicopter supports the [other PX4 return types](../flight_modes/return.md#return_types), including mission landing, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -28,12 +28,13 @@ The default type is recommended.
## Technical Summary
Multicopters use the [home/rally point return type](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0) by default ([RTL_TYPE=0](../advanced_config/parameter_reference.md#RTL_TYPE)).
Multicopters use the [home/rally point return type](../flight_modes/return.md#home_return) by default. ([RTL_TYPE=0](../advanced_config/parameter_reference.md#RTL_TYPE)).
For this return type the copter:
- Ascends to the [minimum return altitude](#minimum-return-altitude) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
- Flies via a direct constant-altitude path to the safe landing point, which will be the nearest of any rally points and the home position.
- Flies via a constant-altitude path to the safe landing point, which will be the nearest of any rally points and the home position.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- On arrival at its destination, it rapidly descends to the "descent altitude" ([RTL_DESCEND_ALT](#RTL_DESCEND_ALT)).
- It waits for a configurable time ([RTL_LAND_DELAY](#RTL_LAND_DELAY)), which may be used for deploying landing gear.
- Then lands.
@@ -52,7 +53,7 @@ The cone affects the minimum return altitude if return mode is triggered within
Inside the cone, the vehicle returns at an altitude calculated from the cone geometry, up to `RTL_RETURN_ALT`.
After reaching the destination, it descends to `RTL_DESCEND_ALT` (if above that altitude) before landing or loitering.
For more information on this return type see [Home/Rally Point Return Type (RTL_TYPE=0)](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0)
For more information on this return type see [Home/Rally Point Return Type (RTL_TYPE=0)](../flight_modes/return.md#home_return)
## Parameters

9
docs/en/flight_modes_vtol/return.md
View File

@@ -4,7 +4,7 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it may either wait (hover or circle) or land.
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#mission-landing-rally-point-return-type-rtl-type-1) return type by default.
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#rtl_type_1) return type by default.
In this return type a vehicle ascends to a minimum safe altitude above obstructions (if needed), and then flies directly to a rally point or the start of a mission landing point (whichever is nearest), or the home position if neither rally points or mission landing pattern is defined.
If the destination is a mission landing pattern, the vehicle will then follow the pattern to land.
If the destination is a rally point or the home location, the vehicle will fly to that destination and land.
@@ -12,7 +12,7 @@ If the destination is a rally point or the home location, the vehicle will fly t
The vehicle will return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
Generally it will follow the same return mode behaviour of the corresponding vehicle type, but will always transition to MC mode (if needed) before landing.
VTOL supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including home/rally point return, mission path and closest safe destination.
VTOL supports the [other PX4 return types](../flight_modes/return.md#return_types), including home/rally point return, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -32,7 +32,7 @@ The default type is recommended.
## Technical Summary
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#mission-landing-rally-point-return-type-rtl-type-1) return type by default, and return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#rtl_type_1) return type by default, and return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
### Fixed-wing Mode (FW) Return
@@ -41,7 +41,8 @@ If returning as a fixed-wing, the vehicle:
- Ascends to a safe minimum return altitude defined by [RTL_RETURN_ALT](#RTL_RETURN_ALT) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
<!-- Note that return altitude cannot be configured using the "cone" parameter in fixed-wing vehicles. -->
- Flies via direct constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
- Flies via a constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- If the destination is a mission landing pattern it will follow the pattern to land.
A mission landing pattern for a VTOL vehicle consists of a [MAV_CMD_DO_LAND_START](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START), one or more position waypoints, and a [MAV_CMD_NAV_VTOL_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_LAND).

2
src/lib/CMakeLists.txt
View File

@@ -48,10 +48,12 @@ add_subdirectory(conversion EXCLUDE_FROM_ALL)
add_subdirectory(crc EXCLUDE_FROM_ALL)
add_subdirectory(crypto EXCLUDE_FROM_ALL)
add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
add_subdirectory(dijkstra EXCLUDE_FROM_ALL)
add_subdirectory(drivers EXCLUDE_FROM_ALL)
add_subdirectory(failure_injection EXCLUDE_FROM_ALL)
add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
add_subdirectory(geo EXCLUDE_FROM_ALL)
add_subdirectory(geofence EXCLUDE_FROM_ALL)
add_subdirectory(gnss EXCLUDE_FROM_ALL)
add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
add_subdirectory(hysteresis EXCLUDE_FROM_ALL)

40
src/lib/dijkstra/CMakeLists.txt Normal file
View File

@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(dijkstra
dijkstra.cpp
)
target_include_directories(dijkstra PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC DijkstraTest.cpp LINKLIBS dijkstra)

323
src/lib/dijkstra/DijkstraTest.cpp Normal file
View File

@@ -0,0 +1,323 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <math.h>
#include "dijkstra.h"
namespace
{
// Helper: assemble cost matrix entries into a row-major buffer. Use INFINITY for
// missing edges; explicit list of finite (i, j, cost) triples is concise in tests.
struct Edge { int from, to; float cost; };
template <int N>
void makeMatrix(float (&m)[N * N], std::initializer_list<Edge> edges)
{
for (int i = 0; i < N * N; ++i) {
m[i] = INFINITY;
}
for (const auto &e : edges) {
m[e.from * N + e.to] = e.cost;
}
}
} // namespace
TEST(DijkstraTest, SingleNodeAtGoal)
{
constexpr int N = 1;
float cost[N * N] = { INFINITY };
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 0.f);
EXPECT_EQ(next[0], -1);
}
TEST(DijkstraTest, AsymmetricLineGraphForward)
{
// 0 -> 1 -> 2, going forward is cheap, going backward is expensive.
constexpr int N = 3;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 2, 1.f},
{1, 0, 100.f},
{2, 1, 100.f},
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 2.f); // 0 -> 1 -> 2 costs 1 + 1
EXPECT_FLOAT_EQ(best[1], 1.f); // 1 -> 2
EXPECT_FLOAT_EQ(best[2], 0.f);
EXPECT_EQ(next[0], 1);
EXPECT_EQ(next[1], 2);
EXPECT_EQ(next[2], -1);
}
TEST(DijkstraTest, AsymmetricLineGraphReverse)
{
// Same matrix as above; with goal = 0, paths must use the expensive back-edges,
// confirming the solver respects edge direction (uses cost[i][j] = i -> j, not j -> i).
constexpr int N = 3;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 2, 1.f},
{1, 0, 100.f},
{2, 1, 100.f},
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 0.f);
EXPECT_FLOAT_EQ(best[1], 100.f);
EXPECT_FLOAT_EQ(best[2], 200.f);
EXPECT_EQ(next[1], 0);
EXPECT_EQ(next[2], 1);
EXPECT_EQ(next[0], -1);
}
TEST(DijkstraTest, TriangleWithBlockedEdge)
{
// Direct edge 0 -> 2 is cheap; remove it and the solver must route via 1.
constexpr int N = 3;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 2, 1.f},
{0, 2, 0.5f},
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 0.5f);
EXPECT_EQ(next[0], 2);
// Block the direct edge and re-solve; path must detour through 1.
cost[0 * N + 2] = INFINITY;
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 2.f);
EXPECT_EQ(next[0], 1);
}
TEST(DijkstraTest, GoalUnreachableNoInfiniteLoop)
{
// Two disconnected components: {0, 1} and {2}. Goal = 2, so 0 and 1 must end
// up unreachable and the algorithm must not loop.
constexpr int N = 3;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 0, 1.f},
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[2], 0.f);
EXPECT_FALSE(best[0] < dijkstra::kUnreachable);
EXPECT_FALSE(best[1] < dijkstra::kUnreachable);
EXPECT_EQ(next[0], -1);
EXPECT_EQ(next[1], -1);
}
TEST(DijkstraTest, NaNTreatedAsMissingEdge)
{
constexpr int N = 3;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 2, 1.f},
{0, 2, NAN}, // NaN must be treated like INFINITY, not as a 0 / negative edge
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 2.f);
EXPECT_EQ(next[0], 1);
}
TEST(DijkstraTest, RejectsInvalidInputs)
{
constexpr int N = 2;
float cost[N * N] = { INFINITY, 1.f, 1.f, INFINITY };
float best[N];
int next[N];
bool vis[N];
EXPECT_FALSE(dijkstra::solveBackward(0, 0, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, -1, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, N, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, 0, nullptr, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, 0, cost, false, nullptr, next, vis));
}
TEST(DijkstraTest, SymmetricPackedMatchesAsymmetric)
{
// Same undirected weighted graph expressed two ways: full N*N matrix with both
// halves filled, and packed upper triangle. Results must match for both layouts.
constexpr int N = 5;
// Edges (a, b, w) with a < b.
struct UndirEdge { int a, b; float w; };
const UndirEdge edges[] = {
{0, 1, 2.f},
{0, 2, 4.f},
{1, 2, 1.f},
{1, 3, 7.f},
{2, 3, 3.f},
{3, 4, 2.f},
};
float full[N * N];
for (int i = 0; i < N * N; ++i) { full[i] = INFINITY; }
constexpr int kPacked = N * (N - 1) / 2;
float packed[kPacked];
for (int i = 0; i < kPacked; ++i) { packed[i] = INFINITY; }
for (const auto &e : edges) {
full[e.a * N + e.b] = e.w;
full[e.b * N + e.a] = e.w;
packed[e.a * (2 * N - e.a - 1) / 2 + (e.b - e.a - 1)] = e.w;
}
float best_full[N], best_pack[N];
int next_full[N], next_pack[N];
bool vis[N];
for (int goal = 0; goal < N; ++goal) {
ASSERT_TRUE(dijkstra::solveBackward(N, goal, full, false, best_full, next_full, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, goal, packed, true, best_pack, next_pack, vis));
for (int i = 0; i < N; ++i) {
EXPECT_FLOAT_EQ(best_full[i], best_pack[i]) << "goal=" << goal << " i=" << i;
EXPECT_EQ(next_full[i], next_pack[i]) << "goal=" << goal << " i=" << i;
}
}
}
TEST(DijkstraTest, ForwardWalkReachesGoal)
{
// 5-node graph: 0 -> 1 -> 2 -> 3 -> 4, plus a long detour 0 -> 4 directly.
// Solving once with goal = 4, then for any start we should reach 4 by following
// next_node without re-solving.
constexpr int N = 5;
float cost[N * N];
makeMatrix<N>(cost, {
{0, 1, 1.f},
{1, 2, 1.f},
{2, 3, 1.f},
{3, 4, 1.f},
{0, 4, 100.f}, // direct but expensive
});
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 4, cost, false, best, next, vis));
for (int start = 0; start < N - 1; ++start) {
int u = start;
int hops = 0;
while (u != 4 && hops < N) {
ASSERT_GE(next[u], 0) << "stuck at " << u << " from start " << start;
u = next[u];
++hops;
}
EXPECT_EQ(u, 4) << "did not reach goal from " << start;
}
}
TEST(DijkstraTest, SymmetricPairIndex)
{
// create a NxN matrix and fill with random data. Then pack the upper triangle into a 1D array.
// Then loop through the upper triangle and read the values from the array using symmetricPairIndex
// and verify that the values match.
constexpr size_t N = 21;
constexpr size_t kPairs = N * (N - 1) / 2;
float matrix[N][N];
float packed[kPairs];
std::srand(42);
size_t idx = 0;
for (size_t i = 0; i < N; ++i) {
for (size_t j = i + 1; j < N; ++j) {
matrix[i][j] = static_cast<float>(std::rand()) / static_cast<float>(RAND_MAX);
matrix[j][i] = matrix[i][j]; // symmetric matrix
packed[idx++] = matrix[i][j];
}
}
size_t counter = 0;
for (size_t i = 0; i < N; ++i) {
for (size_t j = i + 1; j < N; ++j) {
counter++;
EXPECT_FLOAT_EQ(matrix[i][j], packed[dijkstra::symmetricPairIndex(i, j, N)]);
EXPECT_FLOAT_EQ(matrix[i][j], packed[dijkstra::symmetricPairIndex(j, i, N)]);
}
}
// verify that we have the expected number of pairs
EXPECT_EQ(counter, kPairs);
}

114
src/lib/dijkstra/dijkstra.cpp Normal file
View File

@@ -0,0 +1,114 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "dijkstra.h"
namespace dijkstra
{
bool solveBackward(const int num_nodes, const int goal, const float *cost, const bool symmetric,
float *best_cost, int *next_node, bool *visited)
{
if (num_nodes <= 0 || goal < 0 || goal >= num_nodes
|| cost == nullptr || best_cost == nullptr || next_node == nullptr || visited == nullptr) {
return false;
}
for (int i = 0; i < num_nodes; ++i) {
best_cost[i] = kUnreachable;
next_node[i] = -1;
visited[i] = false;
}
best_cost[goal] = 0.f;
visited[goal] = true;
// O(N^2) Dijkstra on the reverse graph. Each iteration relaxes incoming edges
// v -> u using cost(v, u) and, in the same sweep over v, tracks the smallest
// best_cost among still-unvisited nodes — that argmin is the u for the next
// iteration. The very first u is `goal` (the only node with best_cost == 0).
int u = goal;
float u_cost = 0.f;
for (int iter = 0; iter < num_nodes - 1; ++iter) {
int next_u = -1;
float next_cost = kUnreachable;
for (int v = 0; v < num_nodes; ++v) {
if (visited[v]) {
continue;
}
// Edge v -> u. For asymmetric layout, look up the (v, u) entry directly.
// For symmetric packed upper-triangular: index by the unordered pair {v, u}.
float edge;
if (symmetric) {
edge = cost[symmetricPairIndex(v, u, num_nodes)];
} else {
edge = cost[v * num_nodes + u];
}
// Treat +INFINITY and NaN as missing edges. `edge < kUnreachable` is false for
// both because NaN is unordered and INFINITY < INFINITY is false.
if (edge < kUnreachable) {
const float candidate = u_cost + edge;
if (candidate < best_cost[v]) {
best_cost[v] = candidate;
next_node[v] = u;
}
}
// Same sweep: track argmin of best_cost over unvisited nodes for the next iter.
if (best_cost[v] < next_cost) {
next_cost = best_cost[v];
next_u = v;
}
}
if (next_u < 0) {
// No unvisited node is reachable; remaining nodes stay at kUnreachable.
break;
}
visited[next_u] = true;
u = next_u;
u_cost = next_cost;
}
return true;
}
} // namespace dijkstra

123
src/lib/dijkstra/dijkstra.h Normal file
View File

@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file dijkstra.h
*
* Backward single-source Dijkstra over a precomputed asymmetric cost matrix.
*
* `solveBackward` computes the shortest path from every node to a fixed `goal`
* and returns, for each node `i`, the next node to step toward on its shortest
* path to the goal. The intended usage is:
*
* 1. Whenever the graph (cost matrix) or the goal changes, call solveBackward
* once to fill the next-hop table.
* 2. As long as both stay fixed, querying the path from any new start `s` is
* a constant-time table lookup followed by a forward walk:
* u = s; while (u != goal) { emit(u); u = next_node[u]; }
*
* Two cost-matrix layouts are supported, selected by the `symmetric` flag:
*
* - Asymmetric (default, symmetric = false): full N*N row-major matrix.
* `cost[i * num_nodes + j]` is the cost of the directed edge i -> j.
*
* - Symmetric (symmetric = true): packed upper triangle, no diagonal.
* Buffer length is N*(N-1)/2 floats and indexed according to symmetricPairIndex.
* Self-loops are not stored; Dijkstra ignores them anyway.
*
* Entries equal to +INFINITY or NaN are treated as missing edges. Negative
* costs are not supported (Dijkstra assumes non-negative edge weights).
*/
#pragma once
#include <math.h>
#include <stddef.h>
namespace dijkstra
{
/** Sentinel cost for unreachable nodes / missing edges. */
static constexpr float kUnreachable = INFINITY;
/**
* Map the strict upper triangle (without the diagonal) of an N x N symmetric
* matrix into a flat array. The cost of the undirected edge between i and j is
* stored once, at this index.
*
* Required array length is num_nodes * (num_nodes - 1) / 2.
*
* https://stackoverflow.com/questions/27086195/linear-index-upper-triangular-matrix
*
* @param i first node index, in [0, num_nodes)
* @param j second node index, in [0, num_nodes), must differ from i
* @param num_nodes total number of nodes
* @return flat array index in [0, num_nodes * (num_nodes - 1) / 2)
*/
inline size_t symmetricPairIndex(size_t i, size_t j, const size_t num_nodes)
{
if (i > j) {
const size_t tmp = i;
i = j;
j = tmp;
}
return i * (2 * num_nodes - i - 1) / 2 + (j - i - 1);
}
/**
* Compute backward shortest paths from every node to `goal`.
*
* @param num_nodes number of nodes.
* @param goal target node index in [0, num_nodes).
* @param cost cost buffer; layout depends on `symmetric` (see above).
* Missing edges are encoded as +INFINITY or NaN.
* @param symmetric if true, `cost` is the packed upper triangle of a symmetric
* matrix (length N*(N-1)/2). If false, `cost` is the full
* N*N row-major matrix and edges may be asymmetric.
* @param best_cost out, length num_nodes: best_cost[i] = shortest cost from i
* to goal, or kUnreachable. best_cost[goal] = 0.
* @param next_node out, length num_nodes: next_node[i] = the node to step to
* from i on the shortest path to goal, or -1 if i == goal or
* i has no path to goal.
* @param visited scratch buffer, length num_nodes; contents on return are
* not meaningful.
*
* @return true if inputs are valid (non-null, num_nodes > 0, goal in range);
* false otherwise. A `true` return does not imply that goal is
* reachable from any particular node — check best_cost[i] for that.
*/
bool solveBackward(const int num_nodes, const int goal, const float *cost, const bool symmetric,
float *best_cost, int *next_node, bool *visited);
} // namespace dijkstra

41
src/lib/geofence/CMakeLists.txt Normal file
View File

@@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(geofence_utils
geofence_utils.cpp
)
target_link_libraries(geofence_utils PRIVATE geo)
target_include_directories(geofence_utils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC GeofenceUtilsTest.cpp LINKLIBS geofence_utils)

353
src/lib/geofence/GeofenceUtilsTest.cpp Normal file
View File

@@ -0,0 +1,353 @@
/****************************************************************************
*
* Copyright (C) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "geofence_utils.h"
using namespace matrix;
using SS = geofence_utils::SegSegResult;
using AddResult = geofence_utils::PlannerPolygons::AddResult;
// Low level primitives operating on fixed-point int32 coordinates.
TEST(GeofenceUtilsTest, Orient2d)
{
// CCW turn -> +1, CW turn -> -1, collinear -> 0 (in front of, behind, between).
EXPECT_EQ(1, geofence_utils::orient2d(0, 0, 1, 0, 0, 1));
EXPECT_EQ(-1, geofence_utils::orient2d(0, 0, 1, 0, 0, -1));
EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 1, 1));
EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 3, 3));
}
TEST(GeofenceUtilsTest, SegmentsCross)
{
// Vertical ab vs horizontal cd just above the x-axis; strict interior crossing.
EXPECT_EQ(SS::Cross, geofence_utils::segmentsIntersect(0, 0, 0, 100, -1, 1, 100, 1));
// Crossing at very shallow angle - not a problem in fixed point
const int D = 10'000'000; // 100 km
const int d = 1; // 1 cm
EXPECT_EQ(SS::Cross, geofence_utils::segmentsIntersect(0, 0, D, 0, 0, -d, D, d));
}
TEST(GeofenceUtilsTest, SegmentsEndpointOnInterior)
{
// Endpoint c of cd lies strictly on the open ab (axis-aligned).
EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(0, 0, 0, 200, 0, 100, 100, 100));
EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, 0, 0, 0, 200));
// Same configuration on a slanted line.
EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(-100, 0, 100, 200, 0, 100, 100, 100));
EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, -100, 0, 100, 200));
}
TEST(GeofenceUtilsTest, SegmentsCollinear)
{
// Identical segments and sub-intervals are reported as Collinear, not Cross.
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 2, 2, 1, 1, 2, 2));
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 300, 0, 0, 0, 300, 0));
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(1000, 1000, 4000, 2000, 1000, 1000, 4000, 2000));
// Same supporting line, non-overlapping intervals.
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 100, 0, 200, 0, 300, 0));
}
TEST(GeofenceUtilsTest, SegmentsDisjoint)
{
// Parallel, non-collinear, disjoint.
EXPECT_EQ(SS::Disjoint, geofence_utils::segmentsIntersect(0, 0, 300, 0, 1000, 1000, 4000, 2000));
}
TEST(GeofenceUtilsTest, PolygonIsCCW)
{
const Vector2f square[4] = {{0.f, 0.f}, {1.f, 0.f}, {1.f, 1.f}, {0.f, 1.f}};
EXPECT_TRUE(geofence_utils::isPolygonCCW(square, 4));
}
// ===========================================================================
// Segment vs polygon: each test names one geometric configuration and asserts
// the violation flag for both inclusion (outside disallowed) and exclusion
// (inside disallowed), under both polygon winding orders.
// ===========================================================================
constexpr int kMaxN = 8;
// Check segment/polygon interior intersection under both winding direction &
// both zone types. Note that the used lineSegmentIntersectsPolygon first adds
// the polygon to the internal representation which canonicalises orientation.
// So the internal convention (left = illegal) should hold, this is also what we
// test here.
void expectSegmentVsPolygon(const Vector2f *vertices, int n,
const Vector2f &a, const Vector2f &b,
bool intersects_inclusion, bool intersects_exclusion)
{
ASSERT_LE(n, kMaxN);
Vector2f rev[kMaxN];
for (int i = 0; i < n; ++i) { rev[i] = vertices[n - 1 - i]; }
EXPECT_EQ(intersects_inclusion, geofence_utils::lineSegmentIntersectsPolygon(a, b, vertices, n, true));
EXPECT_EQ(intersects_exclusion, geofence_utils::lineSegmentIntersectsPolygon(a, b, vertices, n, false));
EXPECT_EQ(intersects_inclusion, geofence_utils::lineSegmentIntersectsPolygon(a, b, rev, n, true));
EXPECT_EQ(intersects_exclusion, geofence_utils::lineSegmentIntersectsPolygon(a, b, rev, n, false));
}
// Convex polygon (unit square)
const Vector2f kSquare[4] = {{0.f, 0.f}, {1.f, 0.f}, {1.f, 1.f}, {0.f, 1.f}};
TEST(GeofenceUtilsTest, SquareSegmentOutside)
{
// Far outside: inclusion is violated (must stay inside), exclusion is fine.
expectSegmentVsPolygon(kSquare, 4, {4.f, 5.f}, {5.f, 4.f}, true, false);
}
TEST(GeofenceUtilsTest, SquareSegmentInside)
{
// Strict interior: exclusion violated, inclusion fine.
expectSegmentVsPolygon(kSquare, 4, {0.2f, 0.2f}, {0.6f, 0.5f}, false, true);
}
TEST(GeofenceUtilsTest, SquareSegmentStrictCrossing)
{
// Interior endpoint to exterior endpoint via a single proper edge crossing.
expectSegmentVsPolygon(kSquare, 4, {0.5f, 0.5f}, {0.5f, 1.5f}, true, true);
}
TEST(GeofenceUtilsTest, SquareSegmentCrossesTwoEdges)
{
// Exterior endpoints with two proper edge crossings.
expectSegmentVsPolygon(kSquare, 4, {0.5f, -0.5f}, {0.5f, 1.5f}, true, true);
}
TEST(GeofenceUtilsTest, SquareSegmentEdgeToEdgeThroughInterior)
{
// Endpoints on two opposite edges; the segment body lies in the interior.
expectSegmentVsPolygon(kSquare, 4, {0.5f, 0.f}, {0.5f, 1.f}, false, true);
}
TEST(GeofenceUtilsTest, SquareSegmentDiagonalThroughOppositeVertices)
{
// Endpoints are two opposite polygon vertices; segment lies in the interior.
expectSegmentVsPolygon(kSquare, 4, {0.f, 0.f}, {1.f, 1.f}, false, true);
// Segment passes through opposite vertices - both zones violated.
expectSegmentVsPolygon(kSquare, 4, {-0.5f, 0.5f}, {1.5f, 1.5f}, true, true);
// Diagonal through only one vertex - still both zones.
expectSegmentVsPolygon(kSquare, 4, {2.f, 2.f}, {0.5f, 0.5f}, true, true);
}
TEST(GeofenceUtilsTest, SquareSegmentAlongEdge)
{
// Segment coincides with one edge (pure graze, no strict interior or exterior).
expectSegmentVsPolygon(kSquare, 4, {0.f, 0.f}, {1.f, 0.f}, false, false);
}
TEST(GeofenceUtilsTest, SquareSegmentExtendsBeyondEdge)
{
// Segment overlaps edge but is longer. For exclusion this is ok
// (non-intersecting), for inclusion this violates the outside region.
expectSegmentVsPolygon(kSquare, 4, {0.f, 0.f}, {2.f, 0.f}, true, false);
expectSegmentVsPolygon(kSquare, 4, {-1.f, 0.f}, {2.f, 0.f}, true, false);
}
TEST(GeofenceUtilsTest, SquareSegmentTangentThroughVertex)
{
// Tangent line touching the (1,1) corner with both endpoints outside.
// Inclusion violated (line outside), exclusion not (only boundary).
// One case with midpoint = vertex, one !=
expectSegmentVsPolygon(kSquare, 4, {2.f, 0.f}, {0.f, 2.f}, true, false);
expectSegmentVsPolygon(kSquare, 4, {2.f, 0.f}, {-1.f, 3.f}, true, false);
// If we nudge the point inward just a bit, we have a strict intersection
// and both zones are violated.
expectSegmentVsPolygon(kSquare, 4, {2.f, 0.f}, {0.f, 1.98f}, true, true);
}
// Nonconvex polygon (L-shape with a reflex vertex at (1,1))
const Vector2f kLShape[6] = {{0.f, 0.f}, {2.f, 0.f}, {2.f, 1.f}, {1.f, 1.f}, {1.f, 2.f}, {0.f, 2.f}};
TEST(GeofenceUtilsTest, LShapeSegmentDisjointOutside)
{
// Entirely in the notch (the reflex cone, outside the polygon).
expectSegmentVsPolygon(kLShape, 6, {1.5f, 1.5f}, {1.8f, 1.8f}, true, false);
}
TEST(GeofenceUtilsTest, LShapeSegmentEntirelyInside)
{
// Horizontal line inside bottom rectangle - inclusion non-intersecting, exclusion intersecting
expectSegmentVsPolygon(kLShape, 6, {0.5f, 0.5f}, {1.5f, 0.5f}, false, true);
// Skewed line inside upper rectangle - same
expectSegmentVsPolygon(kLShape, 6, {0.3f, 1.2f}, {0.7f, 1.8f}, false, true);
}
TEST(GeofenceUtilsTest, LShapeSegmentCrossesNotchEdge)
{
// Single proper crossing from the upper-rect interior into the notch.
expectSegmentVsPolygon(kLShape, 6, {0.5f, 1.5f}, {1.5f, 1.5f}, true, true);
}
TEST(GeofenceUtilsTest, LShapeSegmentThroughReflexVertexInsideToInside)
{
// Both endpoints inside, segment passes through the reflex (1,1) but
// stays inside. Inclusion non-intersecting, exclusion intersecting.
expectSegmentVsPolygon(kLShape, 6, {0.5f, 1.5f}, {2.f, 0.f}, false, true);
expectSegmentVsPolygon(kLShape, 6, {0.5f, 1.5f}, {1.5f, 0.5f}, false, true);
}
TEST(GeofenceUtilsTest, LShapeSegmentThroughReflexVertexIntoNotch)
{
// One endpoint inside the polygon, the other in the notch; the segment
// crosses the boundary at the reflex vertex.
expectSegmentVsPolygon(kLShape, 6, {0.5f, 0.5f}, {2.f, 2.f}, true, true);
}
TEST(GeofenceUtilsTest, LShapeSegmentBetweenTwoVerticesAcrossNotch)
{
// Endpoints are two convex polygon vertices, (2,1) and (1,2). The chord
// between them crosses the notch (outside the polygon), grazing the
// polygon only at those two vertices.
expectSegmentVsPolygon(kLShape, 6, {2.f, 1.f}, {1.f, 2.f}, true, false);
}
TEST(GeofenceUtilsTest, LShapeSegmentTangentThroughConvexVertex)
{
// These lines touch a vertex from outside. Inclusion -> intersection,
// exclusion -> no intersection.
// Tangent at (2, 0)
expectSegmentVsPolygon(kLShape, 6, {3.f, 1.f}, {1.5f, -0.5f}, true, false);
// Tangent at (0, 0)
expectSegmentVsPolygon(kLShape, 6, {-1.f, 1.f}, {1.f, -1.f}, true, false);
// Tangent at both (1, 2) and (2, 1)
expectSegmentVsPolygon(kLShape, 6, {0.f, 3.f}, {3.f, 0.f}, true, false);
}
// ===========================================================================
// addPolygon rejects input out of bounds
// ===========================================================================
TEST(GeofenceUtilsTest, AddPolygonRejectsOutOfBounds)
{
// Square inside of bounds, 10000 km.
float extent = 10000 * 1000; // km to m
const Vector2f square[4] = {{0.f, 0.f}, {extent, 0.f}, {extent, extent}, {0.f, extent}};
geofence_utils::PlannerPolygons polys;
EXPECT_EQ(polys.addPolygon(square, 4, /*is_inclusion_zone=*/false, /*margin=*/0.f), AddResult::Success);
// Square out of bounds, 12000 km.
extent = 12000 * 1000;
const Vector2f square_larger[4] = {{0.f, 0.f}, {extent, 0.f}, {extent, extent}, {0.f, extent}};
geofence_utils::PlannerPolygons polys_larger;
EXPECT_EQ(polys_larger.addPolygon(square_larger, 4, /*is_inclusion_zone=*/false, /*margin=*/0.f), AddResult::OutOfRange);
}
TEST(GeofenceUtilsTest, AddApproxCircleRejectsOutOfBounds)
{
// 100m circle at origin with 10m margin - works
geofence_utils::PlannerPolygons polys0;
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 0), 100.0f, 10.0f, false),
AddResult::Success
);
// 5000km circle -- same
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 0), 5000.f * 1000.f, 10.0f, false),
AddResult::Success
);
// 10000km circle -- fails (circle is within range but approx circle not)
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 0), 10000.f * 1000.f, 10.0f, false),
AddResult::OutOfRange
);
// 12000km circle -- fails
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 0), 12000.f * 1000.f, 10.0f, false),
AddResult::OutOfRange
);
// 10000km circle, 2000 km margin (outwards, exclusion zone) -- fails
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 0), 12000.f * 1000.f, 2000.f * 1000.f, false),
AddResult::OutOfRange
);
// 2000km circle 10000km out - fails
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 10000.f * 1000.f), 2000.f * 1000.f, 10.f, false),
AddResult::OutOfRange
);
// 100km circle 10000km out - works
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 10000.f * 1000.f), 100.f * 1000.f, 10.f, false),
AddResult::Success
);
// 100km circle 10000km out, 1000km margin - fails
EXPECT_EQ(
polys0.addApproxCircle(matrix::Vector2f(0, 10000.f * 1000.f), 100.f * 1000.f, 1000.f * 1000.f, false),
AddResult::OutOfRange
);
}
// ===========================================================================
// addPolygon rejects non-simple input
// ===========================================================================
TEST(GeofenceUtilsTest, AddPolygonRejectsSelfIntersecting)
{
// Figure-eight quadrilateral: 0->1->2->3 with edges (0,1) and (2,3) crossing.
const Vector2f figure_eight[4] = {{0.f, 0.f}, {10.f, 10.f}, {10.f, 0.f}, {0.f, 10.f}};
geofence_utils::PlannerPolygons polys;
EXPECT_EQ(polys.addPolygon(figure_eight, 4, /*is_inclusion_zone=*/false, /*margin=*/0.f), AddResult::Degenerate);
}
TEST(GeofenceUtilsTest, AddPolygonAcceptsSimpleQuad)
{
// Convex simple quadrilateral; must succeed.
const Vector2f square[4] = {{0.f, 0.f}, {10.f, 0.f}, {10.f, 10.f}, {0.f, 10.f}};
geofence_utils::PlannerPolygons polys;
EXPECT_EQ(polys.addPolygon(square, 4, /*is_inclusion_zone=*/false, /*margin=*/0.f), AddResult::Success);
}

530
src/lib/geofence/geofence_utils.cpp Normal file
View File

@@ -0,0 +1,530 @@
/****************************************************************************
*
* Copyright (c) 2013-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "geofence_utils.h"
#include <lib/geo/geo.h>
namespace geofence_utils
{
namespace
{
int32_t metersToCm(float m) { return roundf(m * CM_PER_M); }
} // namespace
void PlannerPolygons::setNode(int idx, const matrix::Vector2f &p)
{
idx = math::constrain(idx, 0, kMaxNodes - 1);
_x_cm[idx] = metersToCm(p(0));
_y_cm[idx] = metersToCm(p(1));
}
PlannerPolygons::AddResult PlannerPolygons::addPolygon(const matrix::Vector2f *vertices_in, int num_vertices,
bool is_inclusion_zone, float margin)
{
if (_num_polygons >= kMaxPolygons || _num_nodes + num_vertices > kMaxNodes) {
return AddResult::BudgetExceeded;
}
if (margin < -FLT_EPSILON || num_vertices < 3) { return AddResult::Degenerate; }
for (int i = 0; i < num_vertices; ++i) {
// Reject self-intersecting (non-simple) polygons.
const matrix::Vector2f &a = vertices_in[i];
const matrix::Vector2f &b = vertices_in[(i + 1) % num_vertices];
for (int j = i + 2; j < num_vertices; ++j) {
if (i == 0 && j == num_vertices - 1) {
continue; // (last edge, first edge) are adjacent
}
const matrix::Vector2f &c = vertices_in[j];
const matrix::Vector2f &d = vertices_in[(j + 1) % num_vertices];
if (segmentsIntersect(
metersToCm(a(0)), metersToCm(a(1)), metersToCm(b(0)), metersToCm(b(1)),
metersToCm(c(0)), metersToCm(c(1)), metersToCm(d(0)), metersToCm(d(1))) != SegSegResult::Disjoint) {
return AddResult::Degenerate;
}
}
// Reject vertices outside the usable fixed-point range
if (!inFixedPointRange(vertices_in[i](0)) || !inFixedPointRange(vertices_in[i](1))) {
return AddResult::OutOfRange;
}
}
// Canonical orientation: walking vertices in stored order, INSIDE is on
// the left of each edge. CCW for exclusion (bounded interior is forbidden),
// CW for inclusion (unbounded exterior is forbidden). Reverse iff input
// doesn't already match.
const bool reverse = (isPolygonCCW(vertices_in, num_vertices) == is_inclusion_zone);
// Index into vertices_in in stored order (accounts for possible reversal).
auto vertex = [&](int i) -> const matrix::Vector2f & {
return vertices_in[reverse ? (num_vertices - 1 - i) : i];
};
PolygonInfo &poly = _polygons[_num_polygons];
poly.start_index = _num_nodes;
poly.is_inclusion = is_inclusion_zone;
int out_idx = 0;
for (int i = 0; i < num_vertices; i++) {
const int prev = (i + num_vertices - 1) % num_vertices;
const int next = (i + 1) % num_vertices;
const matrix::Vector2f edge_in = vertex(i) - vertex(prev);
const matrix::Vector2f edge_out = vertex(next) - vertex(i);
const float edge_in_norm = edge_in.norm();
const float edge_out_norm = edge_out.norm();
if (edge_in_norm < FLT_EPSILON || edge_out_norm < FLT_EPSILON) {
return AddResult::Degenerate;
}
const matrix::Vector2f edge_in_normalized = edge_in / edge_in_norm;
const matrix::Vector2f edge_out_normalized = edge_out / edge_out_norm;
// Inward normals of the two adjacent edges.
// Canonical stored orientation: left side = forbidden region, right side = allowed region
const matrix::Vector2f n_in = matrix::Vector2f{-edge_in_normalized(1), edge_in_normalized(0)};
const matrix::Vector2f n_out = matrix::Vector2f{-edge_out_normalized(1), edge_out_normalized(0)};
matrix::Vector2f bisector = n_in + n_out;
const float bisector_len = bisector.length();
if (bisector_len < FLT_EPSILON) {
return AddResult::Degenerate; // antiparallel edges - polygon doubles back
}
const matrix::Vector2f normalized_bisector = bisector / bisector_len;
// The following bit ensures that:
// - The original edges are moved away from the forbidden interior by `margin`
// - Sharp convex corners (<60deg) are "cut off" with a new edge that is exactly
// `margin` away from the original vertex. For this the vertex is replaced by
// two new ones.
// Only split convex corners - others are ignored, see _node_not_on_optimal_path
const bool corner_convex = edge_in_normalized(0) * edge_out_normalized(1)
- edge_in_normalized(1) * edge_out_normalized(0) > 0.f;
// Scaling needed wrt the normalised bisector to achieve parallel edges
const float offset_scale = 2.f / bisector_len;
// bisector_len = 2*sin(interior angle/2); split when interior angle < 60 deg <=> bisector_len < 1.
const bool angle_sharp = bisector_len < 1;
// If margin zero, do not split - would output the same vertex twice.
const bool margin_nonzero = margin > FLT_EPSILON;
// If we do not have enough space, do not split the vertex.
// This never happens if the planner-internal node buffer is 2x the original buffer.
const bool space_for_split_vertices = _num_nodes + out_idx + 1 < kMaxNodes;
if (corner_convex && angle_sharp && margin_nonzero && space_for_split_vertices) {
// Sharp corner: cut it off along the line
// - perpendicular to normalized_bisector
// - going through base := vertex(i) - normalized_bisector * margin
const matrix::Vector2f base = vertex(i) - normalized_bisector * margin;
const matrix::Vector2f perp = {-normalized_bisector(1), normalized_bisector(0)};
// Construct s such that:
// base + perp*s is on the offset incoming edge
// base - perp*s is on the offset outgoing edge
const float e_out_dot_b = edge_out_normalized.dot(normalized_bisector);
const float s = margin * (1.f - n_out.dot(normalized_bisector)) / e_out_dot_b;
setNode(poly.start_index + out_idx++, base + perp * s);
setNode(poly.start_index + out_idx++, base - perp * s);
} else {
// Regular corner or no space for split: single bisector vertex displaced by margin / sin(interior angle/2).
setNode(poly.start_index + out_idx++, vertex(i) - normalized_bisector * (margin * offset_scale));
}
}
const int num_out = out_idx;
poly.num_vertices = num_out;
computeBoundingBox(poly.start_index, num_out, poly.min_x, poly.max_x, poly.min_y, poly.max_y);
// In canonical orientation, the node is not possibly on the shortest
// path if the vertex curves right or straight (forbidden region >= 180
// deg)
for (int i = 0; i < num_out; i++) {
const int prev = poly.start_index + (i + num_out - 1) % num_out;
const int curr = poly.start_index + i;
const int next = poly.start_index + (i + 1) % num_out;
const bool curves_right_or_straight = orient2d(_x_cm[prev], _y_cm[prev],
_x_cm[curr], _y_cm[curr],
_x_cm[next], _y_cm[next]) <= 0;
_node_not_on_optimal_path[curr] = curves_right_or_straight;
}
_num_nodes += num_out;
++_num_polygons;
return AddResult::Success;
}
PlannerPolygons::AddResult PlannerPolygons::addApproxCircle(const matrix::Vector2f &center, const float circle_radius,
float margin, const bool is_inclusion_zone)
{
// For planning we approximate circles by regular k-gons. This adds
// quite some nodes and additional restricted area. However, planning
// properly around circles would be more complicated. Read these if that
// becomes a priority regardless:
//
// - https://dl.acm.org/doi/epdf/10.1145/323233.323261
// - https://www.sciencedirect.com/science/article/pii/S0925772106000496
//
// Keep in mind that the added complexity would extend beyond the
// planner into all controllers, which would then have to also fly
// loiter segments, not just pure waypoint sequences.
if (_num_polygons >= kMaxPolygons || _num_nodes + kCircleApproxVertices > kMaxNodes) {
return AddResult::BudgetExceeded;
}
if (circle_radius <= 0.1f) { return AddResult::Degenerate; }
// ensures that the k-gon is, w.r.t the real circle:
// - over-approximation of circle grown by margin, if exclusion
// - under-approximation of circle shrunk by margin, if inclusion
const float k_gon_radius = is_inclusion_zone
? circle_radius - margin
: circle_radius / cosf(M_PI_F / kCircleApproxVertices) + margin;
if (k_gon_radius <= FLT_EPSILON) { // Negative radius - circle empty
return AddResult::Degenerate;
}
if (!inFixedPointRange(fabsf(center(0)) + k_gon_radius)
|| !inFixedPointRange(fabsf(center(1)) + k_gon_radius)) {
return AddResult::OutOfRange;
}
// Canonical orientation: CCW for exclusion, CW for inclusion
const float delta_angle = (is_inclusion_zone ? -1.f : 1.f) * 2.f * M_PI_F / kCircleApproxVertices;
PolygonInfo &poly = _polygons[_num_polygons];
poly.start_index = _num_nodes;
poly.num_vertices = kCircleApproxVertices;
poly.is_inclusion = is_inclusion_zone;
for (int i = 0; i < kCircleApproxVertices; i++) {
const float angle = (i + 0.5f) * delta_angle;
const matrix::Vector2f p = center + matrix::Vector2f{
k_gon_radius * cosf(angle), k_gon_radius * sinf(angle)
};
setNode(poly.start_index + i, p);
}
computeBoundingBox(poly.start_index, kCircleApproxVertices, poly.min_x, poly.max_x, poly.min_y, poly.max_y);
// For regular k-gons, all corners are either convex or reflex based on
// is_inclusion, so the reduced visibility graph criterion is simplified
for (int i = 0; i < kCircleApproxVertices; i++) {
_node_not_on_optimal_path[poly.start_index + i] = is_inclusion_zone;
}
_num_nodes += kCircleApproxVertices;
++_num_polygons;
return AddResult::Success;
}
void PlannerPolygons::computeBoundingBox(const int start_index, const int num_vertices,
int32_t &min_x, int32_t &max_x, int32_t &min_y, int32_t &max_y)
{
// Store the bounding box, so we can do a very cheap check against the
// bounding box, and only do the full line-polygon intersection check if
// the line intersects the bounding box.
min_x = max_x = _x_cm[start_index];
min_y = max_y = _y_cm[start_index];
for (int i = 1; i < num_vertices; i++) {
const int32_t x = _x_cm[start_index + i];
const int32_t y = _y_cm[start_index + i];
if (x < min_x) { min_x = x; }
if (x > max_x) { max_x = x; }
if (y < min_y) { min_y = y; }
if (y > max_y) { max_y = y; }
}
}
bool PlannerPolygons::pointInsideInteriorCone(const PolygonInfo &poly,
int32_t px, int32_t py, int v) const
{
const int prev = poly.start_index + ((v + poly.num_vertices - 1) % poly.num_vertices);
const int curr = poly.start_index + v;
const int next = poly.start_index + ((v + 1) % poly.num_vertices);
const int p_left_of_incoming = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], px, py);
const int p_left_of_outgoing = orient2d(_x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next], px, py);
const int is_convex = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next]);
return is_convex > 0 // If convex==0 (exact 180 deg "corner") the two cases are equivalent
? (p_left_of_incoming > 0 && p_left_of_outgoing > 0) // Convex: interior = intersection of two half-planes
: (p_left_of_incoming > 0 || p_left_of_outgoing > 0); // Reflex: interior = union of two half-planes
}
bool PlannerPolygons::intersectsInsideOf(const PolygonInfo &poly,
int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const
{
// Single pass over polygon edges, doing two jobs:
// (1) classify the test segment vs each polygon edge. early-return on:
// - strict crossings (line segment - polygon edge)
// - interior crossing at vertex
// (2) accumulate midpoint winding contribution -- if (1) is inconclusive,
// this decides if the line is inside or outside
int wn = 0;
bool mid_on_boundary = false;
// Rather than calculating the midpoint (mid = (s + e)/2) explicitly,
// which will lead to rounding error for uneven numbers, we instead
// scale all other inputs by two, reducing the range by a factor of two
// (2^31 cm = 21400 km -> 2^30 cm = 10700 km)
const int32_t twice_mid_x = s_x + e_x;
const int32_t twice_mid_y = s_y + e_y;
for (int i = 0; i < poly.num_vertices; i++) {
const int prev_idx = poly.start_index + (i + poly.num_vertices - 1) % poly.num_vertices;
const int curr_idx = poly.start_index + i;
const int32_t ax = _x_cm[prev_idx], ay = _y_cm[prev_idx];
const int32_t bx = _x_cm[curr_idx], by = _y_cm[curr_idx];
// (1) test segment vs polygon edge (a, b)
switch (segmentsIntersect(ax, ay, bx, by, s_x, s_y, e_x, e_y)) {
case SegSegResult::Cross:
return true;
// skip AInsideCD - a from this segment is b from the adjacent segment
case SegSegResult::BInsideCD:
// Polygon vertex b is exactly on the interior of the test segment.
// Return intersecting if it pokes into the polygon interior through vertex b.
if (pointInsideInteriorCone(poly, s_x, s_y, i) || pointInsideInteriorCone(poly, e_x, e_y, i)) {
return true;
}
break;
default:
break;
}
// (2) midpoint winding contribution
// Ref: Dan Sunday, Inclusion of a Point in a Polygon
// https://web.archive.org/web/20130126163405/http://geomalgorithms.com/a03-_inclusion.html
const int side = orient2d(2 * ax, 2 * ay, 2 * bx, 2 * by, twice_mid_x, twice_mid_y);
if (side == 0) {
const bool mid_on_open_ab = collinearBetween(2 * ax, 2 * ay, 2 * bx, 2 * by, twice_mid_x, twice_mid_y);
const bool mid_is_b = 2 * bx == twice_mid_x && 2 * by == twice_mid_y;
// Skip point a - a from this segment is b from the adjacent segment
if (mid_on_open_ab || mid_is_b) {
mid_on_boundary = true;
}
} else if (2 * ay <= twice_mid_y) {
if (2 * by > twice_mid_y && side > 0) { wn++; }
} else if (2 * by <= twice_mid_y && side < 0) { wn--; }
}
// Midpoint on boundary, segment runs along it (strict crossing detected earlier).
// Non-intersecting in either zone.
if (mid_on_boundary) {
return false;
}
// Exclusion: violation when midpoint is inside (bounded). Inclusion: when outside (unbounded).
// This is the only place we depart from the inside = left = forbidden canonical orientation,
// necessary because of the point membership check.
const bool midpoint_in_bounded_inside = (wn != 0);
return midpoint_in_bounded_inside != poly.is_inclusion;
}
bool PlannerPolygons::intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const
{
const int32_t seg_min_x = s_x < e_x ? s_x : e_x;
const int32_t seg_max_x = s_x > e_x ? s_x : e_x;
const int32_t seg_min_y = s_y < e_y ? s_y : e_y;
const int32_t seg_max_y = s_y > e_y ? s_y : e_y;
for (int p = 0; p < _num_polygons; p++) {
const PolygonInfo &poly = _polygons[p];
if (seg_max_x < poly.min_x || seg_min_x > poly.max_x ||
seg_max_y < poly.min_y || seg_min_y > poly.max_y) {
// Segment is entirely outside this polygon.
// For exclusion zones: outside = safe, skip.
// For inclusion zones: outside = forbidden region, immediate violation.
if (poly.is_inclusion) { return true; }
continue;
}
if (intersectsInsideOf(poly, s_x, s_y, e_x, e_y)) {
return true;
}
}
return false;
}
bool PlannerPolygons::edgeBitangent(int a, int32_t bx, int32_t by) const
{
// Find the polygon owning vertex index a. Indices not belonging to any
// polygon (in particular the destination at index 0) are trivially bitangent.
if (a == destIndex()) { return true; }
for (int p = 0; p < _num_polygons; p++) {
const PolygonInfo &poly = _polygons[p];
if (a >= poly.start_index && a < poly.start_index + poly.num_vertices) {
// Define point c = a + (a-b) = 2a - b, so that ac = ab mirrored about a
// Then: Edge is bitangent at a <=> neither ab nor ac pokes into the interior
int32_t cx = 2 * _x_cm[a] - bx;
int32_t cy = 2 * _y_cm[a] - by;
return !pointInsideInteriorCone(poly, bx, by, a - poly.start_index)
&& !pointInsideInteriorCone(poly, cx, cy, a - poly.start_index);
}
}
return true;
}
bool PlannerPolygons::edgeBitangent(int a, int b) const
{
return edgeBitangent(a, _x_cm[b], _y_cm[b]);
}
bool PlannerPolygons::edgeBitangent(int a, const matrix::Vector2f &b) const
{
return edgeBitangent(a, metersToCm(b(0)), metersToCm(b(1)));
}
bool PlannerPolygons::setDestination(const matrix::Vector2f &p)
{
setNode(destIndex(), p);
const int32_t x_cm = metersToCm(p(0));
const int32_t y_cm = metersToCm(p(1));
if (intersectsAnyInside(x_cm, y_cm, x_cm, y_cm)) {
// If the destination breaches a geofence, still use it but
// signal to the user that it is invalid.
return false;
}
return true;
}
matrix::Vector2f PlannerPolygons::getDestination() const
{
return node(destIndex());
}
bool PlannerPolygons::edgeVisible(int a, int b) const
{
if (a == b) { return false; }
if (_node_not_on_optimal_path[a] || _node_not_on_optimal_path[b]) { return false; }
// Only bitangent edges can be on a shortest path; this check also cheaply
// discards edges that violate the polygon at the vertex they come from.
if (!edgeBitangent(a, b) || !edgeBitangent(b, a)) { return false; }
return !intersectsAnyInside(_x_cm[a], _y_cm[a], _x_cm[b], _y_cm[b]);
}
bool PlannerPolygons::edgeVisible(const matrix::Vector2f &a, int b) const
{
if (_node_not_on_optimal_path[b]) { return false; }
if (!edgeBitangent(b, a)) { return false; }
return !intersectsAnyInside(metersToCm(a(0)), metersToCm(a(1)), _x_cm[b], _y_cm[b]);
}
bool PlannerPolygons::edgeVisible(const matrix::Vector2f &a, const matrix::Vector2f &b) const
{
return !intersectsAnyInside(metersToCm(a(0)), metersToCm(a(1)), metersToCm(b(0)), metersToCm(b(1)));
}
float PlannerPolygons::edgeCost(int a, int b) const
{
return edgeVisible(a, b) ? (node(a) - node(b)).norm() : INFINITY;
}
bool isPolygonCCW(const matrix::Vector2f *vertices, int num_vertices)
{
// https://en.wikipedia.org/wiki/Shoelace_formula
// Triangle formula
float signed_area_2x = 0.f;
for (int i = 0; i < num_vertices; i++) {
const int j = (i + 1) % num_vertices;
signed_area_2x += vertices[i](0) * vertices[j](1)
- vertices[j](0) * vertices[i](1);
}
return signed_area_2x > 0.f;
}
} // namespace geofence_utils

390
src/lib/geofence/geofence_utils.h Normal file
View File

@@ -0,0 +1,390 @@
/****************************************************************************
*
* Copyright (c) 2013-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geofence_utils.h
* Pure geometry functions for geofence point-in-polygon and point-in-circle tests.
*/
#pragma once
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <matrix/math.hpp>
#include <dataman/dataman.h>
#include <px4_platform_common/px4_config.h> // CONFIG_NAVIGATOR_GEOFENCE_MAX_NODES
namespace geofence_utils
{
static constexpr float CM_PER_M = 100.f;
// True iff `m_value` (metres) fits into the planner's fixed-point
// representation. intersectsInsideOf doubles cm coordinates internally, so the
// usable range is 2^30 cm = 10,737 km from the planner's reference.
inline bool inFixedPointRange(float m_value) { return fabsf(m_value) < (1u << 30) / CM_PER_M; }
/**
* Sign of the signed area of triangle abc -- the fundamental 2D orientation
* predicate. Returns:
* +1, if c is left of a->b (CCW turn)
* -1, if c is right of a->b (CW turn)
* 0, if collinear
*
* Refs:
*
* Fundamental ideas, usage of orient2d for higher level geometry:
* Lecture Notes on Geometric Robustness, Jonathan Richard Shewchuk
* https://perso.uclouvain.be/jean-francois.remacle/LMECA2170/robnotes.pdf
*
* The fact that in fixed-point orient2d is automatically exact:
* Computational Geometry in C, Joseph O'Rourke
* https://www.science.smith.edu/~jorourke/books/compgeom.html
*
*/
inline int orient2d(int32_t ax, int32_t ay,
int32_t bx, int32_t by,
int32_t cx, int32_t cy)
{
const int64_t det = (static_cast<int64_t>(bx) - ax) * (static_cast<int64_t>(cy) - ay)
- (static_cast<int64_t>(by) - ay) * (static_cast<int64_t>(cx) - ax);
if (det > 0) { return 1; }
if (det < 0) { return -1; }
return 0;
}
/**
* For a, b, c collinear (orient2d == 0), is c on the open segment ab?
*/
inline bool collinearBetween(int32_t ax, int32_t ay, int32_t bx, int32_t by, int32_t cx, int32_t cy)
{
if (std::abs(ax - bx) >= std::abs(ay - by)) {
return (ax < cx && cx < bx) || (bx < cx && cx < ax);
} else {
return (ay < cy && cy < by) || (by < cy && cy < ay);
}
}
/**
* Classification of the relative position of two segments ab and cd.
*
* Cross proper interior crossing (each segment strictly straddles
* the other's supporting line)
* AInsideCD a strictly between c and d (open interior of segment cd)
* BInsideCD b strictly between c and d
* CInsideAB c strictly between a and b
* DInsideAB d strictly between a and b
* Collinear ab and cd lie on the same supporting line (overlap or not)
* Disjoint none of the above
*
* Ref: SegSegInt implementation from:
* Computational Geometry in C, Joseph O'Rourke
* https://www.science.smith.edu/~jorourke/books/compgeom.html
*/
enum class SegSegResult {
Disjoint,
Cross,
AInsideCD, BInsideCD,
CInsideAB, DInsideAB,
Collinear,
};
inline SegSegResult segmentsIntersect(int32_t ax, int32_t ay, int32_t bx, int32_t by,
int32_t cx, int32_t cy, int32_t dx, int32_t dy)
{
// These two early returns are logically not necessary (would catch the
// case in the end) but improve performance, as most segment pairs are
// expected to be completely disjoint. If both c and d are strictly on
// the same side of a-b (first check, second vice versa) we can already
// conclude the segments are disjoint. This does not detect _all_
// disjoint cases so the final return stays.
const int o1 = orient2d(ax, ay, bx, by, cx, cy);
const int o2 = orient2d(ax, ay, bx, by, dx, dy);
if (o1 != 0 && o1 == o2) { return SegSegResult::Disjoint; }
const int o3 = orient2d(cx, cy, dx, dy, ax, ay);
const int o4 = orient2d(cx, cy, dx, dy, bx, by);
if (o3 != 0 && o3 == o4) { return SegSegResult::Disjoint; }
if (o1 && o2 && o3 && o4 && o1 != o2 && o3 != o4) { return SegSegResult::Cross; }
if (!o1 && !o2 && !o3 && !o4) { return SegSegResult::Collinear; }
// Endpoint strictly on the open interior of the other segment iff:
// - orient2d is zero (point on the supporting line)
// - the point lies strictly between the other two via collinearBetween
if (o3 == 0 && collinearBetween(cx, cy, dx, dy, ax, ay)) { return SegSegResult::AInsideCD; }
if (o4 == 0 && collinearBetween(cx, cy, dx, dy, bx, by)) { return SegSegResult::BInsideCD; }
if (o1 == 0 && collinearBetween(ax, ay, bx, by, cx, cy)) { return SegSegResult::CInsideAB; }
if (o2 == 0 && collinearBetween(ax, ay, bx, by, dx, dy)) { return SegSegResult::DInsideAB; }
return SegSegResult::Disjoint;
}
/**
* Check if a polygon's vertices are ordered counter-clockwise using the shoelace formula.
* Works in local Cartesian coordinates. Returns false for CW or zero-area polygons.
*
* @param vertices polygon vertices in local frame
* @param num_vertices number of vertices
* @return true if the polygon is counter-clockwise
*/
bool isPolygonCCW(const matrix::Vector2f *vertices, int num_vertices);
/**
* Upper bound on the number of vertices addPolygon() emits for a simple polygon:
* - Sharp convex corners (interior angle < 60 deg) are split in two vertices
* - Interior angles of a simple polygon sum to (n - 2) * 180 deg
* - With k corners below 60 deg and the rest below 360 deg, we have
* (n - 2) * 180 < k * 60 + (n - k) * 360
* => k < (180 n + 360) / 300
* so k <= ceil(.) - 1, computed as (a - 1) / b in integer arithmetic.
* Degenerate input (n < 3) is rejected by addPolygon and emits nothing, so
* the bound is 0.
*/
constexpr int maxVerticesAfterSplitting(int num_vertices)
{
return num_vertices < 3 ? 0 : num_vertices + (180 * num_vertices + 360 - 1) / 300;
}
/**
* Fence geometry cache for the geofence avoidance planner. Owns a flat
* int32-cm node buffer that holds polygon vertices, circle k-gon vertices,
* and one destination slot.
*
* Polygons are stored in canonical orientation: the polygon INSIDE is always to
* the left of each edge (CCW for exclusion zones, CW for inclusion zones).
*
* Typical lifecycle:
* 1. addPolygon / addApproxCircle (one per geofence zone)
* 2. setDestination(p) (called whenever destination changes)
* 3. edgeCost(a, b) / edgeVisible (queried during planning)
*/
class PlannerPolygons
{
public:
// By default, limit to 100 nodes, which was found to limit the full update
// on geofence / margin change to about 20 ms. Lower for weaker or RAM-constrained boards.
#ifndef CONFIG_NAVIGATOR_GEOFENCE_MAX_NODES
#define CONFIG_NAVIGATOR_GEOFENCE_MAX_NODES 100
#endif
static constexpr int kMaxNodes = CONFIG_NAVIGATOR_GEOFENCE_MAX_NODES;
static constexpr int kMaxPolygons = 16;
static constexpr int kCircleApproxVertices = 8;
// Number of nodes needed to hold the largest geofence storable in dataman.
// Worst case:
// - kMaxPolygons-1 circles (most nodes per dataman item)
// - one remaining polygon using all remaining fence points, with max amount of split vertices
// - 1 destination slot
// If kMaxNodes > kMaxNodesForAnyStorableFence, we always have enough space.
// Otherwise we may fail with AddResult::BudgetExceeded.
static constexpr int kMaxNodesForAnyStorableFence =
(kMaxPolygons - 1) * kCircleApproxVertices
+ maxVerticesAfterSplitting(DM_KEY_FENCE_POINTS_MAX - (kMaxPolygons - 1))
+ 1;
// Return code specifying why a polygon was not added.
enum class AddResult {
Success,
BudgetExceeded, // node or polygon buffer full (kMaxNodes / kMaxPolygons)
OutOfRange, // vertex or circle extent outside usable fixed-point range
Degenerate, // <3 vertices, zero-length/antiparallel edge, self-intersecting, empty circle, negative margin
};
PlannerPolygons() { reset(); }
// Slot 0 is always reserved for the destination; polygon vertices start at index 1.
void reset()
{
_num_nodes = 1;
_x_cm[0] = 0;
_y_cm[0] = 0;
_num_polygons = 0;
_node_not_on_optimal_path[0] = false;
}
// Append a polygon:
// - canonicalize orientation,
// - offset each vertex outward (exclusion) or inward (inclusion) by `margin` meters,
// - quantize to cm, append as nodes.
AddResult addPolygon(const matrix::Vector2f *vertices_in, int num_vertices,
bool is_inclusion_zone, float margin = 0.f);
// Append an approximate circle (k-gon over/underapproximation).
// - shrink/expand according to margin and is_inclusion_zone
// - quantize to cm, append as nodes.
AddResult addApproxCircle(const matrix::Vector2f &center, const float radius, float margin, const bool is_inclusion_zone);
int numNodes() const { return _num_nodes; }
// Number of polygon nodes (excludes the destination at index 0).
int numVertices() const { return _num_nodes - 1; }
matrix::Vector2f node(int idx) const
{
return matrix::Vector2f{_x_cm[idx] / CM_PER_M, _y_cm[idx] / CM_PER_M};
}
// Update the destination position. Always safe to call after reset().
// Returns false if the destination violates the geofence, but still updates destionation.
bool setDestination(const matrix::Vector2f &p);
// Get the current destination position.
matrix::Vector2f getDestination() const;
// Destination is always at index 0; polygon vertices start at index 1.
static constexpr int destIndex() { return 0; }
// True if the segment (a, b) crosses no geofence interior.
bool edgeVisible(int a, int b) const;
bool edgeVisible(const matrix::Vector2f &a, int b) const; // One float endpoint (e.g. vehicle position).
bool edgeVisible(const matrix::Vector2f &a, const matrix::Vector2f &b) const; // Both float endpoints.
// Traversal cost for edge (a, b): INFINITY if the edge crosses any fence,
// Euclidean distance (metres) otherwise.
float edgeCost(int a, int b) const;
private:
int32_t _x_cm[kMaxNodes];
int32_t _y_cm[kMaxNodes];
struct PolygonInfo {
int start_index;
int num_vertices;
bool is_inclusion;
int32_t min_x, max_x, min_y, max_y; // Bouding box
};
PolygonInfo _polygons[kMaxPolygons];
int _num_polygons{0};
/**
* Reduced visibility graph: we can exclude some vertices a priori
* because they cannot be part of the shortest path. For convex
* inclusion corners or reflex exclusion corners at node B, the path
* (A -> B -> C)
* is a detour compared to
* (A -> just before B -> just after B -> C)
* and so it cannot be globally optimal.
*/
bool _node_not_on_optimal_path[kMaxNodes];
int _num_nodes{0};
/**
* Line-segment vs polygon intersection check.
*
* Only detects intersection with the _interior_ of the polygon in
* canonical orientation, i.e. the forbidden region. Intersection with
* only the boundary is allowed (necessary for shortest path to be well
* defined)
*/
bool intersectsInsideOf(const PolygonInfo &poly, int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const;
/**
* Return true iff the line segment s-e intersects the inside of _any_ stored polygon.
*/
bool intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const;
/**
* Consider extending the triangle poly[v-1] -> poly[v] -> poly[v+1]
* outward from poly[v] (the inside always being left of the points).
* This cone is the local interior of the forbidden region at the given
* vertex.
*
* Returns true if the point p is in the interior of that cone, which is
* the case iff the line segment poly[v] -> p pierces the interior at
* vertex curr.
*/
bool pointInsideInteriorCone(const PolygonInfo &poly, int32_t px, int32_t py, int v) const;
/**
* Return true if the edge ab is bitangent at a: neither the edge nor
* its mirror about `a` enters the forbidden interior there.
*
* Only bitangent edges can be part of a shortest path [*]. Checking
* this first in edgeVisible also throws out invalid (interior-entering)
* edges before the general intersection checks.
*
* * Stephen LaValle, 2006: Planning Algorithms, 6.2.4 - Shortest-Path Roadmaps
*/
bool edgeBitangent(int a, int32_t bx, int32_t by) const;
bool edgeBitangent(int a, int b) const; // Other endpoint is node index b.
bool edgeBitangent(int a, const matrix::Vector2f &b) const; // Other endpoint is an arbitrary position.
/**
* Update {min,max}_{x,y} with the min/max coordinates of the polygon with vertices
* [start_index, start_index + num_vertices).
*/
void computeBoundingBox(const int start_index, const int num_vertices,
int32_t &min_x, int32_t &max_x, int32_t &min_y, int32_t &max_y);
// Write float-metre coordinates into an existing node slot (used internally by
// addPolygon, addApproxCircle, and setDestination).
void setNode(int idx, const matrix::Vector2f &p);
};
// Convenience for unit tests and one-shot callers: build a transient
// PlannerPolygons with the given polygon and query the segment.
inline bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end,
const matrix::Vector2f *vertices, int num_vertices, bool is_inclusion_zone)
{
PlannerPolygons polys;
if (polys.addPolygon(vertices, num_vertices, is_inclusion_zone) != PlannerPolygons::AddResult::Success) {
return false;
}
return !polys.edgeVisible(start, end);
}
} // namespace geofence_utils

22
src/modules/navigator/CMakeLists.txt
View File

@@ -32,6 +32,7 @@
############################################################################
add_subdirectory(MissionFeasibility)
add_subdirectory(RTLPlanner)
if(BUILD_TESTING)
add_subdirectory(test)
@@ -56,6 +57,20 @@ set(NAVIGATOR_SOURCES
mission_feasibility_checker.cpp
geofence.cpp)
set(NAVIGATOR_DEPENDS
dataman_client
geo
adsb
motion_planning
mission_feasibility_checker
rtl_time_estimator)
if(CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE)
list(APPEND NAVIGATOR_SOURCES rtl_geofence_avoidance_helper.cpp)
# geofence_utils (polygon offsetting / visibility graph) is only used by the planner
list(APPEND NAVIGATOR_DEPENDS geofence_utils geofence_avoidance_planner)
endif()
if(CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF)
set(NAVIGATOR_SOURCES
${NAVIGATOR_SOURCES}
@@ -74,10 +89,5 @@ px4_add_module(
rtl_params.yaml
vtol_takeoff_params.yaml
DEPENDS
dataman_client
geo
adsb
motion_planning
mission_feasibility_checker
rtl_time_estimator
${NAVIGATOR_DEPENDS}
)

15
src/modules/navigator/Kconfig
View File

@@ -27,3 +27,18 @@ menuconfig NAVIGATOR_ADSB
---help---
Add support for acting on ADSB transponder_report or ADSB_VEHICLE MAVLink messages.
Actions are warnings, Loiter, Land and RTL without climb.
menuconfig NAVIGATOR_GEOFENCE_AVOIDANCE
bool "Include geofence avoidance path planning for RTL"
default y
depends on MODULES_NAVIGATOR
---help---
Add support for RTL mode to route around geofences.
config NAVIGATOR_GEOFENCE_MAX_NODES
int "Max graph nodes for geofence RTL planner"
default 100
depends on NAVIGATOR_GEOFENCE_AVOIDANCE
---help---
Max number of geofence vertices which the RTL planner can use.
Replanning time (on geofence / safety margin change) is cubic, RAM usage quadratic in this number.

40
src/modules/navigator/RTLPlanner/CMakeLists.txt Normal file
View File

@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(geofence_avoidance_planner
geofence_avoidance_planner.cpp
geofence_avoidance_planner.h
)
target_link_libraries(geofence_avoidance_planner PRIVATE geo geofence_utils perf dijkstra)
px4_add_functional_gtest(SRC GeofenceAvoidancePlannerTest.cpp LINKLIBS geofence_avoidance_planner geo geofence_utils dijkstra perf)

412
src/modules/navigator/RTLPlanner/GeofenceAvoidancePlannerTest.cpp Normal file
View File

@@ -0,0 +1,412 @@
/****************************************************************************
*
* Copyright (C) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "geofence_avoidance_planner.h"
#include <navigator/navigation.h>
class FakeGeofence : public GeofenceInterface
{
public:
FakeGeofence(const matrix::Vector2<double> *vertices, int num_vertices, uint16_t fence_type)
: _vertices(vertices), _num_vertices(num_vertices), _fence_type(fence_type) {}
PolygonInfo getPolygonInfoByIndex(int index) override
{
PolygonInfo info{};
info.fence_type = _fence_type;
info.vertex_count = _num_vertices;
return info;
}
matrix::Vector2<double> getPolygonVertexByIndex(int poly_idx, int vertex_idx) override
{
return _vertices[vertex_idx];
}
int getNumPolygons() const override { return 1; }
private:
const matrix::Vector2<double> *_vertices;
int _num_vertices;
uint16_t _fence_type;
};
class GeofenceAvoidancePlannerTest : public ::testing::Test
{
public:
GeofenceAvoidancePlanner _planner;
};
TEST_F(GeofenceAvoidancePlannerTest, StartEqualsDestination)
{
using namespace matrix;
Vector2<double> point(47.3977, 8.5456);
FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(point);
const int num_waypoints = _planner.updateStartAndFillPath(point);
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence)
{
using namespace matrix;
Vector2<double> start(47.3977, 8.5456);
Vector2<double> destination(47.3984, 8.5470);
FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone)
{
// in this test there is an exclusion zone between start and destination. The shortest path
// is chosen to be via vertex nr 1 and vertex nr 2, so the path should have two waypoints
using namespace matrix;
Vector2<double> start(47.3559582, 8.5192064);
Vector2<double> destination(47.3546153, 8.5193195);
static const Vector2<double> vertices[] = {
{47.3552420, 8.5192293},
{47.3555843, 8.5201174},
{47.3551382, 8.5209143},
{47.3550828, 8.5171901},
};
FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
// start is the current vehicle position, so it is not part of the returned path.
// path = vertex 1 + vertex 2 (2 points)
ASSERT_EQ(num_waypoints, 2);
// index 0 is the first waypoint after start
const Vector2d first_wp = _planner.waypointAtIndex(0);
EXPECT_NEAR(first_wp(0), vertices[1](0), 1e-3);
EXPECT_NEAR(first_wp(1), vertices[1](1), 1e-3);
// index num_waypoints - 1 is the last waypoint before destination
const Vector2d last_wp = _planner.waypointAtIndex(num_waypoints - 1);
EXPECT_NEAR(last_wp(0), vertices[2](0), 1e-3);
EXPECT_NEAR(last_wp(1), vertices[2](1), 1e-3);
}
TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone)
{
using namespace matrix;
Vector2<double> start(47.3560041, 8.5191872);
Vector2<double> destination(47.3553241, 8.5177686);
// inclusion geofence which forms an L, where start and end don't have a direct connection
static const Vector2<double> vertices[] = {
{47.3562241, 8.5186720},
{47.3562479, 8.5195691},
{47.3550948, 8.5196762},
{47.3549660, 8.5171633},
{47.3555168, 8.5171278},
{47.3554507, 8.5187742}
};
FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
// start is the current vehicle position, so it is not part of the returned path.
// path = vertex 5 (1 point)
ASSERT_EQ(num_waypoints, 1);
const Vector2d only_wp = _planner.waypointAtIndex(0);
EXPECT_NEAR(only_wp(0), vertices[5](0), 1e-3);
EXPECT_NEAR(only_wp(1), vertices[5](1), 1e-3);
}
TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion)
{
using namespace matrix;
Vector2<double> start(47.3560041, 8.5191872);
Vector2<double> destination(0, 0);
// inclusion geofence which forms an L, where start and end don't have a direct connection
static const Vector2<double> vertices[] = {
{47.3562241, 8.5186720},
{47.3562479, 8.5195691},
{47.3550948, 8.5196762},
{47.3549660, 8.5171633},
{47.3555168, 8.5171278},
{47.3554507, 8.5187742}
};
FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion)
{
using namespace matrix;
Vector2<double> start(47.3559582, 8.5192064);
// DESTINATION is inside the exclusion zone
Vector2<double> destination(47.3551506, 8.5193859);
static const Vector2<double> vertices[] = {
{47.3552420, 8.5192293},
{47.3555843, 8.5201174},
{47.3551382, 8.5209143},
{47.3550828, 8.5171901},
};
FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
{
using namespace matrix;
// START is inside the exclusion zone
Vector2<double> start(47.3551506, 8.5193859);
Vector2<double> destination(47.3559582, 8.5192064);
static const Vector2<double> vertices[] = {
{47.3552420, 8.5192293},
{47.3555843, 8.5201174},
{47.3551382, 8.5209143},
{47.3550828, 8.5171901},
};
FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, FallsBackToSavedAnchorWhenStartViolatesFence)
{
using namespace matrix;
// `anchor` is in valid airspace (north of the exclusion polygon) and `destination`
// sits just next to it so it's directly reachable from there. `inside_exclusion`
// is inside the exclusion polygon, i.e. it violates the fence and cannot be used
// as a planner start.
Vector2<double> anchor(47.3559582, 8.5192064);
Vector2<double> destination(47.3560100, 8.5192300);
Vector2<double> inside_exclusion(47.3553000, 8.5197000);
static const Vector2<double> vertices[] = {
{47.3552420, 8.5192293},
{47.3555843, 8.5201174},
{47.3551382, 8.5209143},
{47.3550828, 8.5171901},
};
FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
// Seed the planner with the anchor as a known-good fallback start by planning from it:
// an in-fence start is latched internally as the fallback for future calls.
_planner.updateStartAndFillPath(anchor);
// Plan from a position that violates the fence. The planner should detect that the
// provided start has no visible polygon node, fall back to the latched anchor, and
// include the anchor as the first (and only) waypoint since the destination is
// directly reachable from there.
const int num_waypoints = _planner.updateStartAndFillPath(inside_exclusion);
ASSERT_EQ(num_waypoints, 1);
const Vector2d anchor_wp = _planner.waypointAtIndex(0);
EXPECT_NEAR(anchor_wp(0), anchor(0), 1e-3);
EXPECT_NEAR(anchor_wp(1), anchor(1), 1e-3);
}
TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination)
{
using namespace matrix;
Vector2<double> valid(47.3977, 8.5456);
Vector2<double> nan_lat(NAN, 8.5456);
Vector2<double> nan_lon(47.3977, NAN);
FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
_planner.updateDestination(d);
return _planner.updateStartAndFillPath(s);
};
EXPECT_EQ(plan(nan_lat, valid), 0);
EXPECT_EQ(plan(nan_lon, valid), 0);
EXPECT_EQ(plan(valid, nan_lat), 0);
EXPECT_EQ(plan(valid, nan_lon), 0);
}
TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds)
{
using namespace matrix;
Vector2<double> valid(47.3977, 8.5456);
Vector2<double> lat_too_high(91.0, 8.5456);
Vector2<double> lat_too_low(-91.0, 8.5456);
Vector2<double> lon_too_high(47.3977, 181.0);
Vector2<double> lon_too_low(47.3977, -181.0);
FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
_planner.updateDestination(d);
return _planner.updateStartAndFillPath(s);
};
EXPECT_EQ(plan(lat_too_high, valid), 0);
EXPECT_EQ(plan(lat_too_low, valid), 0);
EXPECT_EQ(plan(lon_too_high, valid), 0);
EXPECT_EQ(plan(lon_too_low, valid), 0);
EXPECT_EQ(plan(valid, lat_too_high), 0);
EXPECT_EQ(plan(valid, lat_too_low), 0);
EXPECT_EQ(plan(valid, lon_too_high), 0);
EXPECT_EQ(plan(valid, lon_too_low), 0);
}
TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex)
{
using namespace matrix;
// WE HAVE start end end inside the inclusion fence
Vector2<double> start(47.3560041, 8.5191872);
Vector2<double> destination(47.3553241, 8.5177686);
// BUT we have a fence with a duplicate vertex
static const Vector2<double> vertices[] = {
{47.3562241, 8.5186720},
{47.3562479, 8.5195691},
{47.3550948, 8.5196762},
{47.3549660, 8.5171633},
{47.3555168, 8.5171278},
{47.3554507, 8.5187742},
{47.3554507, 8.5187742}, // last vertex is a duplicate of the previous one
};
FakeGeofence fake(vertices, 7, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
const int num_waypoints = _planner.updateStartAndFillPath(start);
// THEN the pathplanner should fail, printing:
// WARN [geofence_avoidance_planner] geofence avoidance: polygon 0 (7 vertices) rejected by planner
ASSERT_EQ(num_waypoints, 0);
}
TEST_F(GeofenceAvoidancePlannerTest, CursorInterface)
{
using namespace matrix;
// Reuse the exclusion-zone scenario which produces a 2-waypoint path.
Vector2<double> start(47.3559582, 8.5192064);
Vector2<double> destination(47.3546153, 8.5193195);
static const Vector2<double> vertices[] = {
{47.3552420, 8.5192293},
{47.3555843, 8.5201174},
{47.3551382, 8.5209143},
{47.3550828, 8.5171901},
};
FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
_planner.updateGraphFromGeofence(fake, 0.f);
_planner.updateDestination(destination);
_planner.updateStartAndFillPath(start);
ASSERT_EQ(_planner.get_num_waypoints(), 2);
// --- cursor starts at 0 ---
EXPECT_TRUE(_planner.hasMore());
EXPECT_EQ(_planner.getPathCursor(), 0);
// getCurrentWaypoint() and waypointAtIndex(0) agree
const Vector2d cur0 = _planner.getCurrentWaypoint();
const Vector2d idx0 = _planner.waypointAtIndex(0);
EXPECT_NEAR(cur0(0), idx0(0), 1e-6);
EXPECT_NEAR(cur0(1), idx0(1), 1e-6);
// getNextWaypoint() matches waypointAtIndex(1)
const Vector2d nxt0 = _planner.getNextWaypoint();
const Vector2d idx1 = _planner.waypointAtIndex(1);
EXPECT_NEAR(nxt0(0), idx1(0), 1e-6);
EXPECT_NEAR(nxt0(1), idx1(1), 1e-6);
// --- advance to cursor 1 ---
_planner.advanceWaypoint();
EXPECT_TRUE(_planner.hasMore());
EXPECT_EQ(_planner.getPathCursor(), 1);
// current is now the second waypoint
const Vector2d cur1 = _planner.getCurrentWaypoint();
EXPECT_NEAR(cur1(0), idx1(0), 1e-6);
EXPECT_NEAR(cur1(1), idx1(1), 1e-6);
// no next after the last waypoint
EXPECT_FALSE(_planner.getNextWaypoint().isAllFinite());
// --- advance past the end ---
_planner.advanceWaypoint();
EXPECT_FALSE(_planner.hasMore());
EXPECT_FALSE(_planner.getCurrentWaypoint().isAllFinite());
EXPECT_EQ(_planner.getPathCursor(), 2);
}

366
src/modules/navigator/RTLPlanner/geofence_avoidance_planner.cpp Normal file
View File

@@ -0,0 +1,366 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "geofence_avoidance_planner.h"
#include <lib/geo/geo.h>
#include <lib/geofence/geofence_utils.h>
#include <lib/dijkstra/dijkstra.h>
GeofenceAvoidancePlanner::~GeofenceAvoidancePlanner()
{
perf_free(_update_polygons_perf);
perf_free(_update_edge_costs_perf);
perf_free(_plan_path_perf);
perf_free(_lookup_path_perf);
}
static matrix::Vector2f get_vertex_local_position(int poly_index, int vertex_idx,
GeofenceInterface &geofence,
const matrix::Vector2<double> &reference)
{
matrix::Vector2<double> latlon = geofence.getPolygonVertexByIndex(poly_index, vertex_idx);
MapProjection ref{reference(0), reference(1)};
matrix::Vector2f local;
ref.project(latlon(0), latlon(1), local(0), local(1));
return local;
}
void GeofenceAvoidancePlanner::planPath()
{
if (!_polygons_healthy || !_destination_healthy) {
// Do not change _status, it already reflects the reason for the unhealthy state
return;
}
perf_begin(_plan_path_perf);
const bool ret = dijkstra::solveBackward(_polygons.numNodes(), _polygons.destIndex(), _distances, true, _best_distance,
_next_node_buffer, _visited_buffer);
perf_end(_plan_path_perf);
_status = ret ? Status::Success : Status::DijkstraFailed;
}
void GeofenceAvoidancePlanner::updateGraphFromGeofence(GeofenceInterface &geofence, float margin)
{
// Polygons are about to change; any previously latched fallback start may no longer be valid.
_saved_valid_start = matrix::Vector2<double> {(double)NAN, (double)NAN};
_polygons_healthy = true;
const int num_polygons = geofence.getNumPolygons();
int num_vertices{0};
// Before copying the information, sanity check if we have space
for (int poly_idx = 0; poly_idx < num_polygons; poly_idx++) {
PolygonInfo info = geofence.getPolygonInfoByIndex(poly_idx);
if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
// Worst case: the maximum possible number of corners are sharp and have to be split
num_vertices += geofence_utils::maxVerticesAfterSplitting(info.vertex_count);
} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
num_vertices += geofence_utils::PlannerPolygons::kCircleApproxVertices;
}
}
if (num_vertices == 0) {
_polygons_healthy = false;
_status = Status::NoFence;
return;
}
if (num_vertices > kMaxNodes - 1) { // -1 to reserve the destination slot
// Fence larger than the per-board budget (kMaxNodes). On boards where
// kMaxNodes >= kMaxNodesForAnyStorableFence this cannot happen.
_polygons_healthy = false;
_status = Status::BudgetExceeded;
return;
}
_reference = geofence.getPolygonVertexByIndex(0, 0);
if (!updatePolygonsFromGeofence(geofence, margin)) {
_polygons_healthy = false;
return;
}
updateEdgeCosts();
// The graph has changed, so we need to re-plan.
_status = Status::Success;
planPath();
}
bool GeofenceAvoidancePlanner::updatePolygonsFromGeofence(
GeofenceInterface &geofence, float margin)
{
perf_begin(_update_polygons_perf);
const int num_polygons = geofence.getNumPolygons();
_polygons.reset();
for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
PolygonInfo info = geofence.getPolygonInfoByIndex(poly_index);
auto add_result = geofence_utils::PlannerPolygons::AddResult::Success;
if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
// Could skip this local copy and pass e.g. a reference to geofence into addPolygon, so it could access directly
matrix::Vector2f local_in[info.vertex_count];
for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, _reference);
}
const bool is_inclusion = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
add_result = _polygons.addPolygon(local_in, info.vertex_count, is_inclusion, margin);
} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference);
const bool is_inclusion = (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION);
add_result = _polygons.addApproxCircle(center, info.circle_radius, margin, is_inclusion);
}
switch (add_result) {
case geofence_utils::PlannerPolygons::AddResult::Success:
break;
case geofence_utils::PlannerPolygons::AddResult::BudgetExceeded:
_status = Status::BudgetExceeded;
perf_cancel(_update_polygons_perf);
return false;
case geofence_utils::PlannerPolygons::AddResult::OutOfRange:
_status = Status::OutOfRange;
perf_cancel(_update_polygons_perf);
return false;
case geofence_utils::PlannerPolygons::AddResult::Degenerate:
_status = Status::Degenerate;
perf_cancel(_update_polygons_perf);
return false;
}
}
perf_end(_update_polygons_perf);
return true;
}
void GeofenceAvoidancePlanner::updateEdgeCosts()
{
perf_begin(_update_edge_costs_perf);
// All edges in the upper triangle, INCLUDING destination-incident ones (i==0).
// Polygon vertices occupy indices 1..numNodes()-1; destination is at 0.
for (int i = 0; i < _polygons.numNodes(); i++) {
for (int j = i + 1; j < _polygons.numNodes(); j++) {
const size_t idx = dijkstra::symmetricPairIndex(i, j, _polygons.numNodes());
_distances[idx] = _polygons.edgeCost(i, j);
}
}
perf_end(_update_edge_costs_perf);
}
void GeofenceAvoidancePlanner::updateDestination(const matrix::Vector2d &destination)
{
if (!_polygons_healthy) {
// Polygons unhealthy -- _status already reflects the root cause, do not overwrite.
return;
}
if (!destination.isAllFinite() || !latLonWithinBounds(destination)) {
_destination_healthy = false;
_status = Status::DestinationInvalid;
return;
}
MapProjection ref{_reference(0), _reference(1)};
matrix::Vector2f dest_local;
ref.project(destination(0), destination(1), dest_local(0), dest_local(1));
if (!geofence_utils::inFixedPointRange(dest_local(0))
|| !geofence_utils::inFixedPointRange(dest_local(1))) {
_destination_healthy = false;
_status = Status::DestinationInvalid;
return;
}
// PlannerPolygons stores positions in int32-cm, so a setDestination/getDestination
// roundtrip introduces up to 0.5cm of error -- hence the comparison tolerance.
if ((dest_local - _polygons.getDestination()).norm() < 0.1f) {
return;
}
_destination_healthy = _polygons.setDestination(dest_local);
if (!_destination_healthy) {
// Keep the (breaching) destination so RTL can still fall back to a straight
// line; just flag it for the user-facing warning.
_status = Status::DestinationBreachesGeofence;
return;
}
// Destination changed -- only the destination-incident edges (node 0) need
// refreshing; polygon-polygon edges are independent of the destination. This is
// linear in the number of nodes, unlike the quadratic full rebuild, so it is not
// worth a perf counter.
for (int j = 1; j < _polygons.numNodes(); j++) {
_distances[dijkstra::symmetricPairIndex(0, j, _polygons.numNodes())] = _polygons.edgeCost(0, j);
}
planPath();
}
int GeofenceAvoidancePlanner::findBestStartingNode(const matrix::Vector2f &start_local,
bool &destination_directly_reachable) const
{
destination_directly_reachable = false;
float best_cost = INFINITY;
int best_index = -1;
for (int node_idx = 0; node_idx < _polygons.numNodes(); node_idx++) {
if (!_polygons.edgeVisible(start_local, node_idx)) {
continue;
}
if (node_idx == _polygons.destIndex()) {
destination_directly_reachable = true;
continue;
}
const float cost = (start_local - _polygons.node(node_idx)).norm() + _best_distance[node_idx];
if (cost < best_cost) {
best_cost = cost;
best_index = node_idx;
}
}
return best_index;
}
int GeofenceAvoidancePlanner::updateStartAndFillPath(matrix::Vector2d start)
{
// Populate _path so consumers can blindly follow it:
// - append the saved valid start (anchor) if the current `start` is outside the fence
// - then fill the RTL return path by walking the DAG produced by the dijkstra solver
// Could also avoid storing _path entirely and build it ad-hoc from _next_node_buffer.
// Pro: less state, easier implementation of replanning on fence change during RTL
// Con: introduces a lot of new edge cases (safe anchoring / replanning conflict somewhat)
int path_index = 0; // points to the first empty slot, increment after writing
if (!_polygons_healthy || !_destination_healthy) {
_path_length = 0;
_path_cursor = 0;
_straight_line_fallback = true;
return 0;
}
perf_begin(_lookup_path_perf);
MapProjection ref{_reference(0), _reference(1)};
matrix::Vector2f start_local;
ref.project(start(0), start(1), start_local(0), start_local(1));
// Default: plan from given start
bool direct_path_feasible = false;
int best_starting_index = findBestStartingNode(start_local, direct_path_feasible);
const bool path_feasible = best_starting_index >= 0 || direct_path_feasible;
// Save for future fallback
if (path_feasible) {
_saved_valid_start = start;
}
// If planning failed, use previous fallback
if (!path_feasible && _saved_valid_start.isAllFinite()) {
matrix::Vector2f fallback_local;
ref.project(_saved_valid_start(0), _saved_valid_start(1), fallback_local(0), fallback_local(1));
const int fallback_best = findBestStartingNode(fallback_local, direct_path_feasible);
if (fallback_best >= 0 || direct_path_feasible) {
start_local = fallback_local;
best_starting_index = fallback_best;
}
}
// Start with the last valid start if outside the fence and a path exists.
// EXCEPT when no path was found (no point flying to an out-of-fence position).
const bool start_with_anchor = !path_feasible && (best_starting_index >= 0 || direct_path_feasible);
if (start_with_anchor) {
ref.reproject(start_local(0), start_local(1), _path[path_index](0), _path[path_index](1));
path_index++;
}
if (!direct_path_feasible && best_starting_index >= 0) {
int node = best_starting_index;
for (int idx = 0; idx < _polygons.numNodes(); idx++) {
const int next = _next_node_buffer[node];
const matrix::Vector2f p = _polygons.node(node);
ref.reproject(p(0), p(1), _path[path_index](0), _path[path_index](1));
path_index++;
if (next == _polygons.destIndex() || next < 0) {
break;
}
node = next;
}
}
_path_length = path_index;
_path_cursor = 0;
// No path from current position or saved anchor, and destination not
// directly reachable from either. Fall back to flying directly
_straight_line_fallback = (_path_length == 0) && !direct_path_feasible;
perf_end(_lookup_path_perf);
return path_index;
}

196
src/modules/navigator/RTLPlanner/geofence_avoidance_planner.h Normal file
View File

@@ -0,0 +1,196 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file geofence_avoidance_planner.h
* Ensures vehicle waypoints during RTL and autonomous modes remain
* inside inclusion fences and outside exclusion fences.
*/
#include <cstdint>
#include <matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/geofence/geofence_utils.h>
#include "geofence_interface.h"
class GeofenceAvoidancePlanner
{
public:
GeofenceAvoidancePlanner() = default;
~GeofenceAvoidancePlanner();
/**
* Result of the latest graph build & dijkstra run.
* Is converted to user-facing warning in navigator_main.
*/
enum class Status {
Success, // No error
NoFence, // No fence polygons -- avoidance not applicable.
DijkstraFailed, // Dijkstra solve exited due to invalid input data.
DestinationInvalid, // Destination lat/lon non-finite, out of [-90,90]/[-180,180], or out of fixed-point range (internal).
DestinationBreachesGeofence, // Destination breaches geofence
BudgetExceeded, // Node/polygon storage budget exceeded (should not normally happen, see static_assert).
OutOfRange, // A zone's vertex/extent fell outside the usable fixed-point range.
Degenerate, // A zone was degenerate (< 3 vertices, self-intersecting, antiparallel/zero-length edge, empty circle, negative margin).
};
Status status() const { return _status; }
void resetStatus() { _status = Status::Success; }
/**
* True if the latest updateStartAndFillPath() found neither a routed path nor a direct
* line to the destination -- the caller will fly directly and ignore geofences.
*/
bool needsStraightLineFallback() const { return _straight_line_fallback; }
/**
* Fill the path from `start` to the destination. If `start` is
* breaching a (margin-expanded) geofence, start with the last
* non-breaching vehicle position.
*
* Does not re-plan - that only happens after updateGraphFromGeofence
* and updateDestination.
*
* The resulting path is available for external consumers through
* getCurrentWaypoint, getNextWaypoint, advanceWaypoint.
*
* @return Number of waypoints in the path. Only used in tests.
*/
int updateStartAndFillPath(matrix::Vector2d start);
// --- Cursor-based path-following interface ---
// Call set_start_and_plan_path_to_destination() first to plan; it resets the cursor to 0.
// True while there are waypoints left to fly.
bool hasMore() const { return _path_cursor < _path_length; }
// The waypoint to fly to now. NaN if hasMore() is false.
matrix::Vector2d getCurrentWaypoint() const
{
return hasMore() ? _path[_path_cursor] : matrix::Vector2d{(double)NAN, (double)NAN};
}
// The waypoint after current (for populating triplet.next). NaN if current is the last.
matrix::Vector2d getNextWaypoint() const
{
return (_path_cursor + 1 < _path_length) ? _path[_path_cursor + 1] : matrix::Vector2d{(double)NAN, (double)NAN};
}
// Mark current waypoint reached; advance to next. No-op when hasMore() is false.
void advanceWaypoint() { if (_path_cursor < _path_length) { ++_path_cursor; } }
// Number of waypoints to fly.
int get_num_waypoints() const { return _path_length; }
// 0-indexed access to waypoints. Primarily used by time estimators.
matrix::Vector2d waypointAtIndex(int index) const
{
if (index < 0 || index >= _path_length) {
return matrix::Vector2d{(double)NAN, (double)NAN};
}
return _path[index];
}
// Current cursor position (0-indexed).
int getPathCursor() const { return _path_cursor; }
void updateGraphFromGeofence(GeofenceInterface &geofence, float margin = 10.0f);
void updateDestination(const matrix::Vector2d &destination);
private:
static constexpr int kMaxNodes = geofence_utils::PlannerPolygons::kMaxNodes;
static constexpr int num_distances_in_graph = kMaxNodes * (kMaxNodes - 1) / 2;
float _best_distance[kMaxNodes];
float _distances[num_distances_in_graph];
int _next_node_buffer[kMaxNodes];
bool _visited_buffer[kMaxNodes];
// Stored flat path. Worst case: 1 anchor + kMaxNodes DAG vertices.
matrix::Vector2d _path[kMaxNodes + 1];
int _path_length{0};
int _path_cursor{0};
matrix::Vector2<double> _reference; // lat/lon anchor of the local frame
// Cached fence representation.
// - Stores polygons plus safety margin in fixed-point (for robust geometry calculations)
// - Abstracts away geometry, provides edge cost between any two nodes
// - Updated on updateGraphFromGeofence (through updatePolygonsFromGeofence), otherwise read only
geofence_utils::PlannerPolygons _polygons;
bool _polygons_healthy{false};
bool _destination_healthy{false};
Status _status{Status::NoFence};
bool _straight_line_fallback{false};
// Most recent in-fence position passed to updateStartAndFillPath. Used
// by subsequent calls as fallback when we have breached a geofence.
matrix::Vector2<double> _saved_valid_start{(double)NAN, (double)NAN};
perf_counter_t _update_polygons_perf{perf_alloc(PC_ELAPSED, "rtl_planner: polygons")};
perf_counter_t _update_edge_costs_perf{perf_alloc(PC_ELAPSED, "rtl_planner: edge costs")};
perf_counter_t _plan_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: plan path")};
perf_counter_t _lookup_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: lookup path")};
// Sets _status on failure (matching planPath); returns false to abort the build.
bool updatePolygonsFromGeofence(GeofenceInterface &geofence, float margin);
void updateEdgeCosts();
void planPath();
static bool latLonWithinBounds(const matrix::Vector2<double> &lat_lon)
{
return lat_lon(0) >= -90.0 && lat_lon(0) <= 90.0
&& lat_lon(1) >= -180.0 && lat_lon(1) <= 180.0;
}
/**
* @brief Search for the best polygon node visible from start_local.
*
* @param start_local Candidate start in the planner's local frame.
* @param destination_directly_reachable Set true if the destination itself is visible from
* start_local (no detour needed).
* @return Index of the cheapest visible intermediate node (>= 1), or -1 if none is visible.
* Note: destination_directly_reachable being true with a returned -1 means the
* destination is reachable but no intermediate node is cheaper.
*/
int findBestStartingNode(const matrix::Vector2f &start_local, bool &destination_directly_reachable) const;
};

26
src/modules/navigator/RTLPlanner/geofence_interface.h Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include <cstdint>
#include <matrix/math.hpp>
#include "../navigation.h"
struct PolygonInfo {
uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
uint16_t dataman_index;
union {
uint16_t vertex_count;
float circle_radius;
};
};
class GeofenceInterface
{
public:
virtual ~GeofenceInterface() = default;
virtual PolygonInfo getPolygonInfoByIndex(int index) = 0;
virtual matrix::Vector2<double> getPolygonVertexByIndex(int poly_idx, int vertex_idx) = 0;
virtual int getNumPolygons() const = 0;
};

23
src/modules/navigator/geofence.cpp
View File

@@ -48,6 +48,7 @@
#include <dataman_client/DatamanClient.hpp>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/geofence/geofence_utils.h>
#include <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h>
@@ -183,6 +184,8 @@ void Geofence::run()
status.status = geofence_status_s::GF_STATUS_READY;
_geofence_status_pub.publish(status);
_geofence_updated = true;
}
break;
@@ -272,7 +275,7 @@ void Geofence::_updateFence()
} else {
polygon.vertex_count = mission_fence_point.vertex_count;
current_seq += mission_fence_point.vertex_count;
current_seq += polygon.vertex_count;
}
// check if requiremetns for Home location are met
@@ -734,3 +737,21 @@ void Geofence::printStatus()
PX4_INFO("Geofence: polygons: %i inclusion, %i exclusion, %i vertices; circles: %i inclusion, %i exclusion",
num_inclusion_polygons, num_exclusion_polygons, total_num_vertices, num_inclusion_circles, num_exclusion_circles);
}
matrix::Vector2<double>Geofence::getPolygonVertexByIndex(int poly_idx, int idx)
{
PolygonInfo info = _polygons[poly_idx];
mission_fence_point_s vertex{};
dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
const bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + idx,
reinterpret_cast<uint8_t *>(&vertex),
sizeof(mission_fence_point_s));
if (!success) {
return matrix::Vector2<double> {(double)NAN, (double)NAN};
}
return matrix::Vector2d {vertex.lat, vertex.lon};
}

32
src/modules/navigator/geofence.h
View File

@@ -53,6 +53,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_gps.h>
#include "./RTLPlanner/geofence_interface.h"
#if defined(PX4_STORAGEDIR)
#define GEOFENCE_FILENAME PX4_STORAGEDIR"/etc/geofence.txt"
@@ -62,7 +63,8 @@
class Navigator;
class Geofence : public ModuleParams
class Geofence : public ModuleParams, public GeofenceInterface
{
public:
Geofence(Navigator *navigator);
@@ -145,6 +147,18 @@ public:
bool isHomeRequired();
/**
* Returns true once after the polygon set changes (e.g. dataman finished loading a new fence),
* then clears the flag. Navigator polls this to know when to rebuild the avoidance planner's
* vertex graph.
*/
bool consumeFenceUpdated()
{
const bool updated = _geofence_updated;
_geofence_updated = false;
return updated;
}
/**
* print Geofence status to the console
*/
@@ -160,15 +174,6 @@ private:
Error
};
struct PolygonInfo {
uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
uint16_t dataman_index;
union {
uint16_t vertex_count;
float circle_radius;
};
};
Navigator *_navigator{nullptr};
PolygonInfo *_polygons{nullptr};
@@ -188,6 +193,7 @@ private:
uint32_t _opaque_id{0}; ///< dataman geofence id: if it does not match, the polygon data was updated
bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache
bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed
bool _geofence_updated{false}; ///< set when polygons change, consumed by Navigator to rebuild avoidance graph
uORB::Publication<geofence_status_s> _geofence_status_pub{ORB_ID(geofence_status)};
@@ -229,6 +235,12 @@ private:
*/
bool checkCurrentPositionRequirementsForGeofence(const PolygonInfo &polygon);
PolygonInfo getPolygonInfoByIndex(int index) override { return _polygons[index]; }
matrix::Vector2<double>getPolygonVertexByIndex(int poly_idx, int idx) override;
int getNumPolygons() const override { return _num_polygons; }
DEFINE_PARAMETERS(
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
(ParamInt<px4::params::GF_SOURCE>) _param_gf_source,

22
src/modules/navigator/navigator.h
View File

@@ -56,6 +56,10 @@
#include "navigation.h"
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include "RTLPlanner/geofence_avoidance_planner.h"
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/telemetry_status.h>
@@ -192,13 +196,23 @@ public:
Geofence &get_geofence() { return _geofence; }
float get_default_loiter_rad() { return fabsf(_param_nav_loiter_rad.get()); }
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
GeofenceAvoidancePlanner &get_geofence_avoidance_planner() { return _geofence_avoidance_planner; }
/**
* Margin (m) by which the geofence avoidance planner shrinks inclusion
* polygons and expands exclusion polygons.
*/
float geofence_avoidance_margin() const;
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
float get_default_loiter_rad() const { return fabsf(_param_nav_loiter_rad.get()); }
bool get_default_loiter_CCW() { return _param_nav_loiter_rad.get() < -FLT_EPSILON; }
/**
* Returns the default acceptance radius defined by the parameter
*/
float get_default_acceptance_radius();
float get_default_acceptance_radius() const;
/**
* Get the acceptance radius
@@ -371,6 +385,10 @@ private:
hrt_abstime _last_geofence_check{0};
bool _geofence_reposition_sent{false}; /**< true if a reposition triplet has been sent for the current breach */
hrt_abstime _time_loitering_after_gf_breach{0}; /**< latches breach state while loitering, prevents reposition center walking */
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
GeofenceAvoidancePlanner _geofence_avoidance_planner; /**< RTL/auto path planner that routes around fences (visibility graph + Dijkstra) */
float _last_geofence_avoidance_margin{NAN}; /**< margin used for the last polygon rebuild; rebuild when it changes */
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
bool _navigator_status_updated{false};
hrt_abstime _last_navigator_status_publication{0};

86
src/modules/navigator/navigator_main.cpp
View File

@@ -996,6 +996,70 @@ void Navigator::run()
_geofence.run();
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
const bool fence_updated = _geofence.consumeFenceUpdated();
const float margin = geofence_avoidance_margin();
// Margin is baked into polygons, so a margin change requires rebuilding them.
const bool margin_changed = fabsf(margin - _last_geofence_avoidance_margin) > FLT_EPSILON;
using PlannerStatus = GeofenceAvoidancePlanner::Status;
if (fence_updated || margin_changed) {
_geofence_avoidance_planner.updateGraphFromGeofence(_geofence, margin);
const PlannerStatus planner_status = _geofence_avoidance_planner.status();
_last_geofence_avoidance_margin = margin;
// Add granularity with more status values / user messages if needed.
switch (planner_status) {
// Failure in building fence graph / path. Collapse to one generic user message. Add granularity if needed.
case PlannerStatus::BudgetExceeded: // TODO make this more specific now that it is more likely
case PlannerStatus::OutOfRange:
case PlannerStatus::Degenerate:
case PlannerStatus::DijkstraFailed:
mavlink_log_warning(&_mavlink_log_pub, "Geofence data invalid (code %d), RTL will fly directly\t",
(int) planner_status);
events::send<uint8_t>(events::ID("rtl_avoidance_build_failed"), {events::Log::Warning, events::LogInternal::Info},
"Geofence data invalid (code {1}), RTL will fly directly", (uint8_t)planner_status);
break;
default:
// Not an error, or reported elsewhere
break;
}
} else {
// Signal status values not related to updating geofence data. Reset status to not spam.
const PlannerStatus planner_status = _geofence_avoidance_planner.status();
if (planner_status == PlannerStatus::DestinationInvalid) {
mavlink_log_warning(&_mavlink_log_pub, "RTL destination invalid, not updating\t");
events::send(
events::ID("rtl_destination_invalid"),
events::LogLevels(events::Log::Warning, events::LogInternal::Info),
"RTL destination invalid, not updating"
);
_geofence_avoidance_planner.resetStatus();
}
if (planner_status == PlannerStatus::DestinationBreachesGeofence) {
mavlink_log_warning(&_mavlink_log_pub, "RTL destination breaches geofence, will fly directly\t");
events::send(
events::ID("rtl_destination_breaches"),
events::LogLevels(events::Log::Warning, events::LogInternal::Info),
"RTL destination breaches geofence, will fly directly"
);
_geofence_avoidance_planner.resetStatus();
}
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
perf_end(_loop_perf);
}
}
@@ -1137,7 +1201,7 @@ void Navigator::publish_position_setpoint_triplet()
_pos_sp_triplet_updated = false;
}
float Navigator::get_default_acceptance_radius()
float Navigator::get_default_acceptance_radius() const
{
return _param_nav_acc_rad.get();
}
@@ -1610,6 +1674,26 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
return true;
}
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
float Navigator::geofence_avoidance_margin() const
{
// These margins should be above the horizontal tracking error that can be expected for each vehicle type.
// If re-using the enlarged polygons for a predictive geofence failsafe feature, additionally ensure
// that the margin is large enough to turn around / stop / carry out the desired failsafe action in time.
if (_vstatus.is_vtol || _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// Use FW loiter radius even for VTOL in MC -- changing the
// margin on transition is confusing and RTLing in MC as a VTOL
// is a very rare edge case
return get_default_loiter_rad();
}
// MC, rover, unspecified
return 2.0f * get_default_acceptance_radius();
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
void Navigator::preproject_stop_point(double &lat, double &lon)
{
// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back

29
src/modules/navigator/rtl.cpp
View File

@@ -403,6 +403,35 @@ void RTL::setRtlTypeAndDestination()
_rtl_type = new_rtl_type;
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
// Update destination of geofence avoidance planner. Depending on the
// RTL type it is the position of the loiter or mission landing.
GeofenceAvoidancePlanner &planner = _navigator->get_geofence_avoidance_planner();
matrix::Vector2d planner_destination{(double)NAN, (double)NAN};
if (new_rtl_type == RtlType::RTL_DIRECT) {
planner_destination = matrix::Vector2d{landing_loiter.lat, landing_loiter.lon};
} else if (new_rtl_type == RtlType::RTL_DIRECT_MISSION_LAND && _rtl_mission_type_handle) {
planner_destination = _rtl_mission_type_handle->getRtlPlannerDestination();
}
if (planner_destination.isAllFinite()) {
planner.updateDestination(planner_destination);
}
const vehicle_global_position_s gpos = _global_pos_sub.get();
const bool gpos_recent = gpos.timestamp > 0 && hrt_elapsed_time(&gpos.timestamp) < 10_s;
if (gpos_recent) {
planner.updateStartAndFillPath(matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon));
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
// Publish rtl status
rtl_status_s rtl_status{};
rtl_status.safe_points_id = _safe_points_id;

5
src/modules/navigator/rtl_base.h
View File

@@ -41,6 +41,7 @@
#include "mission_base.h"
#include <uORB/topics/rtl_time_estimate.h>
#include <matrix/math.hpp>
class RtlBase : public MissionBase
{
@@ -54,4 +55,8 @@ public:
virtual void setReturnAltMin(bool min) { (void)min;};
virtual void setRtlAlt(float alt) { (void)alt;};
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
virtual matrix::Vector2d getRtlPlannerDestination() { return {(double)NAN, (double)NAN}; }
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
};

118
src/modules/navigator/rtl_direct.cpp
View File

@@ -45,6 +45,9 @@
#include "rtl_direct.h"
#include "mission_item_utils.h"
#include "navigator.h"
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include "rtl_geofence_avoidance_helper.h"
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
@@ -89,6 +92,20 @@ void RtlDirect::on_activation()
events::send<int32_t, int32_t>(events::ID("vrtl_return_at"), events::Log::Info,
"RTL: start return at {1m_v} ({2m_v} above destination)",
(int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
if (geofenceAvoidanceActive()) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: avoiding geofence\t");
events::send(events::ID("rtl_avoiding_geofence"), events::Log::Info, "RTL: avoiding geofence");
} else if (_navigator->get_geofence_avoidance_planner().needsStraightLineFallback()) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: no geofence avoidance path; flying directly\t");
events::send(events::ID("rtl_avoidance_runtime_fallback"), {events::Log::Critical, events::LogInternal::Error},
"RTL: no geofence avoidance path; flying directly");
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
}
void RtlDirect::on_active()
@@ -147,6 +164,15 @@ void RtlDirect::setRtlPosition(const PositionYawSetpoint &rtl_position, const lo
}
}
bool RtlDirect::geofenceAvoidanceActive() const
{
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
return _navigator->get_geofence_avoidance_planner().hasMore();
#else
return false;
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
}
void RtlDirect::_updateRtlState()
{
// RTL_LAND_DELAY > 0 -> wait seconds, < 0 wait indefinitely
@@ -157,7 +183,8 @@ void RtlDirect::_updateRtlState()
switch (_rtl_state) {
case RTLState::CLIMBING:
new_state = RTLState::MOVE_TO_LOITER;
case RTLState::AVOID_GEOFENCE:
new_state = geofenceAvoidanceActive() ? RTLState::AVOID_GEOFENCE : RTLState::MOVE_TO_LOITER;
break;
case RTLState::MOVE_TO_LOITER:
@@ -210,7 +237,6 @@ void RtlDirect::_updateRtlState()
_rtl_state = new_state;
}
void RtlDirect::set_rtl_item()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -236,7 +262,71 @@ void RtlDirect::set_rtl_item()
break;
}
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
case RTLState::AVOID_GEOFENCE: {
GeofenceAvoidancePlanner &planner = _navigator->get_geofence_avoidance_planner();
matrix::Vector2d point = planner.getCurrentWaypoint();
const matrix::Vector2d next_point = planner.getNextWaypoint();
const bool is_first_waypoint = 0 == planner.getPathCursor();
planner.advanceWaypoint();
if (!point.isAllFinite()) {
// Should never happen -- AVOID_GEOFENCE is only entered while hasMore() is true.
// Fall back to RTLing in a straight line
point(0) = _land_approach.lat;
point(1) = _land_approach.lon;
}
float yaw = NAN;
if (!_param_wv_en.get() && point.isAllFinite()) {
yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
point(0), point(1));
}
PositionYawSetpoint pos_yaw_sp {
.lat = point(0),
.lon = point(1),
.alt = _rtl_alt,
.yaw = yaw,
};
setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
// Line following only between points on the path, not when flying to the first point
if (is_first_waypoint) {
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
} else {
pos_sp_triplet->previous = pos_sp_triplet->current;
}
pos_sp_triplet->next.valid = true;
pos_sp_triplet->next.alt = _rtl_alt;
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
if (next_point.isAllFinite()) {
pos_sp_triplet->next.lat = next_point(0);
pos_sp_triplet->next.lon = next_point(1);
} else {
// Last avoidance waypoint -- land approach is next.
pos_sp_triplet->next.lat = _land_approach.lat;
pos_sp_triplet->next.lon = _land_approach.lon;
}
break;
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
case RTLState::MOVE_TO_LOITER: {
// Shift waypoint to enable line following with respect to last waypoint
pos_sp_triplet->previous = pos_sp_triplet->current;
PositionYawSetpoint pos_yaw_sp {
.lat = _land_approach.lat,
.lon = _land_approach.lon,
@@ -408,6 +498,9 @@ RtlDirect::RTLState RtlDirect::getActivationState()
} else if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
activation_state = RTLState::CLIMBING;
} else if (geofenceAvoidanceActive()) {
activation_state = RTLState::AVOID_GEOFENCE;
} else {
activation_state = RTLState::MOVE_TO_LOITER;
}
@@ -439,6 +532,10 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
const float loiter_altitude = min(land_approach.height_m, _rtl_alt);
// If geofence avoidance, this is set to the last waypoint.
// Otherwise, we move directly from vehicle position to loiter.
matrix::Vector2d pos_before_loiter{_global_pos_sub.get().lat, _global_pos_sub.get().lon};
// Sum up time estimate for various segments of the landing procedure
switch (start_state_for_estimate) {
case RTLState::CLIMBING: {
@@ -448,11 +545,24 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
}
}
// FALLTHROUGH
case RTLState::AVOID_GEOFENCE: {
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
pos_before_loiter = add_geofence_avoidance_path_distance(
_rtl_time_estimator,
_navigator->get_geofence_avoidance_planner(),
matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon)
);
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
}
// FALLTHROUGH
case RTLState::MOVE_TO_LOITER: {
matrix::Vector2f direction{};
get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
land_approach.lon, &direction(0), &direction(1));
get_vector_to_next_waypoint(pos_before_loiter(0), pos_before_loiter(1),
land_approach.lat, land_approach.lon, &direction(0), &direction(1));
float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)};
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {

11
src/modules/navigator/rtl_direct.h
View File

@@ -55,6 +55,9 @@
#include <uORB/topics/wind.h>
#include <lib/rtl/rtl_time_estimator.h>
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include "RTLPlanner/geofence_avoidance_planner.h"
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include "mission_block.h"
#include "navigation.h"
#include "safe_point_land.hpp"
@@ -118,6 +121,7 @@ private:
*/
enum class RTLState {
CLIMBING,
AVOID_GEOFENCE,
MOVE_TO_LOITER,
LOITER_DOWN,
LOITER_HOLD,
@@ -160,6 +164,12 @@ private:
RTLState getActivationState();
/**
* @brief Whether the geofence-avoidance planner currently has a path to fly.
* Always false for build without geofence avoidance (CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE).
*/
bool geofenceAvoidanceActive() const;
void setLoiterPosition();
bool _enforce_rtl_alt{false};
@@ -168,7 +178,6 @@ private:
PositionYawSetpoint _destination{(double)NAN, (double)NAN, NAN, NAN}; ///< the RTL position to fly to
loiter_point_s _land_approach;
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should transit to the destination
DEFINE_PARAMETERS(

111
src/modules/navigator/rtl_direct_mission_land.cpp
View File

@@ -42,6 +42,9 @@
#include "rtl_direct_mission_land.h"
#include "mission_item_utils.h"
#include "navigator.h"
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include "rtl_geofence_avoidance_helper.h"
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
#include <drivers/drv_hrt.h>
@@ -116,6 +119,14 @@ void RtlDirectMissionLand::on_activation()
bool RtlDirectMissionLand::setNextMissionItem()
{
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
// Stay on the same mission item until the geofence-avoidance path is fully consumed.
if (_navigator->get_geofence_avoidance_planner().hasMore()) {
return true;
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
return (goToNextPositionItem() == PX4_OK);
}
@@ -169,6 +180,60 @@ void RtlDirectMissionLand::setActiveMissionItems()
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
} else if (_navigator->get_geofence_avoidance_planner().hasMore()) {
GeofenceAvoidancePlanner &planner = _navigator->get_geofence_avoidance_planner();
matrix::Vector2d point = planner.getCurrentWaypoint();
const matrix::Vector2d next_point = planner.getNextWaypoint();
const bool is_first_waypoint = 0 == planner.getPathCursor();
planner.advanceWaypoint();
if (!point.isAllFinite()) {
// Should never happen -- the geofence branch is only entered while hasMore() is true.
// Fall back to flying straight to the destination, as rtl_direct does.
const matrix::Vector2d destination = getRtlPlannerDestination();
if (destination.isAllFinite()) {
point = destination;
} else {
point(0) = _global_pos_sub.get().lat;
point(1) = _global_pos_sub.get().lon;
}
}
// Line following only between points on the path, not when flying to the first point
if (is_first_waypoint) {
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
} else {
pos_sp_triplet->previous = current_setpoint_copy;
}
_mission_item.lat = point(0);
_mission_item.lon = point(1);
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _navigator->get_default_loiter_rad();
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
// If next point does not exist, we have NaN and inalid next setpoint
pos_sp_triplet->next.valid = next_point.isAllFinite();
pos_sp_triplet->next.alt = _rtl_alt;
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
pos_sp_triplet->next.lat = next_point(0);
pos_sp_triplet->next.lon = next_point(1);
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
} else if (mission_item_contains_position(_mission_item)) {
static constexpr size_t max_num_next_items{1u};
@@ -267,9 +332,19 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
is_in_climbing_submode = checkNeedsToClimb();
}
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
matrix::Vector2d hor_position_at_calculation_point = add_geofence_avoidance_path_distance(
_rtl_time_estimator,
_navigator->get_geofence_avoidance_planner(),
matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon)
);
#else
matrix::Vector2d hor_position_at_calculation_point {_global_pos_sub.get().lat, _global_pos_sub.get().lon};
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
if (start_item_index >= 0 && start_item_index < static_cast<int32_t>(_mission.count)) {
float altitude_at_calculation_point;
matrix::Vector2d hor_position_at_calculation_point{_global_pos_sub.get().lat, _global_pos_sub.get().lon};
if (is_in_climbing_submode) {
if (_enforce_rtl_alt) {
@@ -432,3 +507,37 @@ bool RtlDirectMissionLand::checkNeedsToClimb()
return needs_climbing;
}
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
matrix::Vector2d RtlDirectMissionLand::getRtlPlannerDestination()
{
matrix::Vector2d position((double)NAN, (double)NAN);
if (!hasMissionLandStart()) {
return position;
}
int32_t next_mission_item_index{-1};
size_t num_found_items{0U};
getNextPositionItems(_mission.land_start_index + 1, &next_mission_item_index, num_found_items, 1U);
if (num_found_items == 0U) {
return position;
}
mission_item_s next_position_mission_item{};
const dm_item_t mission_dataman_id = static_cast<dm_item_t>(_mission.mission_dataman_id);
const bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index,
reinterpret_cast<uint8_t *>(&next_position_mission_item),
sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT);
if (!success) {
return position;
}
position(0) = next_position_mission_item.lat;
position(1) = next_position_mission_item.lon;
return position;
}
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE

9
src/modules/navigator/rtl_direct_mission_land.h
View File

@@ -44,6 +44,7 @@
#include "rtl_base.h"
#include <lib/rtl/rtl_time_estimator.h>
#include <matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
@@ -65,6 +66,14 @@ public:
void setReturnAltMin(bool min) override { _enforce_rtl_alt = min; };
void setRtlAlt(float alt) override {_rtl_alt = alt;};
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
/**
* @brief Destination the geofence-avoidance planner should route to: the start of the mission
* landing sequence (first position item after DO_LAND_START). (NaN, NaN) if not yet known.
*/
matrix::Vector2d getRtlPlannerDestination() override;
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;

61
src/modules/navigator/rtl_geofence_avoidance_helper.cpp Normal file
View File

@@ -0,0 +1,61 @@
/***************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "rtl_geofence_avoidance_helper.h"
#include <lib/geo/geo.h>
#include <lib/rtl/rtl_time_estimator.h>
matrix::Vector2d add_geofence_avoidance_path_distance(
RtlTimeEstimator &estimator,
const GeofenceAvoidancePlanner &planner,
const matrix::Vector2d &current_position)
{
if (!planner.hasMore()) {
return current_position;
}
matrix::Vector2d from = current_position;
for (int i = planner.getPathCursor(); i < planner.get_num_waypoints(); i++) {
const matrix::Vector2d to = planner.waypointAtIndex(i);
matrix::Vector2f direction{};
get_vector_to_next_waypoint(from(0), from(1), to(0), to(1), &direction(0), &direction(1));
estimator.addDistance(get_distance_to_next_waypoint(from(0), from(1), to(0), to(1)), direction, 0.f);
from = to;
}
return from;
}

61
src/modules/navigator/rtl_geofence_avoidance_helper.h Normal file
View File

@@ -0,0 +1,61 @@
/***************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rtl_geofence_avoidance_helper.h
*
* Shared helper for accumulating the remaining horizontal legs of a
* geofence-avoidance path into an RtlTimeEstimator.
*/
#pragma once
#include <matrix/math.hpp>
#include "RTLPlanner/geofence_avoidance_planner.h"
class RtlTimeEstimator;
/**
* @brief Accumulate the remaining geofence-avoidance legs into the time estimator.
*
* Iterates from the planner's current cursor position to the end of the materialized path,
* adding each leg from current_position (or the previous waypoint) to the next waypoint.
*
* @param estimator Time estimator to accumulate distances into.
* @param planner The geofence avoidance planner (read-only).
* @param current_position Vehicle's current global position (lat, lon).
* @return Horizontal position at the end of the path, or current_position if the path is empty.
*/
matrix::Vector2d add_geofence_avoidance_path_distance(
RtlTimeEstimator &estimator,
const GeofenceAvoidancePlanner &planner,
const matrix::Vector2d &current_position);

1
test/mavsdk_tests/CMakeLists.txt
View File

@@ -29,6 +29,7 @@ if(MAVSDK_FOUND)
test_multicopter_mission.cpp
test_multicopter_offboard.cpp
test_multicopter_manual.cpp
test_multicopter_rtl_with_geofence.cpp
test_vtol_mission.cpp
test_vtol_figure_eight.cpp
test_vtol_rtl.cpp

8
test/mavsdk_tests/autopilot_tester.cpp
View File

@@ -33,6 +33,7 @@
#include "autopilot_tester.h"
#include "math_helpers.h"
#include <atomic>
#include <iostream>
#include <future>
#include <thread>
@@ -893,12 +894,15 @@ void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_numbe
{
auto prom = std::promise<void> {};
auto fut = prom.get_future();
// Guards against bunched progress events (e.g. under high sim speed factor) firing the
// callback twice before unsubscribe takes effect, which would set the promise twice.
std::atomic<bool> done{false};
MissionRaw::MissionProgressHandle handle = _mission_raw->subscribe_mission_progress(
[&prom, &handle, this, sequence_number](MissionRaw::MissionProgress progress) {
[&prom, &handle, &done, this, sequence_number](MissionRaw::MissionProgress progress) {
std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current >= sequence_number) {
if (progress.current >= sequence_number && !done.exchange(true)) {
_mission_raw->unsubscribe_mission_progress(handle);
prom.set_value();
}

210
test/mavsdk_tests/autopilot_tester_rtl.cpp
View File

@@ -172,6 +172,216 @@ void AutopilotTesterRtl::check_rally_point_within(float acceptance_radius_m)
CHECK(within_rally_point);
}
void AutopilotTesterRtl::load_qgc_mission_and_geofence_here(const std::string &plan_file)
{
auto import_result = getMissionRaw()->import_qgroundcontrol_mission(plan_file);
REQUIRE(import_result.first == mavsdk::MissionRaw::Result::Success);
auto &data = import_result.second;
REQUIRE(!data.mission_items.empty());
// Same offset semantics as move_mission_raw_here, but applied to mission and geofence
// together so the fences keep their plan-relative positions.
const auto pos = getTelemetry()->position();
REQUIRE(std::isfinite(pos.latitude_deg));
REQUIRE(std::isfinite(pos.longitude_deg));
const int32_t offset_x = data.mission_items[0].x - static_cast<int32_t>(1e7 * pos.latitude_deg);
const int32_t offset_y = data.mission_items[0].y - static_cast<int32_t>(1e7 * pos.longitude_deg);
// Only items in a global position frame carry lat/lon in x/y. For MAV_FRAME_MISSION
// items (e.g. RTL, DO_* commands) x/y are generic params p5/p6, so offsetting them
// corrupts the params and the item is rejected by the mission-param validation.
auto is_global_position_frame = [](uint32_t frame) {
switch (frame) {
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
return true;
default:
return false;
}
};
auto apply_offset = [&](std::vector<mavsdk::MissionRaw::MissionItem> &items) {
for (auto &item : items) {
if (!is_global_position_frame(item.frame)) {
continue;
}
item.x -= offset_x;
item.y -= offset_y;
}
};
apply_offset(data.mission_items);
apply_offset(data.geofence_items);
REQUIRE(getMissionRaw()->upload_mission(data.mission_items) == mavsdk::MissionRaw::Result::Success);
_geofence_shapes.clear();
if (!data.geofence_items.empty()) {
REQUIRE(getMissionRaw()->upload_geofence(data.geofence_items) == mavsdk::MissionRaw::Result::Success);
// Parse MAVLink fence items into shape records in local NED.
// Polygon vertex items (5001/5002) are emitted as a run with param1 = vertex count;
// circle items (5003/5004) are one-shot with param1 = radius_m.
constexpr uint16_t kCmdPolygonInclusion = 5001;
constexpr uint16_t kCmdPolygonExclusion = 5002;
constexpr uint16_t kCmdCircleInclusion = 5003;
constexpr uint16_t kCmdCircleExclusion = 5004;
const auto ct = get_coordinate_transformation();
auto flush_open_polygon = [this](GeofenceShape & open) {
if (!open.vertices.empty()) {
_geofence_shapes.push_back(std::move(open));
}
open = GeofenceShape{};
};
GeofenceShape open{};
size_t vertex_target = 0;
for (const auto &item : data.geofence_items) {
const double lat = static_cast<double>(item.x) / 1e7;
const double lon = static_cast<double>(item.y) / 1e7;
const auto local = ct.local_from_global({lat, lon});
if (item.command == kCmdPolygonInclusion || item.command == kCmdPolygonExclusion) {
const auto kind = (item.command == kCmdPolygonInclusion)
? GeofenceShape::Kind::PolygonInclusion
: GeofenceShape::Kind::PolygonExclusion;
// Start a new polygon when kind changes or the previous one is full.
if (open.vertices.empty() || open.kind != kind
|| open.vertices.size() >= vertex_target) {
flush_open_polygon(open);
open.kind = kind;
vertex_target = static_cast<size_t>(item.param1);
}
open.vertices.push_back(local);
} else if (item.command == kCmdCircleInclusion || item.command == kCmdCircleExclusion) {
flush_open_polygon(open);
GeofenceShape circle{};
circle.kind = (item.command == kCmdCircleInclusion)
? GeofenceShape::Kind::CircleInclusion
: GeofenceShape::Kind::CircleExclusion;
circle.center = local;
circle.radius_m = static_cast<double>(item.param1);
_geofence_shapes.push_back(std::move(circle));
}
}
flush_open_polygon(open);
}
sleep_for(std::chrono::seconds(1));
}
void AutopilotTesterRtl::start_monitoring_geofence_breach()
{
REQUIRE(!_geofence_shapes.empty()); // load_qgc_mission_and_geofence_here() must run first
REQUIRE(!_geofence_monitor_active);
auto ct = get_coordinate_transformation();
_geofence_breached.store(false);
_geofence_monitor_active.store(true);
_geofence_monitor_handle = getTelemetry()->subscribe_ground_truth(
[this, ct](Telemetry::GroundTruth gt) {
if (!_geofence_monitor_active.load()) {
return;
}
if (std::isnan(gt.latitude_deg) || std::isnan(gt.longitude_deg)) {
return;
}
const auto local = ct.local_from_global({gt.latitude_deg, gt.longitude_deg});
for (size_t i = 0; i < _geofence_shapes.size(); ++i) {
if (point_breaches_shape(local.north_m, local.east_m, _geofence_shapes[i])) {
if (!_geofence_breached.exchange(true)) {
std::cout << time_str() << "GEOFENCE BREACH (shape #" << i
<< ") at NED (" << local.north_m << ", "
<< local.east_m << ")\n";
}
break;
}
}
});
}
void AutopilotTesterRtl::check_no_geofence_breach()
{
if (_geofence_monitor_active.exchange(false)) {
getTelemetry()->unsubscribe_ground_truth(_geofence_monitor_handle);
}
CHECK(!_geofence_breached.load());
}
bool AutopilotTesterRtl::point_in_polygon_local(
double north_m, double east_m,
const std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> &poly)
{
bool inside = false;
const size_t n = poly.size();
for (size_t i = 0, j = n - 1; i < n; j = i++) {
const double ni = poly[i].north_m;
const double ei = poly[i].east_m;
const double nj = poly[j].north_m;
const double ej = poly[j].east_m;
const bool crosses = ((ei > east_m) != (ej > east_m))
&& (north_m < (nj - ni) * (east_m - ei) / (ej - ei) + ni);
if (crosses) {
inside = !inside;
}
}
return inside;
}
bool AutopilotTesterRtl::point_breaches_shape(double north_m, double east_m, const GeofenceShape &shape)
{
switch (shape.kind) {
case GeofenceShape::Kind::PolygonExclusion:
return shape.vertices.size() >= 3
&& point_in_polygon_local(north_m, east_m, shape.vertices);
case GeofenceShape::Kind::PolygonInclusion:
return shape.vertices.size() >= 3
&& !point_in_polygon_local(north_m, east_m, shape.vertices);
case GeofenceShape::Kind::CircleExclusion: {
const double dn = north_m - shape.center.north_m;
const double de = east_m - shape.center.east_m;
return std::sqrt(dn * dn + de * de) < shape.radius_m;
}
case GeofenceShape::Kind::CircleInclusion: {
const double dn = north_m - shape.center.north_m;
const double de = east_m - shape.center.east_m;
return std::sqrt(dn * dn + de * de) > shape.radius_m;
}
}
return false;
}
void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::chrono::seconds timeout)
{
auto prom = std::promise<bool> {};

35
test/mavsdk_tests/autopilot_tester_rtl.h
View File

@@ -35,11 +35,13 @@
#include "autopilot_tester.h"
#include <atomic>
#include <vector>
#include <mavsdk/mavsdk.h>
#include <mavsdk/geometry.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
class AutopilotTesterRtl : public AutopilotTester
@@ -61,11 +63,44 @@ public:
void check_rtl_approaches(float acceptance_radius_m, std::chrono::seconds timeout);
void upload_rally_points();
// Import a QGC .plan and upload both the mission and any geofence polygons/circles it
// contains. Mission and geofence are shifted by the same offset so the fences stay in
// their plan-relative positions (same semantics as load_qgc_mission_raw_and_move_here).
// All fence shapes (inclusion/exclusion, polygon/circle) are cached in local NED for the
// breach monitor.
void load_qgc_mission_and_geofence_here(const std::string &plan_file);
// Subscribe to ground-truth position and assert (latched) that the vehicle never breaches
// any of the cached fence shapes. A breach is entering an exclusion shape or leaving an
// inclusion shape. Call after load_qgc_mission_and_geofence_here().
void start_monitoring_geofence_breach();
// Unsubscribe and CHECK() that no breach was observed.
void check_no_geofence_breach();
private:
struct GeofenceShape {
enum class Kind { PolygonInclusion, PolygonExclusion, CircleInclusion, CircleExclusion };
Kind kind;
std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> vertices{}; // polygons only
mavsdk::geometry::CoordinateTransformation::LocalCoordinate center{}; // circles only
double radius_m{0.0}; // circles only
};
void add_approaches_to_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate);
static bool point_in_polygon_local(
double north_m, double east_m,
const std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> &poly);
static bool point_breaches_shape(double north_m, double east_m, const GeofenceShape &shape);
std::unique_ptr<mavsdk::Failure> _failure{};
std::vector<mavsdk::MissionRaw::MissionItem> _rally_points{};
std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> _local_rally_points{};
std::vector<GeofenceShape> _geofence_shapes{};
mavsdk::Telemetry::GroundTruthHandle _geofence_monitor_handle{};
std::atomic<bool> _geofence_breached{false};
std::atomic<bool> _geofence_monitor_active{false};
};

189
test/mavsdk_tests/multicopter_mission_geofence_avoid.plan Normal file
View File

@@ -0,0 +1,189 @@
{
"UUID": "024f36c3946f05cba1ef5046e644fc91b6ffd007",
"fileType": "Plan",
"geoFence": {
"circles": [
{
"circle": {
"center": [
47.395306422855064,
8.54286777893347
],
"radius": 111.05361447891983
},
"inclusion": false,
"version": 1
}
],
"polygons": [
{
"inclusion": false,
"polygon": [
[
47.3991036,
8.5368519
],
[
47.3985883,
8.543435299999999
],
[
47.3967667,
8.544253699999999
],
[
47.3968827,
8.541781799999999
]
],
"version": 1
},
{
"inclusion": true,
"polygon": [
[
47.39999163985872,
8.535047740378417
],
[
47.39994510029728,
8.545445143543049
],
[
47.39274987948735,
8.551976999573185
],
[
47.39365751759516,
8.53812458836515
]
],
"version": 1
}
],
"version": 2
},
"groundStation": "QGroundControl",
"mission": {
"cruiseSpeed": 15,
"firmwareType": 12,
"globalPlanAltitudeMode": 1,
"hoverSpeed": 5,
"items": [
{
"AMSLAltAboveTerrain": null,
"Altitude": 20,
"AltitudeMode": 1,
"MISSION_ITEM_ID": "1",
"autoContinue": true,
"command": 22,
"doJumpId": 1,
"frame": 3,
"groupTag": 0,
"params": [
0,
0,
0,
null,
47.3977508,
8.5456074,
20
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 20,
"AltitudeMode": 1,
"MISSION_ITEM_ID": "2",
"autoContinue": true,
"command": 16,
"doJumpId": 2,
"frame": 3,
"groupTag": 0,
"params": [
0,
0,
0,
null,
47.396545599479346,
8.544490781870365,
20
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 20,
"AltitudeMode": 1,
"MISSION_ITEM_ID": "3",
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"groupTag": 0,
"params": [
0,
0,
0,
null,
47.39635128979426,
8.53915466175144,
20
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 20,
"AltitudeMode": 1,
"MISSION_ITEM_ID": "4",
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"groupTag": 0,
"params": [
0,
0,
0,
null,
47.39446602392638,
8.540564444881795,
20
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 20,
"doJumpId": 5,
"frame": 2,
"groupTag": 6,
"params": [
0,
0,
0,
0,
0,
0,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.3977444,
8.5455918,
489.338
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}

73
test/mavsdk_tests/test_multicopter_rtl_with_geofence.cpp Normal file
View File

@@ -0,0 +1,73 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "autopilot_tester_rtl.h"
// Exercises the RTL_DIRECT geofence avoidance planner (visibility graph + Dijkstra).
//
// Setup: mission flies the vehicle to a point from which the straight-line return-to-home would
// cut through an exclusion polygon. The planner must detour around it.
//
// Pass: the vehicle returns to home and disarms within the timeout AND its ground-truth position
// never violates any of the loaded geofences during the entire armed flight.
TEST_CASE("RTL direct with geofence obstruction", "[multicopter]")
{
AutopilotTesterRtl tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
// Mission outbound leg ends at a point where straight-line RTL crosses the exclusion polygon
// in the .plan; the planner must detour around it.
tester.load_qgc_mission_and_geofence_here("test/mavsdk_tests/multicopter_mission_geofence_avoid.plan");
// Warning only: the planner still picks up the fence (it reads polygon geometry independent of
// GF_ACTION), but a hypothetical breach won't trigger Hold/RTL failsafe and mask test failure.
tester.set_param_int("GF_ACTION", 1);
tester.set_rtl_type(0); // RTL_DIRECT
tester.set_rtl_appr_force(0);
tester.sleep_for(std::chrono::seconds(3));
tester.arm();
// Latch a breach detector covering everything from here until disarm.
tester.start_monitoring_geofence_breach();
tester.execute_rtl_when_reaching_mission_sequence(3);
tester.wait_until_disarmed(std::chrono::seconds(400));
tester.check_no_geofence_breach();
tester.check_home_within(5.0f);
}