integrate dijkstra lib into avoidance planner

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp

@@ -96,12 +96,11 @@ TEST_F(GeofenceAvoidancePlannerTest, StartEqualsDestination)
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(point, fake);
 	_planner.update_destination(point, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(point, fake);
 
-	ASSERT_EQ(path.num_points, 0);
+	ASSERT_EQ(num_waypoints, 0);
 }
 
 TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence)
@@ -112,18 +111,17 @@ TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence)
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	ASSERT_EQ(path.num_points, 0);
+	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 2 and vertex nr 3, so the path should have two points
+	// 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);
@@ -137,19 +135,23 @@ TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone)
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	// points[0] is the start itself (always emitted); optimal detour is via vertex 1 and 2
-	ASSERT_EQ(path.num_points, 3);
-	EXPECT_NEAR(path.points[0](0), start(0), 1e-3);
-	EXPECT_NEAR(path.points[0](1), start(1), 1e-3);
-	EXPECT_NEAR(path.points[1](0), vertices[1](0), 1e-3);
-	EXPECT_NEAR(path.points[1](1), vertices[1](1), 1e-3);
-	EXPECT_NEAR(path.points[2](0), vertices[2](0), 1e-3);
-	EXPECT_NEAR(path.points[2](1), vertices[2](1), 1e-3);
+	// 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.get_point_at_index(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.get_point_at_index(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)
@@ -170,17 +172,19 @@ TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone)
 
 	FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	ASSERT_EQ(path.num_points, 2);
-	ASSERT_NEAR(path.points[0](0), start(0), 1e-3);
-	ASSERT_NEAR(path.points[0](1), start(1), 1e-3);
-	ASSERT_NEAR(path.points[1](0), vertices[5](0), 1e-3);
-	ASSERT_NEAR(path.points[1](1), vertices[5](1), 1e-3);
+	// 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.get_point_at_index(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;
@@ -199,13 +203,13 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion)
 
 	FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	ASSERT_EQ(path.num_points, 0);
+	ASSERT_EQ(num_waypoints, 0);
 }
+
 TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion)
 {
 	using namespace matrix;
@@ -223,13 +227,13 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion)
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	ASSERT_EQ(path.num_points, 0);
+	ASSERT_EQ(num_waypoints, 0);
 }
+
 TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
 {
 	using namespace matrix;
@@ -238,7 +242,6 @@ TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
 
 	Vector2<double> destination(47.3559582, 8.5192064);
 
-
 	static const Vector2<double> vertices[] = {
 		{47.3552420, 8.5192293},
 		{47.3555843, 8.5201174},
@@ -248,13 +251,45 @@ TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
-	ASSERT_EQ(path.num_points, 0);
+	ASSERT_EQ(num_waypoints, 0);
 }
+
+TEST_F(GeofenceAvoidancePlannerTest, StartInsideGeofence)
+{
+	using namespace matrix;
+	// Start and destination are both in valid airspace (north of the exclusion zone), so the
+	// destination is directly reachable from the start without any detour.
+	Vector2<double> start(47.3559582, 8.5192064);
+	Vector2<double> destination(47.3560100, 8.5192300);
+
+	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.update_vertices(fake);
+	_planner.update_destination(destination, fake);
+
+	// Vehicle was outside the fence at plan time, so the start is used as an anchor the
+	// vehicle must fly back to. Even though the destination is directly visible from there,
+	// the path still has to include the anchor as the first (and only) waypoint.
+	const int num_waypoints =
+		_planner.set_start_and_plan_path_to_destination(start, fake, /*start_is_current_position=*/false);
+
+	ASSERT_EQ(num_waypoints, 1);
+
+	const Vector2d anchor_wp = _planner.get_point_at_index(0);
+	EXPECT_NEAR(anchor_wp(0), start(0), 1e-3);
+	EXPECT_NEAR(anchor_wp(1), start(1), 1e-3);
+}
+
 TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination)
 {
 	using namespace matrix;
@@ -266,15 +301,14 @@ TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination)
 	_planner.update_vertices(fake);
 
 	auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
-		_planner.update_start(s, fake);
 		_planner.update_destination(d, fake);
-		return _planner.planPath();
+		return _planner.set_start_and_plan_path_to_destination(s, fake);
 	};
 
