fix(navigator): report a failed geofence load instead of leaving a fragment (#28603)

* fix(navigator): report a failed geofence load instead of leaving a fragment _updateFence() resets _num_polygons and rebuilds the fence from dataman one polygon at a time. It can stop early two ways: loadWait() failing for an entry, or the array resize failing to allocate. Both logged to the console and left whatever had been loaded so far in place. A fragment of a fence is worse than no fence. Missing inclusion polygons permit positions the fence excluded, missing exclusion polygons open up areas it protected, and unlike an empty fence it still looks to the operator like a fence is loaded. Drop what was loaded on either failure and report it with mavlink_log_critical and an event, so the state is unambiguous and the operator is told the fence is not active rather than finding out in flight. Reported by @lihnucs, who found the allocation path. Assisted-by: Claude:claude-opus-5[1m] Signed-off-by: Julian Oes <julian@oes.ch> * rework(navigator): use _clearFence in ~Geofence() * refactor(navigator): move new functions in hpp * fix(navigator): pub GF_STATUS_FAILED when _updateFence fails and ensure GF is reloaded if previously failed with new request with same opaque id * refactor(navigator): remove redundant check * feat(msg): add GF_STATUS_FAILED to GeofenceStatus.msg * refactor(navigator): rename _fence_updated to _fence_loaded The flag now means the requested fence was loaded successfully, and "updated" reads like a uORB or dataman change. Default it to false since nothing is loaded at boot. Assisted-by: Claude:claude-opus-5 Signed-off-by: Julian Oes <julian@oes.ch> --------- Signed-off-by: Julian Oes <julian@oes.ch> Co-authored-by: jonas <jonas.perolini@rigi.tech>
2026-10-06 09:02:52 +08:00 · 2026-09-17 10:04:40 +12:00
parent fd1ddbc22e
commit 75cb93a7e7
3 changed files with 74 additions and 25 deletions
@@ -5,3 +5,4 @@ uint8 status 				# Current geofence status

 uint8 GF_STATUS_LOADING = 0
 uint8 GF_STATUS_READY = 1
+uint8 GF_STATUS_FAILED = 2
@@ -90,9 +90,7 @@ Geofence::Geofence(Navigator *navigator) :

 Geofence::~Geofence()
 {
-	if (_polygons) {
-		delete[](_polygons);
-	}
+	_clearFence();
 }

 void Geofence::run()
@@ -141,15 +139,23 @@ void Geofence::run()
 				_error_state = DatamanState::ReadWait;
 				_dataman_state = DatamanState::Error;

-			} else if (_opaque_id != _stats.opaque_id) {
+			} else if (_opaque_id != _stats.opaque_id || !_fence_loaded) {

 				_opaque_id = _stats.opaque_id;
-				_fence_updated = false;
+				_fence_loaded = false;

 				_dataman_cache.invalidate();

 				if (_dataman_cache.size() != _stats.num_items) {
 					_dataman_cache.resize(_stats.num_items);
+
+					// A failed allocation leaves the previous cache size unchanged.
+					if (_dataman_cache.size() != _stats.num_items) {
+						PX4_ERR("cache size %i does not match %i items", _dataman_cache.size(), static_cast<int>(_stats.num_items));
+						_clearFence();
+						_finishFenceUpdate(false);
+						break;
+					}
 				}

 				for (int index = 0; index < _dataman_cache.size(); ++index) {
@@ -160,7 +166,7 @@ void Geofence::run()

 			} else {
 				_dataman_state = DatamanState::UpdateRequestWait;
-				_fence_updated = true;
+				_fence_loaded = true;

 				geofence_status_s status{};
 				status.timestamp = hrt_absolute_time();
@@ -178,18 +184,7 @@ void Geofence::run()
 		_dataman_cache.update();

 		if (!_dataman_cache.isLoading()) {
-			_dataman_state = DatamanState::UpdateRequestWait;
-			_updateFence();
-			_fence_updated = true;
-
-			geofence_status_s status{};
-			status.timestamp = hrt_absolute_time();
-			status.geofence_id = _opaque_id;
-			status.status = geofence_status_s::GF_STATUS_READY;
-
-			_geofence_status_pub.publish(status);
-
-			_geofence_updated = true;
+			_finishFenceUpdate(_updateFence());
 		}

 		break;
@@ -210,7 +205,39 @@ void Geofence::updateFence()
 	_initiate_fence_updated = true;
 }

-void Geofence::_updateFence()
+void Geofence::_finishFenceUpdate(bool success)
+{
+	_dataman_state = DatamanState::UpdateRequestWait;
+	_fence_loaded = success;
+
+	if (!success) {
+		_reportFenceLoadFailure();
+	}
+
+	geofence_status_s status{};
+	status.timestamp = hrt_absolute_time();
+	status.geofence_id = _opaque_id;
+	status.status = success ? geofence_status_s::GF_STATUS_READY : geofence_status_s::GF_STATUS_FAILED;
+	_geofence_status_pub.publish(status);
+
+	_geofence_updated = true;
+}
+
+void Geofence::_clearFence()
+{
+	delete[](_polygons);
+	_polygons = nullptr;
+	_num_polygons = 0;
+}
+
+void Geofence::_reportFenceLoadFailure()
+{
+	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence load failed, fence is not active\t");
+	events::send(events::ID("navigator_geofence_load_failed"), {events::Log::Critical, events::LogInternal::Warning},
+		     "Geofence load failed, fence is not active");
+}
+
+bool Geofence::_updateFence()
 {
 	mission_fence_point_s mission_fence_point;
 	bool is_circle_area = false;
@@ -227,7 +254,11 @@ void Geofence::_updateFence()

 		if (!success) {
 			PX4_ERR("loadWait failed, seq: %i", current_seq);
-			break;
+			// A fragment of a fence is worse than none: missing inclusion polygons permit
+			// positions the fence excluded, missing exclusion polygons open up areas it
+			// protected, and it still looks to the operator like a fence is loaded.
+			_clearFence();
+			return false;
 		}

 		switch (mission_fence_point.nav_cmd) {
@@ -264,9 +295,9 @@ void Geofence::_updateFence()
 				}

 				if (!_polygons) {
-					_num_polygons = 0;
 					PX4_ERR("alloc failed");
-					return;
+					_clearFence();
+					return false;
 				}

 				PolygonInfo &polygon = _polygons[_num_polygons];
@@ -303,6 +334,8 @@ void Geofence::_updateFence()
 			break;
 		}
 	}
