From 3e1f18e8de062402be0ef8966072632c0e8b9e08 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 21 Apr 2026 15:17:29 +0300 Subject: [PATCH] added support for circular fences Signed-off-by: RomanBapst --- src/lib/geofence/GeofenceUtilsTest.cpp | 18 +++++ src/lib/geofence/geofence_utils.cpp | 34 ++++++--- src/lib/geofence/geofence_utils.h | 42 +++++++++++ .../geofence_avoidance_planner.cpp | 68 ++++++++++++------ .../geofence_avoidance_planner.h | 1 + src/modules/navigator/geofence.cpp | 69 ++++++++++++------- 6 files changed, 176 insertions(+), 56 deletions(-) diff --git a/src/lib/geofence/GeofenceUtilsTest.cpp b/src/lib/geofence/GeofenceUtilsTest.cpp index f7d63263b83..176a74bac34 100644 --- a/src/lib/geofence/GeofenceUtilsTest.cpp +++ b/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)); + +} diff --git a/src/lib/geofence/geofence_utils.cpp b/src/lib/geofence/geofence_utils.cpp index 290428d6277..60d598ce9f8 100644 --- a/src/lib/geofence/geofence_utils.cpp +++ b/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 ¢er, 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 diff --git a/src/lib/geofence/geofence_utils.h b/src/lib/geofence/geofence_utils.h index 07c71abf3c4..057e1df5200 100644 --- a/src/lib/geofence/geofence_utils.h +++ b/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 +void insidePolygonUpdateState(bool &last_state, + const matrix::Vector2 &v1, + const matrix::Vector2 &v2, + const matrix::Vector2 &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 ¢er, 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 ¢er, float radius); + /** * Offset polygon vertices expand or inward by computing the bisector * of the two normalized edge directions at each vertex. diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp index f5bb0690055..94b4faad0ac 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp @@ -75,7 +75,13 @@ PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2 &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::Vector2getPolygonInfoByIndex(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++; + } } } diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h index 6b55f10c988..b839410fc53 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h +++ b/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; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 96e1a07c2ea..b0753ea6869 100644 --- a/src/modules/navigator/geofence.cpp +++ b/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 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(_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 vertex_i{(double)temp_vertex_i.lat, (double)temp_vertex_i.lon}; + const matrix::Vector2 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(_stats.dataman_id)}; - bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx, - reinterpret_cast(&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(_stats.dataman_id)}; + bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx, + reinterpret_cast(&vertex_current), + sizeof(mission_fence_point_s)); + + if (!success) { + break; + } + + success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx, + reinterpret_cast(&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(_stats.dataman_id), info.dataman_index, + reinterpret_cast(&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(&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;