-	EXPECT_EQ(plan(nan_lat, valid).num_points, 0);
-	EXPECT_EQ(plan(nan_lon, valid).num_points, 0);
-	EXPECT_EQ(plan(valid, nan_lat).num_points, 0);
-	EXPECT_EQ(plan(valid, nan_lon).num_points, 0);
+	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)
@@ -290,19 +324,18 @@ TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds)
 	_planner.update_vertices(fake);
 
 	auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
-		_planner.update_start(s, fake);
 		_planner.update_destination(d, fake);
-		return _planner.planPath();
+		return _planner.set_start_and_plan_path_to_destination(s, fake);
 	};
 
-	EXPECT_EQ(plan(lat_too_high, valid).num_points, 0);
-	EXPECT_EQ(plan(lat_too_low, valid).num_points, 0);
-	EXPECT_EQ(plan(lon_too_high, valid).num_points, 0);
-	EXPECT_EQ(plan(lon_too_low, valid).num_points, 0);
-	EXPECT_EQ(plan(valid, lat_too_high).num_points, 0);
-	EXPECT_EQ(plan(valid, lat_too_low).num_points, 0);
-	EXPECT_EQ(plan(valid, lon_too_high).num_points, 0);
-	EXPECT_EQ(plan(valid, lon_too_low).num_points, 0);
+	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)
@@ -326,11 +359,10 @@ TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex)
 
 	FakeGeofence fake(vertices, 7, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
 	_planner.update_vertices(fake);
-	_planner.update_start(start, fake);
 	_planner.update_destination(destination, fake);
 
-	PlannedPath path = _planner.planPath();
+	const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake);
 
 	// THEN the pathplanner should fail
-	ASSERT_EQ(path.num_points, 0);
+	ASSERT_EQ(num_waypoints, 0);
 }

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -34,12 +34,12 @@
 #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(_setup_perf);
 	perf_free(_setup_distances_perf);
-	perf_free(_update_start_perf);
 	perf_free(_update_destination_perf);
 	perf_free(_plan_path_perf);
 }
@@ -55,104 +55,22 @@ static matrix::Vector2f get_vertex_local_position(int poly_index, int vertex_idx
 	return local;
 }
 
-const PlannedPath &GeofenceAvoidancePlanner::planPath()
+void GeofenceAvoidancePlanner::planPath()
 {
-	// The navigator task stack is only ~2 KB, so we keep the result as a member
-	// and return it by reference to avoid overflowing the stack on RTL entry.
-	_planned_path.num_points = 0;
-	_planned_path.current_index = 0;
-
-	if (!_polygons_healthy || !_start_healthy || !_destination_healthy) {
+	if (!_polygons_healthy || !_destination_healthy) {
 		// we are not in a state where we can reliably plan a path, return an empty one
-		return _planned_path;
+		return;
 	}
 
 	perf_begin(_plan_path_perf);
 
-	reset_graph_state();
 
-	// reset to the start location
-	int graph_index = 0;
+	dijkstra::solveBackward(_num_nodes, _num_nodes - 1, _distances, true, _best_distance,
+				_next_node_buffer, _visited_buffer);
 
-	while (true) {
-		// update the best distances to all neighbouring nodes
-		float min_dist = INFINITY;
-		int next_graph_index = -1;
-
-		for (int i = 0; i < _num_nodes; i++) {
-			if (i == graph_index) {
-				continue;
-			}
-
-			const float distance_to_neighbor = _distances[geofence_utils::symmetricPairIndex(graph_index, i, _num_nodes)];
-
-			if (distance_to_neighbor < INFINITY) {
-				const float total_distance_to_neighbor = distance_to_neighbor + _graph_nodes[graph_index].best_distance;
-
-				if (total_distance_to_neighbor < _graph_nodes[i].best_distance) {
-					_graph_nodes[i].best_distance = total_distance_to_neighbor;
-					_graph_nodes[i].previous_index = graph_index;
-				}
-			}
-
-			if (!_graph_nodes[i].visited && _graph_nodes[i].best_distance < min_dist) {
-				min_dist = _graph_nodes[i].best_distance;
-				next_graph_index = i;
-			}
-		}
-
-		graph_index = next_graph_index;
-
-		if (graph_index < 0) {
-			// we are stuck, destination does not seem to be reachable
-			perf_end(_plan_path_perf);
-			return _planned_path;
-		}
-
-		_graph_nodes[graph_index].visited = true;
-
-		if (_graph_nodes[graph_index].type == Node::DESTINATION) {
-			break;
-		}
-
-	}
-
-	// backtrack from destination to start and count the nodes along the way
-	int count = 0;
-	int idx = graph_index;
-
-	while (idx != 0) {
-		if (idx < 0) {
-			// can happen if the destination is not reachable
-			perf_end(_plan_path_perf);
-			return _planned_path;
-		}
-
-		count++;
-		idx = _graph_nodes[idx].previous_index;
-	}
-
-	_planned_path.num_points = count;
-
-	// Walk backwards and fill points in reverse, converting local to lat/lon.
-	// The last iteration lands on the start node, which becomes points[0].
-	MapProjection ref{_reference(0), _reference(1)};
-	idx = _graph_nodes[graph_index].previous_index; // skip destination
-
-	for (int i = _planned_path.num_points - 1; i >= 0; i--) {
-		if (idx < 0) {
-			// this should never happen, but just in case, we return an empty path
-			_planned_path.num_points = 0;
-			break;
-		}
-
-		ref.reproject(_graph_nodes[idx].position(0), _graph_nodes[idx].position(1),
-			      _planned_path.points[i](0), _planned_path.points[i](1));
-		idx = _graph_nodes[idx].previous_index;
-	}
 
 	perf_end(_plan_path_perf);
-	return _planned_path;
+	return;
 }
 
 bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, float margin)
