AC_Avoidance: Use closest fence point if Dijkstra starts outside fence

[aadarshagarwal1]

Resolves #31399

Bug Description

If a Rover (or Copter) using OADijkstra starts slightly outside an inclusion fence (e.g., due to GPS drift or loitering near the edge), calc_shortest_path fails.

• update_visgraph returns true but generates 0 nodes because all paths to internal nodes intersect the fence.
• The pathfinder returns DIJKSTRA_ERROR_OUT_OF_MEMORY (or general failure) and the vehicle cannot RTL or navigate back.

The Fix

This PR implements a "Recovery Mode" inside calc_shortest_path:

1. Checks if _source_visgraph is empty after the initial update.
2. If empty, iterates through _inclusion_polygon_pts to find the geometrically closest point on the fence boundary.
3. Temporarily sets the path start node to this "Safe Boundary Point."
4. Re-runs update_visgraph from this safe spot (which guarantees valid connections).
5. Prepends the "Void -> Fence" leg to the final path so the vehicle drives to the fence first.

Verification

• Compiled successfully for Rover.
• Logic relies on Vector2f::closest_point for robust geometry handling.

[aadarshagarwal1]

Hi @rmackay9 ,

This PR is ready for review (CI is passing). It solves the edge case where OADijkstra fails if the vehicle starts slightly outside an inclusion fence (common with GPS drift).

Summary of fix:

1. Detects if the graph generation fails due to being outside the fence.
2. Finds the nearest point on the fence boundary.
3. Generates a path starting from that boundary point, allowing the vehicle to re-enter the safe zone and continue navigation.

I have tested this compilation locally. Could you please take a look when you have a moment? Thanks!