Skip to content

AC_Avoidance: Use closest fence point if Dijkstra starts outside fence #31827

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
aadarshagarwal1 wants to merge 1 commit into
base: master
Choose a base branch
from
+51 −1
Open

AC_Avoidance: Use closest fence point if Dijkstra starts outside fence #31827

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 48 additions & 1 deletion libraries/AC_Avoidance/AP_OADijkstra.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -887,10 +887,27 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d
}

// create visgraphs of origin and destination to fence points
if (!update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, _path_source, true, _path_destination)) {
// 1. Try normal visgraph creation
bool source_valid = update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, _path_source, true, _path_destination);

// 2. DETECT FAILURE: If valid but empty, we are likely outside the fence (The Void)
if (source_valid && _source_visgraph.num_items() == 0) {
Vector2f recovery_pt;
// Attempt to snap to the nearest fence boundary
if (find_closest_fence_point(_path_source, recovery_pt)) {
// SNAP: Move the start point to the safe boundary
_path_source = recovery_pt;

// RE-RUN: Try to connect from this new safe spot
source_valid = update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, _path_source, true, _path_destination);
}
}

if (!source_valid) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
}

if (!update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, _path_destination)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
Expand Down Expand Up @@ -1015,6 +1032,36 @@ bool AP_OADijkstra::convert_node_to_point(const AP_OAVisGraph::OAItemID& id, Vec
// we should never reach here but just in case
return false;
}
// Helper to find the closest point on the inclusion polygon boundary
bool AP_OADijkstra::find_closest_fence_point(const Vector2f& src, Vector2f& closest_pt)
{
// If we have fewer than 3 points, it's not a polygon
if (_inclusion_polygon_numpoints < 3) {
return false;
}

float best_dist_sq = FLT_MAX;
bool found = false;

// Iterate through every line segment of the polygon
for (uint8_t i = 0; i < _inclusion_polygon_numpoints; i++) {
// Get current point and next point (wrapping around to 0 at the end)
const Vector2f &p1 = _inclusion_polygon_pts[i];
const Vector2f &p2 = _inclusion_polygon_pts[(i + 1) % _inclusion_polygon_numpoints];

// Find the closest spot on this specific segment
Vector2f point_on_seg = Vector2f::closest_point(src, p1, p2);

// Check if this spot is the closest one we've seen so far
float dist_sq = (src - point_on_seg).length_squared();
if (dist_sq < best_dist_sq) {
best_dist_sq = dist_sq;
closest_pt = point_on_seg;
found = true;
}
}
return found;
}
#endif // AP_FENCE_ENABLED


Expand Down
3 changes: 3 additions & 0 deletions libraries/AC_Avoidance/AP_OADijkstra.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,9 @@ class AP_OADijkstra {
// resulting path is stored in _shortest_path array as vector offsets from EKF origin
bool calc_shortest_path(const Location &origin, const Location &destination, AP_OADijkstra_Error &err_id);

// Helper to recover if start point is outside fence
bool find_closest_fence_point(const Vector2f& src, Vector2f& closest_pt);

// shortest path state variables
bool _inclusion_polygon_with_margin_ok;
bool _exclusion_polygon_with_margin_ok;
Expand Down
Loading