@@ -174,7 +92,7 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa
 		}
 	}
 
-	if (num_vertices == 0 || num_vertices > kMaxNodes - 2) { // -2 to reserve start and destination slots
+	if (num_vertices == 0 || num_vertices > kMaxNodes - 1) { // -1 to reserve the destination slot
 		_polygons_healthy = false;
 		return false;
 	}
@@ -191,22 +109,21 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa
 
 	update_distances_between_vertices(geofence);
 
-	// invalidate start and end to make sure they are refreshed
-	_start_healthy = false;
+	// invalidate destination to force a refresh on the next update_destination()
 	_destination_healthy = false;
 
 	perf_end(_setup_perf);
+
+	planPath();
 	return true;
 }
 
 bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
 	GeofenceInterface &geofence, float margin)
 {
-	// local frame is anchored at _reference (set by update_vertices); this can be computed
-	// once up-front; start and destination nodes are filled in later when they are known
 	const int num_polygons = geofence.getNumPolygons();
 
-	int node_index = 1; // reserve index 0 for the start
+	int node_index = 0;
 
 	for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
 
@@ -229,11 +146,7 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
 			}
 
 			for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
-				_graph_nodes[node_index].type = Node::Type::VERTEX;
-				_graph_nodes[node_index].position = local_out[vertex_idx];
-				_graph_nodes[node_index].best_distance = FLT_MAX;
-				_graph_nodes[node_index].previous_index = -1;
-				_graph_nodes[node_index].visited = false;
+				_positions[node_index] = local_out[vertex_idx];
 				node_index++;
 			}
 
@@ -256,18 +169,14 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
 
 			for (int i = 0; i < kCircleApproxVertices; i++) {
 				const float angle = i * delta_angle;
-				_graph_nodes[node_index].type = Node::Type::VERTEX;
-				_graph_nodes[node_index].position = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)};
-				_graph_nodes[node_index].best_distance = FLT_MAX;
-				_graph_nodes[node_index].previous_index = -1;
-				_graph_nodes[node_index].visited = false;
+				_positions[node_index] = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)};
 				node_index++;
 			}
 		}
 	}
 
-	// node_index now points at the reserved destination slot; total count includes it
-	_num_vertices = node_index - 1;
+	// node_index equals the polygon vertex count; the destination is reserved one slot beyond
+	_num_vertices = node_index;
 	_num_nodes = node_index + 1;	// +1 for the destination
 
 	return true;