+
+	return true;
 }

 bool Geofence::checkHomeRequirementsForGeofence(const PolygonInfo &polygon)
@@ -138,7 +138,7 @@ public:
 	 */
 	int loadFromFile(const char *filename);

-	bool isEmpty() { return (!_fence_updated || (_num_polygons == 0)); }
+	bool isEmpty() { return (!_fence_loaded || (_num_polygons == 0)); }

 	int getSource() { return _param_gf_source.get(); }
 	int getGeofenceAction() { return _param_gf_action.get(); }
@@ -191,7 +191,7 @@ private:
 	MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]

 	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 _fence_loaded{false};  ///< true if the requested fence was successfully loaded
 	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

@@ -199,9 +199,24 @@ private:

 	/**
 	 * implementation of updateFence()
+	 * @return false if the fence failed to load and was cleared
 	 */
-	void _updateFence();
+	bool _updateFence();

+	/**
+	 * Finish a fence update, report its result, and notify the avoidance planner.
+	 */
+	void _finishFenceUpdate(bool success);
+
+	/**
+	 * Free the loaded polygons and leave the fence empty.
+	 */
+	void _clearFence();
+
+	/**
+	 * Tell the operator that the fence failed to load and is not active.
+	 */
+	void _reportFenceLoadFailure();

 	/**
 	 * Check if a single point is within a polygon