added support for circular fences

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

src/lib/geofence/GeofenceUtilsTest.cpp

@@ -59,3 +59,21 @@ TEST(GeofenceUtilsTest, SegmentsCross)
 
 	EXPECT_TRUE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
 }
+TEST(GeofenceUtilsTest, SegmentAndCircleIntersect)
+{
+	// vertical line from origin straight up
+	Vector2f p1(0.f, 0.f);
+	Vector2f p2(0.f, 1.f);
+	Vector2f center(0.0f, 1.0f);
+
+	EXPECT_TRUE(geofence_utils::lineSegmentIntersectsCircle(p1, p2, center, 0.5f));
+
+	p1 = Vector2f(-1.0f, 0.0f);
+	p2 = Vector2f(1.f, 0.0f);
+	center = Vector2f(0.f, 1.f);
+
+	EXPECT_TRUE(geofence_utils::lineSegmentIntersectsCircle(p1, p2, center, 1.1f));
+
+	EXPECT_FALSE(geofence_utils::lineSegmentIntersectsCircle(p1, p2, center, 0.9f));
+
+}

src/lib/geofence/geofence_utils.cpp

@@ -40,20 +40,10 @@ namespace geofence_utils
 bool insidePolygon(const matrix::Vector2f *vertices, int num_vertices,
 		   const matrix::Vector2f &point)
 {
-	/**
-	 * Adaptation of algorithm originally presented as
-	 * PNPOLY - Point Inclusion in Polygon Test
-	 * W. Randolph Franklin (WRF)
-	 * Only supports non-complex polygons (not self intersecting)
-	 */
 	bool c = false;
 
 	for (int i = 0, j = num_vertices - 1; i < num_vertices; j = i++) {
-		if (((vertices[i](1) >= point(1)) != (vertices[j](1) >= point(1))) &&
-		    (point(0) <= (vertices[j](0) - vertices[i](0)) * (point(1) - vertices[i](1)) /
-		     (vertices[j](1) - vertices[i](1)) + vertices[i](0))) {
-			c = !c;
-		}
+		insidePolygonUpdateState(c, vertices[i], vertices[j], point);
 	}
 
 	return c;
@@ -84,6 +74,26 @@ bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2,
 	return t > 0.0f && t < 1.0f && u > 0.0f && u < 1.0f;
 }
 
+bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end,
+				 const matrix::Vector2f &center, float radius)
+{
+	const matrix::Vector2f d = end - start;
+	const float len_sq = d.dot(d);
+
+	if (len_sq < FLT_EPSILON) {
+		return false;
+	}
+
+	const float t = math::constrain((center - start).dot(d) / len_sq, 0.f, 1.f);
+	const matrix::Vector2f closest = start + d * t;
+
+	if ((closest - center).norm() >= radius) {
+		return false;
+	}
+
+	return (start - center).norm() >= radius || (end - center).norm() >= radius;
+}
+
 bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices,
 			   float margin, bool expand,
 			   matrix::Vector2f *vertices_out)
@@ -132,4 +142,6 @@ bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices
 	return true;
 }
 
+
+
 } // namespace geofence_utils

src/lib/geofence/geofence_utils.h