@@ -276,20 +185,17 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
 void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterface &geofence)
 {
 	perf_begin(_setup_distances_perf);
-	// vertices occupy indices 1 .. _num_vertices; index 0 and the last slot hold the
-	// start and destination, which are handled by update_start / update_destination
-	const int last_vertex = _num_vertices;
 
 	// loop through all possible vertex to vertex combinations and store the distance between them
-	for (int i = 1; i <= last_vertex; i++) {
-		for (int j = i + 1; j <= last_vertex; j++) {
+	for (int i = 0; i < _num_vertices; i++) {
+		for (int j = i + 1; j < _num_vertices; j++) {
 			const size_t idx = geofence_utils::symmetricPairIndex(i, j, _num_nodes);
 
-			const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[i].position,
-					   _graph_nodes[j].position, _reference);
+			const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[i],
+					   _positions[j], _reference);
 
 			if (clear) {
-				_distances[idx] = (_graph_nodes[i].position - _graph_nodes[j].position).norm();
+				_distances[idx] = (_positions[i] - _positions[j]).norm();
 
 			} else {
 				_distances[idx] = INFINITY;
@@ -300,56 +206,6 @@ void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterfa
 	perf_end(_setup_distances_perf);
 }
 
-void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, GeofenceInterface &geofence)
-{
-	if (!start.isAllFinite() || !lat_lon_within_bounds(start) || !_polygons_healthy) {
-		_start_healthy = false;
-		return;
-	}
-
-	perf_begin(_update_start_perf);
-
-	MapProjection ref{_reference(0), _reference(1)};
-	matrix::Vector2f start_local;
-	ref.project(start(0), start(1), start_local(0), start_local(1));
-
-	_graph_nodes[0].position = start_local;
-
-	// distances from start to each vertex
-	for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) {
-		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, graph_idx, _num_nodes);
-
-		const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position,
-				   _graph_nodes[graph_idx].position, _reference);
-
-		if (clear) {
-			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[graph_idx].position).norm();
-
-		} else {
-			_distances[dist_idx] = INFINITY;
-		}
-	}
-
-	// refresh start <-> destination edge if destination is already known
-	if (_destination_healthy) {
-		const int dest_idx = _num_nodes - 1;
-		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes);
-
-		const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position,
-				   _graph_nodes[dest_idx].position, _reference);
-
-		if (clear) {
-			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm();
-
-		} else {
-			_distances[dist_idx] = INFINITY;
-		}
-	}
-
-	_start_healthy = true;
-	perf_end(_update_start_perf);
-}
-
 void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destination, GeofenceInterface &geofence)
 {
 	if (!destination.isAllFinite() || !lat_lon_within_bounds(destination) || !_polygons_healthy) {
@@ -363,38 +219,23 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
 
 	const int dest_idx = _num_nodes - 1;
 
-	if (_destination_healthy && (dest_local - _graph_nodes[dest_idx].position).norm() < FLT_EPSILON) {
+	if (_destination_healthy && (dest_local - _positions[dest_idx]).norm() < FLT_EPSILON) {
 		// no change in destination, skip the update
 		return;
 	}
 
 	perf_begin(_update_destination_perf);
-	_graph_nodes[dest_idx].position = dest_local;
+	_positions[dest_idx] = dest_local;
 
 	// distances from each vertex to destination
-	for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) {
+	for (int graph_idx = 0; graph_idx < _num_vertices; graph_idx++) {
 		const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, dest_idx, _num_nodes);
 
-		const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[graph_idx].position,
-				   _graph_nodes[dest_idx].position, _reference);
+		const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[graph_idx],
+				   _positions[dest_idx], _reference);
 
 		if (clear) {
-			_distances[dist_idx] = (_graph_nodes[graph_idx].position - _graph_nodes[dest_idx].position).norm();
-
-		} else {
-			_distances[dist_idx] = INFINITY;
-		}
-	}
-
-	// refresh start <-> destination edge if start is already known
-	if (_start_healthy) {
-		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes);
-
-		const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position,
-				   _graph_nodes[dest_idx].position, _reference);
-
-		if (clear) {
-			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm();
+			_distances[dist_idx] = (_positions[graph_idx] - _positions[dest_idx]).norm();
 
 		} else {
 			_distances[dist_idx] = INFINITY;
@@ -403,6 +244,8 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
 
 	_destination_healthy = true;
 	perf_end(_update_destination_perf);
+
+	planPath();
 }
 
 bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon)
@@ -414,21 +257,105 @@ bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2<doubl
 	return true;
 }
 
