perf(navigator): instrument geofence avoidance planner with perf counters

Adds five PC_ELAPSED counters around the components expected to dominate
RTL path planning cost so they show up in `perf` output:

  rtl_planner: setup            - update_vertices() (fence-load entry)
  rtl_planner: setup distances  - O(n^2) line-fence checks
  rtl_planner: update start     - O(n) line checks on RTL entry
  rtl_planner: update destination
  rtl_planner: plan path        - Dijkstra in planPath()

Skips the no-work early-return paths (unhealthy state, invalid input) via
perf_cancel so reported numbers reflect actual work done.

src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt

@@ -42,5 +42,5 @@ px4_add_library(geofence_avoidance_planner
 )
 
 px4_add_functional_gtest(SRC GeofenceBreachAvoidanceTest.cpp LINKLIBS geofence_breach_avoidance modules__navigator motion_planning)
-target_link_libraries(geofence_avoidance_planner PRIVATE geo geofence_utils)
+target_link_libraries(geofence_avoidance_planner PRIVATE geo geofence_utils perf)
-px4_add_unit_gtest(SRC GeofenceAvoidancePlannerTest.cpp LINKLIBS geofence_avoidance_planner geo geofence_utils)
+px4_add_unit_gtest(SRC GeofenceAvoidancePlannerTest.cpp LINKLIBS geofence_avoidance_planner geo geofence_utils perf)

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -35,6 +35,15 @@
 #include <lib/geo/geo.h>
 #include <lib/geofence/geofence_utils.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);
+}
+
 static matrix::Vector2f get_vertex_local_position(int poly_index, int vertex_idx,
 		GeofenceInterface &geofence,
 		const matrix::Vector2<double> &reference)
@@ -58,6 +67,8 @@ const PlannedPath &GeofenceAvoidancePlanner::planPath()
 		return _planned_path;
 	}
 
+	perf_begin(_plan_path_perf);
+
 	reset_graph_state();
 
 	// reset to the start location
@@ -111,6 +122,7 @@ const PlannedPath &GeofenceAvoidancePlanner::planPath()
 
 		if (last_graph_index == graph_index) {
 			// we are stuck, destination does not seem to be reachable
+			perf_end(_plan_path_perf);
 			return _planned_path;
 		}
 
@@ -128,6 +140,7 @@ const PlannedPath &GeofenceAvoidancePlanner::planPath()
 	while (idx != 0) {
 		if (idx < 0) {
 			// can happen if the destination is not reachable
+			perf_end(_plan_path_perf);
 			return _planned_path;
 		}
 
@@ -154,6 +167,7 @@ const PlannedPath &GeofenceAvoidancePlanner::planPath()
 		idx = _graph_nodes[idx].previous_index;
 	}
 
+	perf_end(_plan_path_perf);
 	return _planned_path;
 }
 
@@ -181,14 +195,18 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa
 		return false;
 	}
 
+	perf_begin(_setup_perf);
+
 	_reference = geofence.getPolygonVertexByIndex(0, 0);
 
 	if (!update_graph_nodes_without_start_and_destination(geofence, margin)) {
 		_polygons_healthy = false;
+		perf_cancel(_setup_perf);
 		return false;
 	}
 
 	update_distances_between_vertices(geofence);
+	perf_end(_setup_perf);
 	return true;
 }
 
@@ -268,6 +286,7 @@ 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;
@@ -288,6 +307,8 @@ void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterfa
 			}
 		}
 	}
+
+	perf_end(_setup_distances_perf);
 }
 
 void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, GeofenceInterface &geofence)
@@ -297,6 +318,8 @@ void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, Geofe
 		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));
@@ -335,6 +358,7 @@ void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, Geofe
 	}
 
 	_start_healthy = true;
+	perf_end(_update_start_perf);
 }
 
 void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destination, GeofenceInterface &geofence)
@@ -344,6 +368,8 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
 		return;
 	}
 
+	perf_begin(_update_destination_perf);
+
 	MapProjection ref{_reference(0), _reference(1)};
 	matrix::Vector2f dest_local;
 	ref.project(destination(0), destination(1), dest_local(0), dest_local(1));
@@ -382,6 +408,7 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
 	}
 
 	_destination_healthy = true;
+	perf_end(_update_destination_perf);
 }
 
 bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon)

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -41,6 +41,7 @@
 
 #include <cstdint>
 #include <matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
 #include "geofence_interface.h"
 
 static constexpr int kMaxNodes = 100;
@@ -117,7 +118,7 @@ class GeofenceAvoidancePlanner
 {
 public:
 	GeofenceAvoidancePlanner() = default;
-	~GeofenceAvoidancePlanner() = default;
+	~GeofenceAvoidancePlanner();
 
 	const PlannedPath &planPath();
 
@@ -140,6 +141,12 @@ private:
 	bool _start_healthy{false};
 	bool _destination_healthy{false};
 
+	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);