@@ -43,6 +43,35 @@
 namespace geofence_utils
 {
 
+/**
+ * Update the PNPOLY inside/outside state for a single polygon edge.
+ * Templated on the coordinate scalar type so the same test can be used
+ * with either float (local frame, meters) or double (lat/lon degrees).
+ *
+ * @param last_state  state passed in on each call as reference, init to false
+ * @param v1          edge start vertex
+ * @param v2          edge end vertex
+ * @param point       test point
+ */
+template <typename T>
+void insidePolygonUpdateState(bool &last_state,
+			      const matrix::Vector2<T> &v1,
+			      const matrix::Vector2<T> &v2,
+			      const matrix::Vector2<T> &point)
+{
+	/**
+	 * Adaptation of algorithm originally presented as
+	 * PNPOLY - Point Inclusion in Polygon Test
+	 * W. Randolph Franklin (WRF)
+	 * Only supports non-complex polygons (not self intersecting)
+	 */
+	if (((v1(1) >= point(1)) != (v2(1) >= point(1))) &&
+	    (point(0) <= (v2(0) - v1(0)) * (point(1) - v1(1)) /
+	     (v2(1) - v1(1)) + v1(0))) {
+		last_state = !last_state;
+	}
+}
+
 /**
  * Check if a point is inside a circle.
  *
@@ -67,6 +96,19 @@ bool insideCircle(const matrix::Vector2<double> &center, float radius,
 bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2,
 		       const matrix::Vector2f &v1, const matrix::Vector2f &v2);
 
+/**
+ * Check if a line segment intersects a circle.
+ * Works in local Cartesian coordinates (meters).
+ *
+ * @param start   segment start in local frame
+ * @param end     segment end in local frame
+ * @param center  circle center in local frame
+ * @param radius  circle radius in meters
+ * @return true if the segment intersects the circle
+ */
+bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end,
+				 const matrix::Vector2f &center, float radius);
+
 /**
  * Offset polygon vertices expand or inward by computing the bisector
  * of the two normalized edge directions at each vertex.

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -75,7 +75,13 @@ PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2<double> &st
 
 	for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
 		const PolygonInfo info = geofence->getPolygonInfoByIndex(poly_index);
-		num_vertices_total += info.vertex_count;
+
+		if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
+			num_vertices_total += kCircleApproxVertices;
+
+		} else {
+			num_vertices_total += info.vertex_count;
+		}
 	}
 
 	if (num_vertices_total - 2 > kMaxNodes) {
@@ -213,30 +219,50 @@ bool GeofenceAvoidancePlanner::calculate_graph_nodes(const matrix::Vector2<doubl
 	for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
 		PolygonInfo info = geofence->getPolygonInfoByIndex(poly_index);
 
-		// Project vertices to local frame
-		matrix::Vector2f local_in[info.vertex_count];
-		matrix::Vector2f local_out[info.vertex_count];
 
-		for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
-			local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, start);
-		}
+		if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
 
-		// In order to avoid moving too close to the edges of the polygons, we either expand (inclusion polygon)
-		// or shrink (exclusion) the polygon by a margin and use the resulting vertices as graph nodes.
-		const bool should_expand = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
+			// Project vertices to local frame
+			matrix::Vector2f local_in[info.vertex_count];
+			matrix::Vector2f local_out[info.vertex_count];
 
-		if (!geofence_utils::expandOrShrinkPolygon(local_in, info.vertex_count, margin, should_expand,
-				local_out)) {
-			return false;
-		}
+			for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
+				local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, start);
+			}
 
-		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;
-			node_index++;
+			// In order to avoid moving too close to the edges of the polygons, we either expand (inclusion polygon)
+			// or shrink (exclusion) the polygon by a margin and use the resulting vertices as graph nodes.
+			const bool should_expand = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
+
+			if (!geofence_utils::expandOrShrinkPolygon(local_in, info.vertex_count, margin, should_expand,
+					local_out)) {
+				return false;
+			}
+
+			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;
+				node_index++;
+			}
+
+		} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
+			const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, start);
+			const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices;
+			const float enlarged_radius = info.circle_radius / (2.f * cosf(delta_angle)) + (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION ?
+						      -margin : margin);
+
+			for (int i = 0; i < kCircleApproxVertices; i++) {
+				const float angle = i * (2.f * M_PI_F / kCircleApproxVertices);
+				_graph_nodes[node_index].type = Node::Type::VERTEX;
+				_graph_nodes[node_index].position = center + matrix::Vector2f{enlarged_radius * cosf(angle), enlarged_radius * sinf(angle)};
+				_graph_nodes[node_index].best_distance = FLT_MAX;
+				_graph_nodes[node_index].previous_index = -1;
+				_graph_nodes[node_index].visited = false;
+				node_index++;
+			}
 		}
 	}
 

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -44,6 +44,7 @@
 #include "geofence_interface.h"
 
 static constexpr int kMaxNodes = 100;
+static constexpr int kCircleApproxVertices = 8;
 
 struct PlannedPath {
 	static constexpr int kMaxPathPoints = 32;

src/modules/navigator/geofence.cpp

@@ -494,6 +494,8 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
 	mission_fence_point_s temp_vertex_j{};
 	bool c = false;
 
+	const matrix::Vector2<double> test_point{lat, lon};
+
 	for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
 
 		dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
@@ -525,11 +527,10 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
 
 		}
 
-		if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
-		    (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
-		     (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
-			c = !c;
-		}
+		const matrix::Vector2<double> vertex_i{(double)temp_vertex_i.lat, (double)temp_vertex_i.lon};
+		const matrix::Vector2<double> vertex_j{(double)temp_vertex_j.lat, (double)temp_vertex_j.lon};
+
+		geofence_utils::insidePolygonUpdateState(c, vertex_i, vertex_j, test_point);
 	}
 
 	return c;
@@ -792,36 +793,56 @@ bool Geofence::checkIfLineViolatesAnyFence(const matrix::Vector2f &start_local,
 	for (int poly_idx = 0; poly_idx < _num_polygons; poly_idx++) {
 		PolygonInfo info = _polygons[poly_idx];
 
-		for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
-			mission_fence_point_s vertex_current{};
-			mission_fence_point_s vertex_previous{};
+		if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
 
-			int prev_idx = vertex_idx == 0 ? info.vertex_count - 1 : vertex_idx - 1;
+			for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
+				mission_fence_point_s vertex_current{};
+				mission_fence_point_s vertex_previous{};
 
-			dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
-			bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
-							       reinterpret_cast<uint8_t *>(&vertex_current),
-							       sizeof(mission_fence_point_s));
+				int prev_idx = vertex_idx == 0 ? info.vertex_count - 1 : vertex_idx - 1;
+
+				dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
+				bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
+								       reinterpret_cast<uint8_t *>(&vertex_current),
+								       sizeof(mission_fence_point_s));
+
+				if (!success) {
+					break;
+				}
+
+				success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx,
+								  reinterpret_cast<uint8_t *>(&vertex_previous),
+								  sizeof(mission_fence_point_s));
+
+				if (!success) {
+					break;
+				}
+
+				matrix::Vector2f vertex_current_local = ref.project(vertex_current.lat, vertex_current.lon);
+				matrix::Vector2f vertex_previous_local = ref.project(vertex_previous.lat, vertex_previous.lon);
+
+				if (geofence_utils::segmentsIntersect(start_local, end_local, vertex_current_local, vertex_previous_local)) {
+					return true;
+				}
+			}
+
+		} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
+			mission_fence_point_s circle_point{};
+			bool success = _dataman_cache.loadWait(static_cast<dm_item_t>(_stats.dataman_id), info.dataman_index,
+							       reinterpret_cast<uint8_t *>(&circle_point), sizeof(mission_fence_point_s));
 
 			if (!success) {
+				PX4_ERR("dm_read failed");
 				break;
 			}
 
-			success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx,
-							  reinterpret_cast<uint8_t *>(&vertex_previous),
-							  sizeof(mission_fence_point_s));
+			matrix::Vector2f circle_center_local = ref.project(circle_point.lat, circle_point.lon);
 
-			if (!success) {
-				break;
-			}
-
-			matrix::Vector2f vertex_current_local = ref.project(vertex_current.lat, vertex_current.lon);
-			matrix::Vector2f vertex_previous_local = ref.project(vertex_previous.lat, vertex_previous.lon);
-
-			if (geofence_utils::segmentsIntersect(start_local, end_local, vertex_current_local, vertex_previous_local)) {
+			if (geofence_utils::lineSegmentIntersectsCircle(start_local, end_local, circle_center_local, circle_point.circle_radius)) {
 				return true;
 			}
 		}
+
 	}
 
 	return false;