-void GeofenceAvoidancePlanner::reset_graph_state()
+int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vector2d start, GeofenceInterface &geofence,
+		bool start_is_current_position)
 {
+	_start_is_current_position = start_is_current_position;
 
-	_graph_nodes[0].best_distance = 0.0f;
-	_graph_nodes[0].previous_index = 0;
-	_graph_nodes[0].visited = true;
-
-	for (int i = 1; i <= _num_vertices; i++) {
-		_graph_nodes[i].best_distance = FLT_MAX;
-		_graph_nodes[i].previous_index = -1;
-		_graph_nodes[i].visited = false;
+	if (!_polygons_healthy || !_destination_healthy) {
+		return 0;
 	}
 
-	_graph_nodes[_num_nodes - 1].best_distance = FLT_MAX;
-	_graph_nodes[_num_nodes - 1].previous_index = -1;
-	_graph_nodes[_num_nodes - 1].visited = false;
-	_graph_nodes[_num_nodes - 1].type = Node::Type::DESTINATION;
+	MapProjection ref{_reference(0), _reference(1)};
+	ref.project(start(0), start(1), _start_local(0), _start_local(1));
+
+	float best_cost = INFINITY;
+
+	_best_starting_index = -1;
+
+	for (int node_idx = 0; node_idx < _num_nodes; node_idx++) {
+		// check if node is reachable from this position
+		if (!geofence.checkIfLineViolatesAnyFence(_start_local, _positions[node_idx], _reference)) {
+
+			if (node_idx == _num_nodes - 1) {
+				// destination is directly reachable from start: no detour planning needed.
+				// If the vehicle is already at the start, no waypoints at all are needed.
+				// Otherwise we still need the anchor (point 0) so the vehicle flies back into the fence first.
+				_best_starting_index = -1;
+				_num_path_points = 0;
+				return start_is_current_position ? 0 : 1;
+			}
+
+			const float cost = (_start_local - _positions[node_idx]).norm() + _best_distance[node_idx];
+
+			if (cost < best_cost) {
+				best_cost = cost;
+				_best_starting_index = node_idx;
+			}
+		}
+	}
+
+	int current_index = _best_starting_index;
+	_num_path_points = 0;
+
+	if (current_index < 0) {
+		// no reachable path to destination
+		return 0;
+	}
+
+	for (int idx = 0; idx < _num_nodes; idx++) {
+		int next = _next_node_buffer[current_index];
+		_num_path_points++;
+
+		if (next == _num_nodes - 1 || next < 0) {
+			break;
+		}
+
+		current_index = next;
+	}
+
+	// When the vehicle is already at the start, the start itself is not a waypoint to fly to.
+	// Otherwise the start is the anchor (point 0) the vehicle has to fly back to first.
+	return start_is_current_position ? _num_path_points : _num_path_points + 1;
+}
+
+matrix::Vector2d GeofenceAvoidancePlanner::get_point_at_index(int index) const
+{
+	const matrix::Vector2d nan_point{(double)NAN, (double)NAN};
+
+	// When start_is_current_position == false, the start (anchor) occupies index 0 of the path
+	// and intermediate vertices follow at index 1..N. When true, the start is omitted and the
+	// intermediate vertices start at index 0.
+	const int anchor_offset = _start_is_current_position ? 0 : 1;
+	const int total_points = _num_path_points + anchor_offset;
+
+	if (index < 0 || index >= total_points) {
+		return nan_point;
+	}
+
+	if (anchor_offset == 1 && index == 0) {
+		matrix::Vector2d start_global;
+		MapProjection ref{_reference(0), _reference(1)};
+		ref.reproject(_start_local(0), _start_local(1), start_global(0), start_global(1));
+		return start_global;
+	}
+
+	if (_best_starting_index < 0) {
+		return nan_point;
+	}
+
+	int next = _best_starting_index;
+
+	for (int i = 0; i < index - anchor_offset; i++) {
+		next = _next_node_buffer[next];
+	}
+
+	if (next < 0 || next >= _num_nodes) {
+		return nan_point;
+	}
+
+	matrix::Vector2d global;
+	MapProjection ref{_reference(0), _reference(1)};
+	ref.reproject(_positions[next](0), _positions[next](1), global(0), global(1));
+	return global;
 }

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -119,37 +119,52 @@ public:
 	GeofenceAvoidancePlanner() = default;
 	~GeofenceAvoidancePlanner();
 
-	const PlannedPath &planPath();
 
-	bool update_vertices(GeofenceInterface &geofence, float margin = 10.0f);
-	void update_start(const matrix::Vector2d &start, GeofenceInterface &geofence);
+	int set_start_and_plan_path_to_destination(matrix::Vector2d start, GeofenceInterface &geofence,
+			bool start_is_current_position = true);
+
+	matrix::Vector2d get_point_at_index(int index) const;
+
+	// True if the start passed to the last plan call was the vehicle's current position. False means
+	// planning was anchored to a stored point (e.g. last valid in-fence position when the vehicle is
+	// outside the fence), so the leg from current position to point 0 is unaccounted for in the path.
+	bool start_is_current_position() const { return _start_is_current_position; }
+
+	bool update_vertices(GeofenceInterface &geofence, float margin = 50.0f);
+
 	void update_destination(const matrix::Vector2d &destination, GeofenceInterface &geofence);
 
 private:
 
 	static constexpr int num_distances_in_graph = (kMaxNodes) * (kMaxNodes - 1) / 2;
 
-	Node _graph_nodes[kMaxNodes];
+	float _best_distance[kMaxNodes];
 	float _distances[num_distances_in_graph];
-	PlannedPath _planned_path{};
+	int _next_node_buffer[kMaxNodes];
+	bool _visited_buffer[kMaxNodes];
+
+	int _best_starting_index{-1};
+	int _num_path_points{0};
+
+	matrix::Vector2f _start_local;
+
+	matrix::Vector2f _positions[kMaxNodes];
 	int _num_nodes{0};
 	int _num_vertices{0};
 	matrix::Vector2<double> _reference; // lat/lon anchor of the local frame
 
 	bool _polygons_healthy{false};
-	bool _start_healthy{false};
 	bool _destination_healthy{false};
+	bool _start_is_current_position{true};
 
 	perf_counter_t _setup_perf{perf_alloc(PC_ELAPSED, "rtl_planner: setup")};
 	perf_counter_t _setup_distances_perf{perf_alloc(PC_ELAPSED, "rtl_planner: setup distances")};
-	perf_counter_t _update_start_perf{perf_alloc(PC_ELAPSED, "rtl_planner: update start")};
 	perf_counter_t _update_destination_perf{perf_alloc(PC_ELAPSED, "rtl_planner: update destination")};
 	perf_counter_t _plan_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: plan path")};
 
 	bool update_graph_nodes_without_start_and_destination(GeofenceInterface &geofence, float margin);
 	void update_distances_between_vertices(GeofenceInterface &geofence);
-
-	void reset_graph_state();
+	void planPath();
 
 	bool lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon);
 

