mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(navigator): check geofence breaches for mission segments (not only waypoints) (#28760)
* feat(navigator): check geofence breaches for mission segments (not only waypoints) * refactor(navigator): move mission path batch off the stack * fix(navigator): keep the mission verdict while the fence reloads * test(navigator): check a mission leg across the antimeridian --------- Co-authored-by: jonas <jonas.perolini@rigi.tech>
This commit is contained in:
1 parent
92b8bc4342
commit
dd804e4b9c
14 files changed
+916
-43
No files matched your search
@@ -57,6 +57,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE=n
|
||||
# CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS is not set
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
|
||||
@@ -45,6 +45,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_PATH_CHECKS is not set
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
|
||||
@@ -44,6 +44,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
# CONFIG_NAVIGATOR_ADSB is not set
|
||||
# CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set
|
||||
# CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS is not set
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
|
||||
@@ -46,6 +46,15 @@ menuconfig NAVIGATOR_GEOFENCE_AVOIDANCE
|
||||
Add support for Return mode to route around geofences.
|
||||
The node budget (and thus RAM usage) is set at runtime with GF_AVOID_NODES.
|
||||
|
||||
menuconfig NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
bool "Check the mission flight path against the geofence at upload"
|
||||
default y
|
||||
depends on MODULES_NAVIGATOR
|
||||
---help---
|
||||
Reject an uploaded mission whose flight path crosses a geofence, not only a
|
||||
mission with a waypoint outside it. Boards without the flash for it keep the
|
||||
waypoint-only check.
|
||||
|
||||
menuconfig NAVIGATOR_ADSB_FAKE_TRAFFIC
|
||||
bool "Include fake-traffic injection for DAA testing"
|
||||
default n
|
||||
|
||||
@@ -845,6 +845,15 @@ bool Geofence::isBelowMaxAltitude(float altitude)
|
||||
return inside_fence;
|
||||
}
|
||||
|
||||
bool Geofence::isWithinAltitudeBand(float altitude)
|
||||
{
|
||||
if (!isEmpty() && _altitude_max > _altitude_min) {
|
||||
return !(altitude > _altitude_max || altitude < _altitude_min);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
|
||||
{
|
||||
if (isEmpty()) {
|
||||
@@ -853,10 +862,8 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
|
||||
}
|
||||
|
||||
/* Vertical check */
|
||||
if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly
|
||||
if (altitude > _altitude_max || altitude < _altitude_min) {
|
||||
return false;
|
||||
}
|
||||
if (!isWithinAltitudeBand(altitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Horizontal check: iterate all polygons & circles */
|
||||
|
||||
@@ -119,6 +119,13 @@ public:
|
||||
* On failure, results are false when the output buffer and count are valid.
|
||||
*/
|
||||
bool checkPathBatch(const PathCheck *paths, size_t num_paths, bool *results);
|
||||
bool isReadyForPathChecks() const { return _path_check_ready; }
|
||||
|
||||
/** True while a fence update is loading or a failed load waits for its retry. */
|
||||
bool isFenceUpdatePending() const
|
||||
{
|
||||
return _initiate_fence_updated || _dataman_state != DatamanState::UpdateRequestWait || _fence_retry_time != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief check if the horizontal distance to Home is greater than the maximum allowed distance
|
||||
@@ -135,6 +142,9 @@ public:
|
||||
*/
|
||||
bool isBelowMaxAltitude(float altitude);
|
||||
|
||||
/** Check the altitude band from a loaded fence file, if configured. */
|
||||
bool isWithinAltitudeBand(float altitude);
|
||||
|
||||
virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
|
||||
|
||||
bool valid();
|
||||
|
||||
@@ -784,12 +784,31 @@ MissionBase::checkMissionRestart()
|
||||
void
|
||||
MissionBase::check_mission_valid(bool forced)
|
||||
{
|
||||
// Allow forcing it, since we currently not rechecking if parameters have changed.
|
||||
if (forced ||
|
||||
(_navigator->get_mission_result()->mission_id != _mission.mission_id) ||
|
||||
(_navigator->get_mission_result()->geofence_id != _mission.geofence_id) ||
|
||||
(_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) {
|
||||
const bool mission_changed = _navigator->get_mission_result()->mission_id != _mission.mission_id;
|
||||
const bool inputs_changed = mission_changed
|
||||
|| (_navigator->get_mission_result()->geofence_id != _mission.geofence_id)
|
||||
|| (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count);
|
||||
|
||||
if (_navigator->get_geofence().isFenceUpdatePending()) {
|
||||
// A fence upload reloads the fence for about a second. Keep the last verdict until then
|
||||
// and recheck when it is ready: reporting the mission invalid now would make Commander
|
||||
// treat it as missing and fail over out of Mission mode.
|
||||
_mission_checked = false;
|
||||
_mission_check_pending = true;
|
||||
|
||||
// A mission that was never checked has no verdict to keep.
|
||||
if (mission_changed) {
|
||||
_navigator->get_mission_result()->valid = false;
|
||||
_navigator->set_mission_result_updated();
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Allow forcing it, since we currently not rechecking if parameters have changed.
|
||||
if (forced || _mission_check_pending || inputs_changed) {
|
||||
|
||||
_mission_check_pending = false;
|
||||
_navigator->get_mission_result()->mission_id = _mission.mission_id;
|
||||
_navigator->get_mission_result()->geofence_id = _mission.geofence_id;
|
||||
_navigator->get_mission_result()->home_position_counter = _navigator->get_home_position()->update_count;
|
||||
@@ -1583,6 +1602,7 @@ bool MissionBase::canRunMissionFeasibility()
|
||||
{
|
||||
return _navigator->home_global_position_valid() && // Need to have a home position checked
|
||||
_navigator->get_global_position()->timestamp > 0 && // Need to have a position, for first waypoint check
|
||||
_navigator->get_geofence().isReadyForPathChecks() &&
|
||||
(_geofence_status_sub.get().timestamp > 0) && // Geofence data must be loaded
|
||||
(_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
|
||||
(_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
|
||||
|
||||
@@ -442,6 +442,7 @@ protected:
|
||||
bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/
|
||||
bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/
|
||||
bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/
|
||||
bool _mission_check_pending{false}; /**< Retry a check deferred while the geofence was unavailable. */
|
||||
bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/
|
||||
mission_s _mission; /**< Currently active mission*/
|
||||
float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */
|
||||
|
||||
@@ -109,52 +109,179 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission)
|
||||
bool
|
||||
MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid)
|
||||
{
|
||||
if (_navigator->get_geofence().isHomeRequired() && !home_valid) {
|
||||
Geofence &geofence = _navigator->get_geofence();
|
||||
|
||||
if (geofence.isHomeRequired() && !home_valid) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
|
||||
events::send(events::ID("navigator_mis_geofence_no_home"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence requires a valid home position");
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
|
||||
if (_navigator->get_geofence().valid()) {
|
||||
for (size_t i = 0; i < mission.count; i++) {
|
||||
struct mission_item_s missionitem = {};
|
||||
if (!geofence.isReadyForPathChecks()) {
|
||||
logGeofenceUnavailable();
|
||||
return false;
|
||||
}
|
||||
|
||||
bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i,
|
||||
reinterpret_cast<uint8_t *>(&missionitem),
|
||||
sizeof(mission_item_s));
|
||||
if (!geofence.valid()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
logDatamanReadFailure(i, mission.mission_dataman_id);
|
||||
return false;
|
||||
enum class Failure { None, DatamanRead, NoHome, Waypoint } failure = Failure::None;
|
||||
size_t failed_item = 0;
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
/* Check mission positions and the paths between them. */
|
||||
static GeofencePathBatch batch{}; // keep the shared batch off the stack.
|
||||
batch.count = 0;
|
||||
matrix::Vector2d previous_position{};
|
||||
bool have_previous_position = false;
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
|
||||
for (size_t i = 0; i < mission.count; i++) {
|
||||
struct mission_item_s missionitem = {};
|
||||
|
||||
bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i,
|
||||
reinterpret_cast<uint8_t *>(&missionitem),
|
||||
sizeof(mission_item_s));
|
||||
|
||||
if (!success) {
|
||||
failure = Failure::DatamanRead;
|
||||
failed_item = i;
|
||||
break;
|
||||
}
|
||||
|
||||
if (!mission_item_contains_position(missionitem)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (missionitem.altitude_is_relative && !home_valid) {
|
||||
failure = Failure::NoHome;
|
||||
failed_item = i;
|
||||
break;
|
||||
}
|
||||
|
||||
// Geofence function checks against home altitude amsl
|
||||
missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
|
||||
|
||||
bool point_valid = PX4_ISFINITE(missionitem.lat) && PX4_ISFINITE(missionitem.lon)
|
||||
&& PX4_ISFINITE(missionitem.altitude)
|
||||
&& fabs(missionitem.lat) <= 90.0 && fabs(missionitem.lon) <= 180.0;
|
||||
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
|
||||
if (point_valid) {
|
||||
if (!have_previous_position) {
|
||||
// Check polygon membership once; the paths check all later positions.
|
||||
point_valid = geofence.checkPointAgainstAllGeofences(missionitem.lat, missionitem.lon, missionitem.altitude);
|
||||
|
||||
} else {
|
||||
// Home-distance and altitude limits are separate from the horizontal fence shapes.
|
||||
point_valid = geofence.isCloserThanMaxDistToHome(missionitem.lat, missionitem.lon, missionitem.altitude)
|
||||
&& geofence.isBelowMaxAltitude(missionitem.altitude)
|
||||
&& geofence.isWithinAltitudeBand(missionitem.altitude);
|
||||
}
|
||||
}
|
||||
|
||||
if (missionitem.altitude_is_relative && !home_valid) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
|
||||
events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence requires a valid home position");
|
||||
return false;
|
||||
}
|
||||
#else
|
||||
// Without path checks every position is checked against the whole fence.
|
||||
point_valid = point_valid && geofence.checkPointAgainstAllGeofences(missionitem.lat, missionitem.lon,
|
||||
missionitem.altitude);
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
|
||||
// Geofence function checks against home altitude amsl
|
||||
missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
|
||||
if (!point_valid) {
|
||||
failure = Failure::Waypoint;
|
||||
failed_item = i;
|
||||
break;
|
||||
}
|
||||
|
||||
if (mission_item_contains_position(missionitem) && !_navigator->get_geofence().checkPointAgainstAllGeofences(
|
||||
missionitem.lat, missionitem.lon, missionitem.altitude)) {
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
const matrix::Vector2d position {missionitem.lat, missionitem.lon};
|
||||
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1);
|
||||
events::send<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence violation for waypoint {1}",
|
||||
i + 1);
|
||||
return false;
|
||||
}
|
||||
if (!have_previous_position) {
|
||||
// A zero-length path also rejects a lone waypoint on a fence boundary.
|
||||
previous_position = position;
|
||||
have_previous_position = true;
|
||||
}
|
||||
|
||||
batch.paths[batch.count] = {previous_position, position};
|
||||
batch.mission_indices[batch.count++] = static_cast<uint16_t>(i);
|
||||
previous_position = position;
|
||||
|
||||
if (batch.count == kGeofencePathBatchSize && !checkGeofencePathBatch(batch)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
}
|
||||
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
|
||||
// Report an earlier buffered path breach before the current item's failure.
|
||||
if (!checkGeofencePathBatch(batch)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
|
||||
switch (failure) {
|
||||
case Failure::None:
|
||||
return true;
|
||||
|
||||
case Failure::DatamanRead:
|
||||
logDatamanReadFailure(failed_item, mission.mission_dataman_id);
|
||||
break;
|
||||
|
||||
case Failure::NoHome:
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
|
||||
events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence requires a valid home position");
|
||||
break;
|
||||
|
||||
case Failure::Waypoint:
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", failed_item + 1);
|
||||
events::send<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence violation for waypoint {1}", failed_item + 1);
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
bool MissionFeasibilityChecker::checkGeofencePathBatch(GeofencePathBatch &batch)
|
||||
{
|
||||
if (batch.count == 0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!_navigator->get_geofence().checkPathBatch(batch.paths, batch.count, batch.results)) {
|
||||
logGeofenceUnavailable();
|
||||
return false;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < batch.count; ++i) {
|
||||
if (!batch.results[i]) {
|
||||
const uint16_t waypoint = batch.mission_indices[i] + 1;
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence breach on path to waypoint %u\t",
|
||||
static_cast<unsigned>(waypoint));
|
||||
events::send<uint16_t>(events::ID("navigator_mis_geofence_path_violation"),
|
||||
{events::Log::Error, events::LogInternal::Info},
|
||||
"Geofence breach on path to waypoint {1}", waypoint);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
batch.count = 0;
|
||||
return true;
|
||||
}
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
|
||||
void MissionFeasibilityChecker::logGeofenceUnavailable()
|
||||
{
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: geofence path check unavailable\t");
|
||||
events::send(events::ID("navigator_mis_geofence_unavailable"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: geofence path check unavailable");
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::logDatamanReadFailure(const size_t mission_item, const uint8_t dataman_id)
|
||||
{
|
||||
|
||||
@@ -46,17 +46,32 @@
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include "MissionFeasibility/FeasibilityChecker.hpp"
|
||||
#include "geofence.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class MissionFeasibilityChecker: public ModuleParams
|
||||
{
|
||||
private:
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
static constexpr size_t kGeofencePathBatchSize = Geofence::MAX_PATH_CHECKS;
|
||||
struct GeofencePathBatch {
|
||||
Geofence::PathCheck paths[kGeofencePathBatchSize];
|
||||
uint16_t mission_indices[kGeofencePathBatchSize];
|
||||
bool results[kGeofencePathBatchSize];
|
||||
size_t count{0};
|
||||
};
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
|
||||
Navigator *_navigator{nullptr};
|
||||
DatamanClient &_dataman_client;
|
||||
FeasibilityChecker _feasibility_checker;
|
||||
|
||||
bool checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid);
|
||||
#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
bool checkGeofencePathBatch(GeofencePathBatch &batch);
|
||||
#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS
|
||||
void logGeofenceUnavailable();
|
||||
void logDatamanReadFailure(const size_t mission_item, const uint8_t dataman_id);
|
||||
|
||||
public:
|
||||
|
||||
@@ -37,6 +37,9 @@ px4_add_functional_gtest(SRC test_geofence.cpp LINKLIBS modules__navigator modul
|
||||
px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator modules__dataman)
|
||||
px4_add_functional_gtest(SRC test_RTL.cpp LINKLIBS modules__navigator modules__dataman)
|
||||
px4_add_functional_gtest(SRC test_geofence_paths.cpp LINKLIBS modules__navigator modules__dataman)
|
||||
if(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS)
|
||||
px4_add_functional_gtest(SRC test_mission_feasibility_geofence.cpp LINKLIBS modules__navigator modules__dataman)
|
||||
endif()
|
||||
px4_add_functional_gtest(SRC test_mission_route_cache.cpp LINKLIBS modules__navigator navigator_mission_route modules__dataman)
|
||||
|
||||
if(CONFIG_MODULES_VISION_TARGET_ESTIMATOR)
|
||||
|
||||
@@ -44,6 +44,7 @@
|
||||
|
||||
#include "mission_base.h"
|
||||
#include "navigator.h"
|
||||
#include "support/geofence_test_helpers.h"
|
||||
#include "support/mission_route_cache_test_peer.h"
|
||||
#include "support/navigator_dataman_test.h"
|
||||
#include "support/vector_mission_item_store.h"
|
||||
@@ -52,7 +53,10 @@
|
||||
#include <vector>
|
||||
|
||||
#include <cstring>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/mavlink_log.h>
|
||||
|
||||
class MissionBaseTestPeer : public MissionBase
|
||||
@@ -119,6 +123,15 @@ public:
|
||||
return _mission.current_seq;
|
||||
}
|
||||
|
||||
void checkMission(bool forced = false)
|
||||
{
|
||||
// Normal MissionBase callers mark the check before calling the validator.
|
||||
_mission_checked = true;
|
||||
check_mission_valid(forced);
|
||||
}
|
||||
|
||||
bool missionChecked() const { return _mission_checked; }
|
||||
|
||||
using MissionBase::findNextPositionIndex;
|
||||
using MissionBase::findPreviousPositionIndex;
|
||||
using MissionBase::getNonJumpItem;
|
||||
@@ -186,6 +199,224 @@ protected:
|
||||
IgnoreDoJumpMissionBaseTestPeer mission_base{};
|
||||
};
|
||||
|
||||
class MissionBaseTestNavigator : public Navigator
|
||||
{
|
||||
public:
|
||||
using Navigator::updateParams;
|
||||
};
|
||||
|
||||
class MissionBaseFeasibilityTest : public NavigatorDatamanTestBase
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
param_reset_all();
|
||||
_navigator.updateParams();
|
||||
ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0));
|
||||
ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_FENCE_POINTS_0));
|
||||
|
||||
home_position_s *home = _navigator.get_home_position();
|
||||
home->timestamp = hrt_absolute_time();
|
||||
home->lat = kBaseLat;
|
||||
home->lon = kBaseLon;
|
||||
home->alt = 0.f;
|
||||
home->valid_hpos = true;
|
||||
home->valid_alt = true;
|
||||
_home_pub.publish(*home);
|
||||
_navigator.get_land_detected()->landed = true;
|
||||
|
||||
vehicle_status_s status{};
|
||||
status.timestamp = hrt_absolute_time();
|
||||
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
*_navigator.get_vstatus() = status;
|
||||
_status_pub.publish(status);
|
||||
|
||||
mission_fence_point_s circle{};
|
||||
circle.nav_cmd = NAV_CMD_FENCE_CIRCLE_INCLUSION;
|
||||
circle.frame = NAV_FRAME_GLOBAL;
|
||||
circle.lat = kBaseLat;
|
||||
circle.lon = kBaseLon;
|
||||
circle.circle_radius = 100.f;
|
||||
ASSERT_TRUE(_dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, 0,
|
||||
reinterpret_cast<uint8_t *>(&circle), sizeof(circle)));
|
||||
mission_stats_entry_s stats{};
|
||||
stats.dataman_id = DM_KEY_FENCE_POINTS_0;
|
||||
stats.opaque_id = 1;
|
||||
stats.num_items = 1;
|
||||
ASSERT_TRUE(_dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0,
|
||||
reinterpret_cast<uint8_t *>(&stats), sizeof(stats)));
|
||||
_navigator.get_geofence().updateFence();
|
||||
ASSERT_TRUE(finishFenceUpdate());
|
||||
|
||||
mission_s mission{};
|
||||
mission.timestamp = hrt_absolute_time();
|
||||
mission.mission_id = 1;
|
||||
mission.geofence_id = stats.opaque_id;
|
||||
mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
_mission_base.loadTestMission({makePositionItem(kBaseLat, kBaseLon, kAlt)}, mission);
|
||||
ASSERT_TRUE(writeWaypoint(0.f));
|
||||
}
|
||||
|
||||
void TearDown() override { param_reset_all(); }
|
||||
|
||||
bool finishFenceUpdate()
|
||||
{
|
||||
Geofence &fence = _navigator.get_geofence();
|
||||
const hrt_abstime start = hrt_absolute_time();
|
||||
|
||||
while (!fence.isReadyForPathChecks() && hrt_elapsed_time(&start) < 1_s) {
|
||||
fence.run();
|
||||
px4_usleep(1000);
|
||||
}
|
||||
|
||||
return fence.isReadyForPathChecks();
|
||||
}
|
||||
|
||||
bool writeWaypoint(float north_m)
|
||||
{
|
||||
mission_item_s item = makePositionItem(kBaseLat, kBaseLon, kAlt);
|
||||
add_vector_to_global_position(kBaseLat, kBaseLon, north_m, 0.f, &item.lat, &item.lon);
|
||||
return _dataman_client.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0,
|
||||
reinterpret_cast<uint8_t *>(&item), sizeof(item));
|
||||
}
|
||||
|
||||
DatamanClient _dataman_client{};
|
||||
MissionBaseTestNavigator _navigator{};
|
||||
MissionBaseTestPeer _mission_base{&_navigator};
|
||||
uORB::Publication<home_position_s> _home_pub{ORB_ID(home_position)};
|
||||
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
|
||||
};
|
||||
|
||||
TEST_F(MissionBaseFeasibilityTest, DeferredCheckPreservesVerdictUntilRetry)
|
||||
{
|
||||
_mission_base.checkMission(true);
|
||||
const mission_result_s checked_result = *_navigator.get_mission_result();
|
||||
ASSERT_TRUE(checked_result.valid);
|
||||
|
||||
// Change storage without changing IDs to verify that the pending check really runs.
|
||||
ASSERT_TRUE(writeWaypoint(200.f));
|
||||
_navigator.get_geofence().updateFence();
|
||||
_mission_base.checkMission(true);
|
||||
EXPECT_TRUE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_FALSE(_mission_base.missionChecked());
|
||||
EXPECT_EQ(_navigator.get_mission_result()->mission_id, checked_result.mission_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->geofence_id, checked_result.geofence_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, checked_result.home_position_counter);
|
||||
|
||||
ASSERT_TRUE(finishFenceUpdate());
|
||||
_mission_base.checkMission();
|
||||
ASSERT_FALSE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_TRUE(_mission_base.missionChecked());
|
||||
|
||||
// A completed failure stays cached until another check is requested.
|
||||
ASSERT_TRUE(writeWaypoint(0.f));
|
||||
_mission_base.checkMission();
|
||||
EXPECT_FALSE(_navigator.get_mission_result()->valid);
|
||||
|
||||
_navigator.get_geofence().updateFence();
|
||||
_mission_base.checkMission(true);
|
||||
EXPECT_FALSE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_FALSE(_mission_base.missionChecked());
|
||||
ASSERT_TRUE(finishFenceUpdate());
|
||||
_mission_base.checkMission();
|
||||
EXPECT_TRUE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_TRUE(_mission_base.missionChecked());
|
||||
}
|
||||
|
||||
TEST_F(MissionBaseFeasibilityTest, SpentFenceRetriesLetTheCheckReject)
|
||||
{
|
||||
_mission_base.checkMission(true);
|
||||
ASSERT_TRUE(_navigator.get_mission_result()->valid);
|
||||
Geofence &fence = _navigator.get_geofence();
|
||||
fence.updateFence();
|
||||
|
||||
for (unsigned attempt = 0; attempt <= GeofenceTestPeer::maxLoadRetries(); ++attempt) {
|
||||
SCOPED_TRACE(attempt);
|
||||
// The verdict is kept while a retry is still pending.
|
||||
_mission_base.checkMission(true);
|
||||
EXPECT_TRUE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_FALSE(_mission_base.missionChecked());
|
||||
GeofenceTestPeer::expireRetryDelay(fence);
|
||||
fence.run();
|
||||
fence.run();
|
||||
ASSERT_TRUE(GeofenceTestPeer::failPendingRead(fence));
|
||||
fence.run();
|
||||
fence.run();
|
||||
}
|
||||
|
||||
// Once the fence gives up, the deferred check runs and rejects the mission.
|
||||
ASSERT_FALSE(fence.isFenceUpdatePending());
|
||||
_mission_base.checkMission(true);
|
||||
EXPECT_FALSE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_TRUE(_mission_base.missionChecked());
|
||||
}
|
||||
|
||||
enum class MissionFeasibilityInput { Mission, Geofence, Home };
|
||||
|
||||
struct ChangedMissionInput {
|
||||
const char *name;
|
||||
MissionFeasibilityInput input;
|
||||
bool keeps_verdict;
|
||||
};
|
||||
|
||||
class MissionBaseChangedInputTest : public MissionBaseFeasibilityTest,
|
||||
public ::testing::WithParamInterface<ChangedMissionInput> {};
|
||||
|
||||
TEST_P(MissionBaseChangedInputTest, DeferredCheckInvalidatesOnlyAnUncheckedMission)
|
||||
{
|
||||
_mission_base.checkMission(true);
|
||||
const mission_result_s checked_result = *_navigator.get_mission_result();
|
||||
ASSERT_TRUE(checked_result.valid);
|
||||
|
||||
mission_s mission{};
|
||||
mission.timestamp = hrt_absolute_time();
|
||||
mission.mission_id = checked_result.mission_id;
|
||||
mission.geofence_id = checked_result.geofence_id;
|
||||
mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
switch (GetParam().input) {
|
||||
case MissionFeasibilityInput::Mission:
|
||||
++mission.mission_id;
|
||||
break;
|
||||
|
||||
case MissionFeasibilityInput::Geofence:
|
||||
++mission.geofence_id;
|
||||
break;
|
||||
|
||||
case MissionFeasibilityInput::Home:
|
||||
++_navigator.get_home_position()->update_count;
|
||||
break;
|
||||
}
|
||||
|
||||
_mission_base.loadTestMission({makePositionItem(kBaseLat, kBaseLon, kAlt)}, mission);
|
||||
_navigator.get_geofence().updateFence();
|
||||
_mission_base.checkMission(true);
|
||||
// A fence or Home change keeps the checked mission flyable until the recheck.
|
||||
EXPECT_EQ(_navigator.get_mission_result()->valid, GetParam().keeps_verdict);
|
||||
EXPECT_FALSE(_mission_base.missionChecked());
|
||||
// No result has been computed for the changed inputs yet.
|
||||
EXPECT_EQ(_navigator.get_mission_result()->mission_id, checked_result.mission_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->geofence_id, checked_result.geofence_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, checked_result.home_position_counter);
|
||||
|
||||
ASSERT_TRUE(finishFenceUpdate());
|
||||
_mission_base.checkMission();
|
||||
EXPECT_TRUE(_navigator.get_mission_result()->valid);
|
||||
EXPECT_TRUE(_mission_base.missionChecked());
|
||||
EXPECT_EQ(_navigator.get_mission_result()->mission_id, mission.mission_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->geofence_id, mission.geofence_id);
|
||||
EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, _navigator.get_home_position()->update_count);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(ChangedInputs, MissionBaseChangedInputTest, ::testing::Values(
|
||||
ChangedMissionInput{"Mission", MissionFeasibilityInput::Mission, false},
|
||||
ChangedMissionInput{"Geofence", MissionFeasibilityInput::Geofence, true},
|
||||
ChangedMissionInput{"Home", MissionFeasibilityInput::Home, true}),
|
||||
[](const ::testing::TestParamInfo<ChangedMissionInput> &test_info)
|
||||
{
|
||||
return test_info.param.name;
|
||||
});
|
||||
|
||||
#if CONFIG_NAVIGATOR_FULL_MISSION_CACHE_SIZE > 0
|
||||
class MissionBaseRouteCacheSyncTest : public NavigatorDatamanTestBase
|
||||
{
|
||||
|
||||
@@ -0,0 +1,435 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* EVENT
|
||||
* @skip-file
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "mission_feasibility_checker.h"
|
||||
#include "support/geofence_test_helpers.h"
|
||||
#include "support/mission_route_test_helpers.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/event.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/mavlink_log.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class MissionFeasibilityGeofenceTest : public navigator_test::GeofenceTestBase
|
||||
{
|
||||
protected:
|
||||
using navigator_test::GeofenceTestBase::GeofenceTestBase;
|
||||
|
||||
static constexpr float kAltitude = 500.f;
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
param_reset_all();
|
||||
_navigator.updateParams();
|
||||
ASSERT_TRUE(resetFence(kAltitude - 100.f));
|
||||
ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0));
|
||||
_home_pub.publish(*_navigator.get_home_position());
|
||||
|
||||
vehicle_status_s status{};
|
||||
status.timestamp = hrt_absolute_time();
|
||||
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
*_navigator.get_vstatus() = status;
|
||||
_status_pub.publish(status);
|
||||
ASSERT_TRUE(loadFence({}));
|
||||
}
|
||||
|
||||
void TearDown() override { param_reset_all(); }
|
||||
|
||||
mission_item_s waypoint(const matrix::Vector2f &offset) const
|
||||
{
|
||||
return navigator_test::makePositionItemFromOffset(_reference(0), _reference(1), offset(0), offset(1), kAltitude);
|
||||
}
|
||||
|
||||
static mission_item_s changeSpeed()
|
||||
{
|
||||
mission_item_s item{};
|
||||
item.nav_cmd = NAV_CMD_DO_CHANGE_SPEED;
|
||||
item.params[0] = 1.f;
|
||||
item.params[1] = 5.f;
|
||||
return item;
|
||||
}
|
||||
|
||||
FencePoints inclusionSquare() const
|
||||
{
|
||||
return polygon(true, {{-1000.f, -1000.f}, {1000.f, -1000.f}, {1000.f, 1000.f}, {-1000.f, 1000.f}});
|
||||
}
|
||||
|
||||
void drainReports()
|
||||
{
|
||||
mavlink_log_s report{};
|
||||
event_s event{};
|
||||
|
||||
while (_log_sub.update(&report)) {}
|
||||
|
||||
while (_event_sub.update(&event)) {}
|
||||
}
|
||||
|
||||
bool missionFeasible(const std::vector<mission_item_s> &items)
|
||||
{
|
||||
drainReports();
|
||||
|
||||
for (size_t i = 0; i < items.size(); ++i) {
|
||||
mission_item_s item = items[i];
|
||||
EXPECT_TRUE(_dataman_client.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, static_cast<uint32_t>(i),
|
||||
reinterpret_cast<uint8_t *>(&item), sizeof(item)));
|
||||
}
|
||||
|
||||
mission_s mission{};
|
||||
mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
mission.count = static_cast<uint16_t>(items.size());
|
||||
MissionFeasibilityChecker checker(&_navigator, _dataman_client);
|
||||
return checker.checkMissionFeasible(mission);
|
||||
}
|
||||
|
||||
bool logContains(const char *text)
|
||||
{
|
||||
mavlink_log_s report{};
|
||||
bool found = false;
|
||||
|
||||
while (_log_sub.update(&report)) {
|
||||
found |= strstr(reinterpret_cast<const char *>(report.text), text) != nullptr;
|
||||
}
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
::testing::AssertionResult waypointEvent(uint32_t id, int16_t expected_waypoint)
|
||||
{
|
||||
event_s event{};
|
||||
|
||||
while (_event_sub.update(&event)) {
|
||||
if (event.id == id) {
|
||||
int16_t waypoint_index;
|
||||
memcpy(&waypoint_index, event.arguments, sizeof(waypoint_index));
|
||||
|
||||
if (waypoint_index != expected_waypoint) {
|
||||
return ::testing::AssertionFailure() << "reported waypoint " << waypoint_index
|
||||
<< ", expected " << expected_waypoint;
|
||||
}
|
||||
|
||||
return ::testing::AssertionSuccess();
|
||||
}
|
||||
}
|
||||
|
||||
return ::testing::AssertionFailure() << "expected waypoint event not published";
|
||||
}
|
||||
|
||||
void expectPathViolation(int16_t waypoint_index)
|
||||
{
|
||||
const std::string message = "Geofence breach on path to waypoint " + std::to_string(waypoint_index);
|
||||
EXPECT_TRUE(logContains(message.c_str()));
|
||||
EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_path_violation"), waypoint_index));
|
||||
}
|
||||
|
||||
uORB::Publication<home_position_s> _home_pub{ORB_ID(home_position)};
|
||||
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _log_sub{ORB_ID(mavlink_log)};
|
||||
uORB::Subscription _event_sub{ORB_ID(event)};
|
||||
};
|
||||
|
||||
struct MissionLegCase {
|
||||
const char *name;
|
||||
matrix::Vector2f start;
|
||||
matrix::Vector2f end;
|
||||
bool feasible;
|
||||
};
|
||||
|
||||
class MissionGeofenceLegTest : public MissionFeasibilityGeofenceTest,
|
||||
public ::testing::WithParamInterface<MissionLegCase> {};
|
||||
|
||||
TEST_P(MissionGeofenceLegTest, ChecksLegAcrossCommandItems)
|
||||
{
|
||||
const MissionLegCase &test = GetParam();
|
||||
ASSERT_TRUE(loadFence(exclusionSquare()));
|
||||
EXPECT_EQ(missionFeasible({changeSpeed(), waypoint(test.start), changeSpeed(), waypoint(test.end)}), test.feasible);
|
||||
|
||||
if (!test.feasible) {
|
||||
expectPathViolation(4);
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(MissionLegs, MissionGeofenceLegTest, ::testing::Values(
|
||||
MissionLegCase{"AcrossExclusionPolygon", {0.f, 100.f}, {0.f, 500.f}, false},
|
||||
MissionLegCase{"ClearOfExclusionPolygon", {60.f, 100.f}, {60.f, 500.f}, true}),
|
||||
[](const ::testing::TestParamInfo<MissionLegCase> &test_info)
|
||||
{
|
||||
return test_info.param.name;
|
||||
});
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, FirstPositionMustPassEveryInclusion)
|
||||
{
|
||||
FencePoints points = inclusionSquare();
|
||||
const FencePoints smaller = polygon(true, {{-100.f, -100.f}, {100.f, -100.f}, {100.f, 100.f}, {-100.f, 100.f}});
|
||||
points.insert(points.end(), smaller.begin(), smaller.end());
|
||||
ASSERT_TRUE(loadFence(points));
|
||||
EXPECT_FALSE(missionFeasible({changeSpeed(), waypoint({0.f, 200.f})}));
|
||||
EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 2));
|
||||
EXPECT_TRUE(missionFeasible({waypoint({0.f, 0.f}), waypoint({50.f, 50.f})}));
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, SinglePositionOnBoundaryIsRejected)
|
||||
{
|
||||
const FencePoints points = exclusionSquare();
|
||||
ASSERT_TRUE(loadFence(points));
|
||||
mission_item_s item = waypoint({0.f, 0.f});
|
||||
item.lat = points.front().lat;
|
||||
item.lon = points.front().lon;
|
||||
ASSERT_TRUE(_fence.checkPointAgainstAllGeofences(item.lat, item.lon, item.altitude));
|
||||
EXPECT_FALSE(missionFeasible({item}));
|
||||
expectPathViolation(1);
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceAcceptsPositionsAndCommandOnlyMission)
|
||||
{
|
||||
EXPECT_TRUE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 5000.f})}));
|
||||
EXPECT_TRUE(missionFeasible({changeSpeed()}));
|
||||
}
|
||||
|
||||
// Home sits next to the antimeridian.
|
||||
class AntimeridianMissionGeofenceTest : public MissionFeasibilityGeofenceTest
|
||||
{
|
||||
protected:
|
||||
AntimeridianMissionGeofenceTest() : MissionFeasibilityGeofenceTest(47.0, 179.999) {}
|
||||
};
|
||||
|
||||
TEST_F(AntimeridianMissionGeofenceTest, ShortLegMayCrossTheAntimeridian)
|
||||
{
|
||||
// About 150 m across the antimeridian.
|
||||
const mission_item_s west = navigator_test::makePositionItem(47.0, 179.999, kAltitude);
|
||||
const mission_item_s east = navigator_test::makePositionItem(47.0, -179.999, kAltitude);
|
||||
EXPECT_TRUE(missionFeasible({west, east}));
|
||||
EXPECT_FALSE(logContains("geofence path check unavailable"));
|
||||
// An exclusion zone between the waypoint and the antimeridian still blocks the leg.
|
||||
ASSERT_TRUE(loadFence(polygon(false, {{-20.f, 20.f}, {20.f, 20.f}, {20.f, 60.f}, {-20.f, 60.f}})));
|
||||
EXPECT_FALSE(missionFeasible({west, east}));
|
||||
expectPathViolation(2);
|
||||
}
|
||||
|
||||
struct MissionBatchCase {
|
||||
const char *name;
|
||||
size_t safe_positions;
|
||||
bool final_leg_breaches;
|
||||
};
|
||||
|
||||
class MissionGeofenceBatchTest : public MissionFeasibilityGeofenceTest,
|
||||
public ::testing::WithParamInterface<MissionBatchCase> {};
|
||||
|
||||
TEST_P(MissionGeofenceBatchTest, ChecksBatchesAndResetsForNextMission)
|
||||
{
|
||||
const MissionBatchCase &test = GetParam();
|
||||
ASSERT_TRUE(loadFence(exclusionSquare()));
|
||||
std::vector<mission_item_s> items{changeSpeed()};
|
||||
|
||||
for (size_t i = 0; i < test.safe_positions; ++i) {
|
||||
items.push_back(waypoint({0.f, 100.f}));
|
||||
items.push_back(changeSpeed());
|
||||
}
|
||||
|
||||
items.push_back(waypoint({0.f, test.final_leg_breaches ? 500.f : 150.f}));
|
||||
EXPECT_EQ(missionFeasible(items), !test.final_leg_breaches);
|
||||
|
||||
if (test.final_leg_breaches) {
|
||||
expectPathViolation(static_cast<int16_t>(items.size()));
|
||||
}
|
||||
|
||||
// The next upload must not retain paths from the previous validation.
|
||||
EXPECT_TRUE(missionFeasible({waypoint({0.f, 150.f})}));
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(MissionBatches, MissionGeofenceBatchTest, ::testing::Values(
|
||||
MissionBatchCase{"BreachAtFirstBatchEnd", Geofence::MAX_PATH_CHECKS - 1, true},
|
||||
MissionBatchCase{"BreachAfterFirstBatch", Geofence::MAX_PATH_CHECKS, true},
|
||||
MissionBatchCase{"ClearFullBatches", 2 * Geofence::MAX_PATH_CHECKS - 1, false},
|
||||
MissionBatchCase{"ClearFinalRemainder", 2 * Geofence::MAX_PATH_CHECKS + 1, false}),
|
||||
[](const ::testing::TestParamInfo<MissionBatchCase> &test_info)
|
||||
{
|
||||
return test_info.param.name;
|
||||
});
|
||||
|
||||
enum class MissionScalarLimit { Horizontal, Vertical, AltitudeBand };
|
||||
|
||||
struct MissionScalarCase {
|
||||
const char *name;
|
||||
MissionScalarLimit limit;
|
||||
float east;
|
||||
float altitude;
|
||||
bool relative;
|
||||
bool feasible;
|
||||
};
|
||||
|
||||
class MissionGeofenceScalarTest : public MissionFeasibilityGeofenceTest,
|
||||
public ::testing::WithParamInterface<MissionScalarCase> {};
|
||||
|
||||
TEST_P(MissionGeofenceScalarTest, ChecksEveryPosition)
|
||||
{
|
||||
const MissionScalarCase &test = GetParam();
|
||||
ASSERT_TRUE(loadFence(inclusionSquare()));
|
||||
|
||||
if (test.limit == MissionScalarLimit::AltitudeBand) {
|
||||
GeofenceTestPeer::setAltitudeBand(_fence, 450.f, 550.f);
|
||||
|
||||
} else {
|
||||
const float maximum = test.limit == MissionScalarLimit::Horizontal ? 300.f : 150.f;
|
||||
ASSERT_EQ(param_set(param_find(test.limit == MissionScalarLimit::Horizontal ? "GF_MAX_HOR_DIST" : "GF_MAX_VER_DIST"),
|
||||
&maximum), PX4_OK);
|
||||
_navigator.updateParams();
|
||||
}
|
||||
|
||||
mission_item_s second = waypoint({0.f, test.east});
|
||||
second.altitude = test.altitude;
|
||||
second.altitude_is_relative = test.relative;
|
||||
EXPECT_EQ(missionFeasible({waypoint({0.f, 100.f}), changeSpeed(), second}), test.feasible);
|
||||
|
||||
if (!test.feasible) {
|
||||
EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 3));
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(MissionScalarLimits, MissionGeofenceScalarTest, ::testing::Values(
|
||||
MissionScalarCase{"WithinHomeDistance", MissionScalarLimit::Horizontal, 200.f, 500.f, false, true},
|
||||
MissionScalarCase{"BeyondHomeDistance", MissionScalarLimit::Horizontal, 400.f, 500.f, false, false},
|
||||
MissionScalarCase{"WithinRelativeAltitude", MissionScalarLimit::Vertical, 200.f, 100.f, true, true},
|
||||
MissionScalarCase{"BeyondRelativeAltitude", MissionScalarLimit::Vertical, 200.f, 200.f, true, false},
|
||||
MissionScalarCase{"BelowAltitudeBand", MissionScalarLimit::AltitudeBand, 200.f, 440.f, false, false},
|
||||
MissionScalarCase{"AboveAltitudeBand", MissionScalarLimit::AltitudeBand, 200.f, 560.f, false, false},
|
||||
MissionScalarCase{"AtAltitudeBandMinimum", MissionScalarLimit::AltitudeBand, 200.f, 450.f, false, true},
|
||||
MissionScalarCase{"AtAltitudeBandMaximum", MissionScalarLimit::AltitudeBand, 200.f, 550.f, false, true}),
|
||||
[](const ::testing::TestParamInfo<MissionScalarCase> &test_info)
|
||||
{
|
||||
return test_info.param.name;
|
||||
});
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceStillChecksHomeLimits)
|
||||
{
|
||||
const float maximum = 300.f;
|
||||
ASSERT_EQ(param_set(param_find("GF_MAX_HOR_DIST"), &maximum), PX4_OK);
|
||||
_navigator.updateParams();
|
||||
EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 400.f})}));
|
||||
EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 2));
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceIgnoresStoredAltitudeBand)
|
||||
{
|
||||
GeofenceTestPeer::setAltitudeBand(_fence, 450.f, 550.f);
|
||||
mission_item_s second = waypoint({0.f, 200.f});
|
||||
second.altitude = 600.f;
|
||||
EXPECT_TRUE(missionFeasible({waypoint({0.f, 100.f}), second}));
|
||||
}
|
||||
|
||||
enum class LaterMissionFailure { Altitude, NoHome };
|
||||
|
||||
class MissionGeofenceFailureOrderTest : public MissionFeasibilityGeofenceTest,
|
||||
public ::testing::WithParamInterface<LaterMissionFailure> {};
|
||||
|
||||
TEST_P(MissionGeofenceFailureOrderTest, EarlierPathBreachPrecedesLaterFailure)
|
||||
{
|
||||
ASSERT_TRUE(loadFence(exclusionSquare()));
|
||||
mission_item_s invalid = waypoint({100.f, 500.f});
|
||||
|
||||
if (GetParam() == LaterMissionFailure::NoHome) {
|
||||
_navigator.get_home_position()->valid_hpos = false;
|
||||
_home_pub.publish(*_navigator.get_home_position());
|
||||
invalid.altitude_is_relative = true;
|
||||
invalid.altitude = 100.f;
|
||||
|
||||
} else {
|
||||
const float maximum = 150.f;
|
||||
ASSERT_EQ(param_set(param_find("GF_MAX_VER_DIST"), &maximum), PX4_OK);
|
||||
_navigator.updateParams();
|
||||
invalid.altitude = 600.f;
|
||||
}
|
||||
|
||||
// The first leg crosses the exclusion, but is still buffered when the third item fails.
|
||||
EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 500.f}), invalid}));
|
||||
expectPathViolation(2);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(MissionFailureOrder, MissionGeofenceFailureOrderTest,
|
||||
::testing::Values(LaterMissionFailure::Altitude, LaterMissionFailure::NoHome),
|
||||
[](const ::testing::TestParamInfo<LaterMissionFailure> &test_info)
|
||||
{
|
||||
return test_info.param == LaterMissionFailure::NoHome ? "MissingHome" : "AltitudeLimit";
|
||||
});
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, RelativeAltitudeRequiresValidHome)
|
||||
{
|
||||
_navigator.get_home_position()->valid_hpos = false;
|
||||
_home_pub.publish(*_navigator.get_home_position());
|
||||
mission_item_s relative = waypoint({0.f, 100.f});
|
||||
relative.altitude_is_relative = true;
|
||||
relative.altitude = 100.f;
|
||||
EXPECT_FALSE(missionFeasible({relative}));
|
||||
EXPECT_TRUE(logContains("Geofence requires valid home position"));
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, MissingFenceCacheRejectsUpload)
|
||||
{
|
||||
ASSERT_TRUE(loadFence(exclusionSquare()));
|
||||
GeofenceTestPeer::invalidateCache(_fence);
|
||||
EXPECT_FALSE(missionFeasible({waypoint({100.f, 100.f}), waypoint({100.f, 500.f})}));
|
||||
EXPECT_TRUE(logContains("geofence path check unavailable"));
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, PendingFenceRefreshRejectsUpload)
|
||||
{
|
||||
_fence.updateFence();
|
||||
EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f})}));
|
||||
EXPECT_TRUE(logContains("geofence path check unavailable"));
|
||||
}
|
||||
|
||||
TEST_F(MissionFeasibilityGeofenceTest, UnreadableMissionStorageRejectsUpload)
|
||||
{
|
||||
drainReports();
|
||||
mission_s mission{};
|
||||
// This key cannot hold a mission_item_s, so readSync fails without a timeout.
|
||||
mission.mission_dataman_id = DM_KEY_FENCE_POINTS_STATE;
|
||||
mission.count = 1;
|
||||
MissionFeasibilityChecker checker(&_navigator, _dataman_client);
|
||||
EXPECT_FALSE(checker.checkMissionFeasible(mission));
|
||||
EXPECT_TRUE(logContains("dataman read failed at item 0"));
|
||||
EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_dm_read_fail"), 0));
|
||||
}
|
||||
@@ -53,6 +53,7 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
@@ -303,16 +304,28 @@ TEST_F(PrecTakeoffIntegrationTest, MissionTakeoffCorrectionPreservesPlannedDesti
|
||||
uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||
uORB::Publication<vehicle_global_position_s> global_pos_pub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Publication<vehicle_land_detected_s> land_detected_pub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Publication<geofence_status_s> geofence_status_pub{ORB_ID(geofence_status)};
|
||||
uORB::Publication<mission_s> mission_pub{ORB_ID(mission)};
|
||||
ASSERT_TRUE(home_pub.publish(home));
|
||||
ASSERT_TRUE(vehicle_status_pub.publish(vehicle_status));
|
||||
ASSERT_TRUE(global_pos_pub.publish(*_navigator->get_global_position()));
|
||||
ASSERT_TRUE(land_detected_pub.publish(*_navigator->get_land_detected()));
|
||||
geofence_status_s geofence_status{};
|
||||
geofence_status.timestamp = hrt_absolute_time();
|
||||
geofence_status.status = geofence_status_s::GF_STATUS_READY;
|
||||
ASSERT_TRUE(geofence_status_pub.publish(geofence_status));
|
||||
|
||||
// Load an empty fence so mission validation sees the completed fence update.
|
||||
DatamanClient dataman;
|
||||
mission_stats_entry_s fence_stats{};
|
||||
fence_stats.dataman_id = DM_KEY_FENCE_POINTS_0;
|
||||
ASSERT_TRUE(dataman.writeSync(DM_KEY_FENCE_POINTS_STATE, 0,
|
||||
reinterpret_cast<uint8_t *>(&fence_stats), sizeof(fence_stats)));
|
||||
Geofence &fence = _navigator->get_geofence();
|
||||
fence.updateFence();
|
||||
const hrt_abstime start = hrt_absolute_time();
|
||||
|
||||
while (!fence.isReadyForPathChecks() && hrt_elapsed_time(&start) < 1_s) {
|
||||
fence.run();
|
||||
px4_usleep(1000);
|
||||
}
|
||||
|
||||
ASSERT_TRUE(fence.isReadyForPathChecks());
|
||||
|
||||
mission_item_s planned_takeoff{};
|
||||
planned_takeoff.nav_cmd = NAV_CMD_VTOL_TAKEOFF;
|
||||
@@ -326,7 +339,6 @@ TEST_F(PrecTakeoffIntegrationTest, MissionTakeoffCorrectionPreservesPlannedDesti
|
||||
planned_land.nav_cmd = NAV_CMD_VTOL_LAND;
|
||||
planned_land.altitude = kGroundAltitude;
|
||||
|
||||
DatamanClient dataman;
|
||||
ASSERT_TRUE(dataman.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0));
|
||||
ASSERT_TRUE(dataman.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0,
|
||||
reinterpret_cast<uint8_t *>(&planned_takeoff), sizeof(planned_takeoff)));
|
||||
|
||||
Reference in new issue
Block a user