include start into planned path to avoid corner cases when rtl is engaged

outside of geofence

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

src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp

@@ -142,12 +142,14 @@ TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone)
 
 	PlannedPath path = _planner.planPath();
 
-	ASSERT_EQ(path.num_points, 2);
-	// The optimal path is via vertex 1 and 2
-	EXPECT_NEAR(path.points[0](0), vertices[1](0), 1e-3);
-	EXPECT_NEAR(path.points[0](1), vertices[1](1), 1e-3);
-	EXPECT_NEAR(path.points[1](0), vertices[2](0), 1e-3);
-	EXPECT_NEAR(path.points[1](1), vertices[2](1), 1e-3);
+	// 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);
 }
 
 TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone)
@@ -173,9 +175,11 @@ TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone)
 
 	PlannedPath path = _planner.planPath();
 
-	ASSERT_EQ(path.num_points, 1);
-	ASSERT_NEAR(path.points[0](0), vertices[5](0), 1e-3);
-	ASSERT_NEAR(path.points[0](1), vertices[5](1), 1e-3);
+	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);
 }
 TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion)
 {

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -120,7 +120,7 @@ PlannedPath GeofenceAvoidancePlanner::planPath()
 		_graph_nodes[graph_index].visited = true;
 	}
 
-	// figure out the path by backtracking from destination to start
+	// backtrack from destination to start and count the nodes along the way
 	int count = 0;
 	int idx = graph_index;
 
@@ -135,10 +135,10 @@ PlannedPath GeofenceAvoidancePlanner::planPath()
 		idx = _graph_nodes[idx].previous_index;
 	}
 
+	path.num_points = count;
 
-	path.num_points = count - 1;
-
-	// Walk backwards again and fill points in reverse, converting local to lat/lon
+	// 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
 

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -47,7 +47,7 @@ static constexpr int kMaxNodes = 100;
 static constexpr int kCircleApproxVertices = 8;
 
 struct PlannedPath {
-	// does not include the start and the end, just intermediate points
+	// points[0] is the start (latched valid position); intermediate waypoints follow; destination is not included
 	matrix::Vector2<double> points[kMaxNodes];
 	int num_points{0};
 	int current_index{0};

src/modules/navigator/geofence.h

@@ -154,7 +154,7 @@ public:
 		_avoidance_planner.update_destination(destination, *this);
 	}
 
-	PlannedPath planPath() { return _avoidance_planner.planPath(); };
+	PlannedPath planPath() { return _avoidance_planner.planPath(); }
 
 	/**
 	 * print Geofence status to the console

src/modules/navigator/navigator.h

@@ -322,10 +322,7 @@ public:
 		_geofence.updateDestinationForRTLPathPlanner(destination);
 	}
 
-	PlannedPath planPath()
-	{
-		return _geofence.planPath();
-	}
+	PlannedPath planPath() { return _geofence.planPath(); }
 
 	/**
 	 * Returns the last position that was confirmed to be inside all geofences.

src/modules/navigator/navigator_main.cpp

@@ -1039,7 +1039,7 @@ 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_valid_position_in_fence = matrix::Vector2<double> {current_latitude, current_longitude};
 		}
 
 		_geofence_result.timestamp = hrt_absolute_time();
@@ -1630,7 +1630,7 @@ matrix::Vector2<double> Navigator::getRtlPlanningStart()
 		return _last_valid_position_in_fence;
 	}
 
-	return matrix::Vector2<double>{_global_pos.lat, _global_pos.lon};
+	return matrix::Vector2<double> {_global_pos.lat, _global_pos.lon};
 }
 
 void Navigator::preproject_stop_point(double &lat, double &lon)