src/modules/navigator/geofence.h

@@ -144,9 +144,14 @@ public:
 
 	bool isHomeRequired();
 
-	void updateStartForRTLPathPlanner(const matrix::Vector2<double> &start)
+	int set_start_and_start_path_to_destination(const matrix::Vector2<double> &start, bool start_is_current_position = true)
 	{
-		_avoidance_planner.update_start(start, *this);
+		return _avoidance_planner.set_start_and_plan_path_to_destination(start, *this, start_is_current_position);
+	}
+
+	bool plannerStartIsCurrentPosition() const
+	{
+		return _avoidance_planner.start_is_current_position();
 	}
 
 	void updateDestinationForRTLPathPlanner(const matrix::Vector2<double> &destination)
@@ -154,7 +159,11 @@ public:
 		_avoidance_planner.update_destination(destination, *this);
 	}
 
-	const PlannedPath &planPath() { return _avoidance_planner.planPath(); }
+	const matrix::Vector2d get_point_at_index(int index) const
+	{
+		return _avoidance_planner.get_point_at_index(index);
+
+	}
 
 	/**
 	 * print Geofence status to the console

src/modules/navigator/navigator.h

@@ -312,9 +312,9 @@ public:
 
 	void trigger_hagl_failsafe(uint8_t nav_state);
 
-	void updateStartOfRTLPathPlanner(const matrix::Vector2<double> &start)
+	int set_start_and_plan_path_to_destination(const matrix::Vector2<double> &start, bool start_is_current_position = true)
 	{
-		_geofence.updateStartForRTLPathPlanner(start);
+		return _geofence.set_start_and_start_path_to_destination(start, start_is_current_position);
 	}
 
 	void updateDestinationOfRTLPathPlanner(const matrix::Vector2<double> &destination)
@@ -322,7 +322,15 @@ public:
 		_geofence.updateDestinationForRTLPathPlanner(destination);
 	}
 
-	const PlannedPath &planPath() { return _geofence.planPath(); }
+	matrix::Vector2d get_point_at_index(int index) const
+	{
+		return _geofence.get_point_at_index(index);
+	}
+
+	bool geofencePlannerStartIsCurrentPosition() const
+	{
+		return _geofence.plannerStartIsCurrentPosition();
+	}
 
 	/**
 	 * Returns the last position that was confirmed to be inside all geofences.
@@ -331,6 +339,8 @@ public:
 	 */
 	matrix::Vector2<double> getRtlPlanningStart();
 
+	bool was_last_position_outside_fence() const { return _last_position_was_outside_fence; }
+
 private:
 
 	int _local_pos_sub{-1};
@@ -386,6 +396,7 @@ private:
 	hrt_abstime _last_geofence_check{0};
 
 	matrix::Vector2d _last_valid_position_in_fence{static_cast<double>(NAN), static_cast<double>(NAN)};
+	bool _last_position_was_outside_fence{false};
 
 	bool _navigator_status_updated{false};
 	hrt_abstime _last_navigator_status_publication{0};

src/modules/navigator/navigator_main.cpp

@@ -1040,6 +1040,10 @@ void Navigator::geofence_breach_check()
 		// latch the current position as a valid RTL planner start whenever no fence is violated
 		if (!_geofence_result.geofence_max_dist_triggered && !_geofence_result.geofence_custom_fence_triggered) {
 			_last_valid_position_in_fence = matrix::Vector2<double> {current_latitude, current_longitude};
+			_last_position_was_outside_fence = false;
+
+		} else {
+			_last_position_was_outside_fence = true;
 		}
 
 		_geofence_result.timestamp = hrt_absolute_time();

src/modules/navigator/rtl.cpp

@@ -186,8 +186,12 @@ void RTL::on_inactive()
 				&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
 
 		if (global_position_recently_updated) {
-			_navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart());
-			_rtl_direct.updatePlannedPath();
+			if (_navigator->was_last_position_outside_fence()) {
+				_navigator->set_start_and_plan_path_to_destination(_navigator->getRtlPlanningStart(), false);
+
+			} else {
+				_navigator->set_start_and_plan_path_to_destination(matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon), true);
+			}
 		}
 
 		publishRemainingTimeEstimate();

src/modules/navigator/rtl_direct.cpp

@@ -72,12 +72,11 @@ void RtlDirect::on_activation()
 {
 	_global_pos_sub.update();
 	_vehicle_status_sub.update();
+	const bool start_is_current_position = !_navigator->was_last_position_outside_fence();
+	_num_waypoints_for_geofence_avoidance =
+		_navigator->set_start_and_plan_path_to_destination(_navigator->getRtlPlanningStart(), start_is_current_position);
 
-	_navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart());
-
-	// destination will only update if the location is different or the previous location was invalid
-	_navigator->updateDestinationOfRTLPathPlanner(matrix::Vector2d{_land_approach.lat, _land_approach.lon});
-	_geofence_aware_return_path = _navigator->planPath();
+	_current_geofence_avoidance_index = 0;
 
 	parameters_update();
 
@@ -96,7 +95,7 @@ void RtlDirect::on_activation()
 				       (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
 
 	// send out message only for the first point
-	if (_geofence_aware_return_path.num_points > 0) {
+	if (_num_waypoints_for_geofence_avoidance > 0) {
 		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");
 	}
@@ -166,7 +165,7 @@ void RtlDirect::_updateRtlState()
 
 	switch (_rtl_state) {
 	case RTLState::CLIMBING:
-		if (_geofence_aware_return_path.hasNextPoint()) {
+		if (_current_geofence_avoidance_index < _num_waypoints_for_geofence_avoidance) {
 			new_state = RTLState::AVOID_GEOFENCE;
 
 		} else {
@@ -176,7 +175,7 @@ void RtlDirect::_updateRtlState()
 		break;
 
 	case RTLState::AVOID_GEOFENCE:
-		if (_geofence_aware_return_path.hasNextPoint()) {
+		if (_current_geofence_avoidance_index <  _num_waypoints_for_geofence_avoidance) {
 			new_state = RTLState::AVOID_GEOFENCE;
 
 		} else {
@@ -264,7 +263,7 @@ void RtlDirect::set_rtl_item()
 	case RTLState::AVOID_GEOFENCE: {
 
 
-			matrix::Vector2d point = _geofence_aware_return_path.getAndPopCurrentPoint();
+			matrix::Vector2d point = _navigator->get_point_at_index(_current_geofence_avoidance_index++);
 
 			if (point.isAllNan()) {
 				// should never happen
@@ -457,7 +456,7 @@ RtlDirect::RTLState RtlDirect::getActivationState()
 	} else if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
 		activation_state = RTLState::CLIMBING;
 
-	} else if (_geofence_aware_return_path.hasNextPoint()) {
+	} else if (_current_geofence_avoidance_index < _num_waypoints_for_geofence_avoidance) {
 		activation_state = RTLState::AVOID_GEOFENCE;
 
 	} else {
@@ -502,17 +501,32 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 
 		// FALLTHROUGH
 		case RTLState::AVOID_GEOFENCE: {
-				const int num_points = _geofence_aware_return_path.num_points;
 
-				for (int i = 0; i < num_points - 1; ++i) {
-					matrix::Vector2d start = _geofence_aware_return_path.getPoint(i);
-					matrix::Vector2d end = _geofence_aware_return_path.getPoint(i + 1);
+				// If the path was planned from a stored anchor (vehicle was outside the fence at plan time),
+				// the leg from the current position back to point 0 is unaccounted for in the path itself.
+				// Add it while the vehicle has not yet reached point 0 (index <= 1: not started, or heading there).
+				if (!_navigator->geofencePlannerStartIsCurrentPosition() && _current_geofence_avoidance_index <= 1) {
+					const matrix::Vector2d current_pos{_global_pos_sub.get().lat, _global_pos_sub.get().lon};
+					const matrix::Vector2d first_waypoint = _navigator->get_point_at_index(0);
+
+					matrix::Vector2f direction{};
+					get_vector_to_next_waypoint(current_pos(0), current_pos(1), first_waypoint(0), first_waypoint(1),
+								    &direction(0), &direction(1));
+					const float dist = get_distance_to_next_waypoint(current_pos(0), current_pos(1),
+							   first_waypoint(0), first_waypoint(1));
+
+					_rtl_time_estimator.addDistance(dist, direction, 0.f);
+				}
+
+				// accumulate distances between the path waypoints, which have not been reached yet
+				for (int i = _current_geofence_avoidance_index; i < _num_waypoints_for_geofence_avoidance - 1; ++i) {
+					const matrix::Vector2d start = _navigator->get_point_at_index(i);
+					const matrix::Vector2d end = _navigator->get_point_at_index(i + 1);
 
 					matrix::Vector2f direction{};
 					get_vector_to_next_waypoint(start(0), start(1), end(0), end(1), &direction(0),
 								    &direction(1));
-					float dist{get_distance_to_next_waypoint(start(0), start(1),
-							end(0), end(1))};
+					const float dist = get_distance_to_next_waypoint(start(0), start(1), end(0), end(1));
 
 					_rtl_time_estimator.addDistance(dist, direction, 0.f);
 				}
@@ -524,8 +538,8 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 
 				float dist{0.f};
 
-				if (_geofence_aware_return_path.num_points > 0) {
-					const matrix::Vector2d last_point = _geofence_aware_return_path.getPoint(_geofence_aware_return_path.num_points - 1);
+				if (_num_waypoints_for_geofence_avoidance > 0) {
+					const matrix::Vector2d last_point = _navigator->get_point_at_index(_num_waypoints_for_geofence_avoidance);
 					get_vector_to_next_waypoint(last_point(0), last_point(1), land_approach.lat,
 								    land_approach.lon, &direction(0), &direction(1));
 					dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon);
@@ -691,8 +705,3 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
-
-void RtlDirect::updatePlannedPath()
-{
-	_geofence_aware_return_path =  _navigator->planPath();
-}

src/modules/navigator/rtl_direct.h

@@ -112,8 +112,6 @@ public:
 
 	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
 
-	void updatePlannedPath();
-
 private:
 	/**
 	 * @brief Return to launch state machine.
@@ -172,9 +170,11 @@ private:
 
 	PositionYawSetpoint _destination{(double)NAN, (double)NAN, NAN, NAN}; ///< the RTL position to fly to
 	loiter_point_s _land_approach;
-	PlannedPath _geofence_aware_return_path;
 	float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should transit to the destination
 
+	int _num_waypoints_for_geofence_avoidance{0}; ///< number of waypoints to be used for geofence avoidance in RTL
+	int _current_geofence_avoidance_index{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
 		(ParamFloat<px4::params::RTL_LAND_DELAY>)  _param_rtl_land_delay,