Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AC_Avoidance_config.h"
17 :
18 : #if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
19 :
20 : #include "AP_OADijkstra.h"
21 : #include "AP_OAPathPlanner.h"
22 :
23 : #include <AC_Fence/AC_Fence.h>
24 :
25 : #if AP_FENCE_ENABLED
26 :
27 : #include <AP_AHRS/AP_AHRS.h>
28 : #include <AP_Logger/AP_Logger.h>
29 : #include <GCS_MAVLink/GCS.h>
30 :
31 : #define OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK 32 // expanding arrays for fence points and paths to destination will grow in increments of 20 elements
32 : #define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX 255 // index use to indicate we do not have a tentative short path for a node
33 : #define OA_DIJKSTRA_ERROR_REPORTING_INTERVAL_MS 5000 // failure messages sent to GCS every 5 seconds
34 :
35 : /// Constructor
36 7 : AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) :
37 7 : _inclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
38 7 : _exclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
39 7 : _exclusion_circle_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
40 7 : _short_path_data(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
41 7 : _path(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
42 21 : _options(options)
43 : {
44 7 : }
45 :
46 : // calculate a destination to avoid fences
47 : // returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required
48 : // next_destination_new will be non-zero if there is a next destination
49 : // dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear
50 270 : AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_loc,
51 : const Location &destination,
52 : const Location &next_destination,
53 : Location& origin_new,
54 : Location& destination_new,
55 : Location& next_destination_new,
56 : bool& dest_to_next_dest_clear)
57 : {
58 270 : WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());
59 :
60 : // avoidance is not required if no fences
61 270 : if (!some_fences_enabled()) {
62 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = true;
63 0 : Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
64 0 : return DIJKSTRA_STATE_NOT_REQUIRED;
65 : }
66 :
67 : // no avoidance required if destination is same as current location
68 270 : if (current_loc.same_latlon_as(destination)) {
69 : // we do not check path to next destination so conservatively set to false
70 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
71 0 : Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
72 0 : return DIJKSTRA_STATE_NOT_REQUIRED;
73 : }
74 :
75 : // check for inclusion polygon updates
76 270 : if (check_inclusion_polygon_updated()) {
77 1 : _inclusion_polygon_with_margin_ok = false;
78 1 : _polyfence_visgraph_ok = false;
79 1 : _shortest_path_ok = false;
80 : }
81 :
82 : // check for exclusion polygon updates
83 270 : if (check_exclusion_polygon_updated()) {
84 1 : _exclusion_polygon_with_margin_ok = false;
85 1 : _polyfence_visgraph_ok = false;
86 1 : _shortest_path_ok = false;
87 : }
88 :
89 : // check for exclusion circle updates
90 270 : if (check_exclusion_circle_updated()) {
91 1 : _exclusion_circle_with_margin_ok = false;
92 1 : _polyfence_visgraph_ok = false;
93 1 : _shortest_path_ok = false;
94 : }
95 :
96 : // create inner polygon fence
97 270 : if (!_inclusion_polygon_with_margin_ok) {
98 1 : _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, _error_id);
99 1 : if (!_inclusion_polygon_with_margin_ok) {
100 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
101 0 : report_error(_error_id);
102 0 : Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination);
103 0 : return DIJKSTRA_STATE_ERROR;
104 : }
105 : }
106 :
107 : // create exclusion polygon outer fence
108 270 : if (!_exclusion_polygon_with_margin_ok) {
109 1 : _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, _error_id);
110 1 : if (!_exclusion_polygon_with_margin_ok) {
111 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
112 0 : report_error(_error_id);
113 0 : Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination);
114 0 : return DIJKSTRA_STATE_ERROR;
115 : }
116 : }
117 :
118 : // create exclusion circle points
119 270 : if (!_exclusion_circle_with_margin_ok) {
120 1 : _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, _error_id);
121 1 : if (!_exclusion_circle_with_margin_ok) {
122 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
123 0 : report_error(_error_id);
124 0 : Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination);
125 0 : return DIJKSTRA_STATE_ERROR;
126 : }
127 : }
128 :
129 : // create visgraph for all fence (with margin) points
130 270 : if (!_polyfence_visgraph_ok) {
131 1 : _polyfence_visgraph_ok = create_fence_visgraph(_error_id);
132 1 : if (!_polyfence_visgraph_ok) {
133 0 : _shortest_path_ok = false;
134 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
135 0 : report_error(_error_id);
136 0 : Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination);
137 0 : return DIJKSTRA_STATE_ERROR;
138 : }
139 : // reset logging count to restart logging updated graph
140 1 : _log_num_points = 0;
141 1 : _log_visgraph_version++;
142 : }
143 :
144 : // Log one visgraph point per loop
145 270 : if (_polyfence_visgraph_ok && (_log_num_points < total_numpoints()) && (_options & AP_OAPathPlanner::OARecoveryOptions::OA_OPTION_LOG_DIJKSTRA_POINTS) ) {
146 0 : Vector2f vis_point;
147 0 : if (get_point(_log_num_points, vis_point)) {
148 0 : Location log_location(Vector3f{vis_point.x, vis_point.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN);
149 0 : Write_Visgraph_point(_log_visgraph_version, _log_num_points, log_location.lat, log_location.lng);
150 0 : _log_num_points++;
151 : }
152 : }
153 :
154 : // rebuild path if destination or next_destination has changed
155 270 : if (!destination.same_latlon_as(_destination_prev) || !next_destination.same_latlon_as(_next_destination_prev)) {
156 2 : _destination_prev = destination;
157 2 : _next_destination_prev = next_destination;
158 2 : _shortest_path_ok = false;
159 : }
160 :
161 : // calculate shortest path from current_loc to destination
162 270 : if (!_shortest_path_ok) {
163 2 : _shortest_path_ok = calc_shortest_path(current_loc, destination, _error_id);
164 2 : if (!_shortest_path_ok) {
165 0 : dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
166 0 : report_error(_error_id);
167 0 : Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination);
168 0 : return DIJKSTRA_STATE_ERROR;
169 : }
170 : // start from 2nd point on path (first is the original origin)
171 2 : _path_idx_returned = 1;
172 :
173 : // check if path from destination to next_destination intersects with a fence
174 2 : _dest_to_next_dest_clear = false;
175 2 : if (!next_destination.is_zero()) {
176 0 : Vector2f seg_start, seg_end;
177 0 : if (destination.get_vector_xy_from_origin_NE_cm(seg_start) &&
178 0 : next_destination.get_vector_xy_from_origin_NE_cm(seg_end)) {
179 0 : _dest_to_next_dest_clear = !intersects_fence(seg_start, seg_end);
180 : }
181 : }
182 : }
183 :
184 : // path has been created, return latest point
185 270 : Vector2f dest_pos;
186 270 : const uint8_t path_length = get_shortest_path_numpoints() > 0 ? (get_shortest_path_numpoints() - 1) : 0;
187 270 : if ((_path_idx_returned < path_length) && get_shortest_path_point(_path_idx_returned, dest_pos)) {
188 :
189 : // for the first point return origin as current_loc
190 227 : Vector2f origin_pos;
191 227 : if ((_path_idx_returned > 0) && get_shortest_path_point(_path_idx_returned-1, origin_pos)) {
192 : // convert offset from ekf origin to Location
193 227 : Location temp_loc(Vector3f{origin_pos.x, origin_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN);
194 227 : origin_new = temp_loc;
195 : } else {
196 : // for first point use current loc as origin
197 0 : origin_new = current_loc;
198 : }
199 :
200 : // convert offset from ekf origin to Location
201 227 : Location temp_loc(Vector3f{dest_pos.x, dest_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN);
202 227 : destination_new = destination;
203 227 : destination_new.lat = temp_loc.lat;
204 227 : destination_new.lng = temp_loc.lng;
205 :
206 : // provide next destination to allow smooth cornering
207 227 : next_destination_new.zero();
208 227 : Vector2f next_dest_pos;
209 227 : if ((_path_idx_returned + 1 < path_length) && get_shortest_path_point(_path_idx_returned + 1, next_dest_pos)) {
210 : // convert offset from ekf origin to Location
211 176 : Location next_loc(Vector3f{next_dest_pos.x, next_dest_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN);
212 176 : next_destination_new = destination;
213 176 : next_destination_new.lat = next_loc.lat;
214 176 : next_destination_new.lng = next_loc.lng;
215 : } else {
216 : // return destination as next_destination
217 51 : next_destination_new = destination;
218 : }
219 :
220 : // path to next destination clear state is still valid from previous calcs (was calced along with shortest path)
221 227 : dest_to_next_dest_clear = _dest_to_next_dest_clear;
222 :
223 : // check if we should advance to next point for next iteration
224 227 : const bool near_oa_wp = current_loc.get_distance(destination_new) <= 2.0f;
225 227 : const bool past_oa_wp = current_loc.past_interval_finish_line(origin_new, destination_new);
226 227 : if (near_oa_wp || past_oa_wp) {
227 10 : _path_idx_returned++;
228 : }
229 : // log success
230 227 : Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, get_shortest_path_numpoints(), destination, destination_new);
231 227 : return DIJKSTRA_STATE_SUCCESS;
232 : }
233 :
234 : // we have reached the destination so avoidance is no longer required
235 : // path to next destination clear state is still valid from previous calcs
236 43 : dest_to_next_dest_clear = _dest_to_next_dest_clear;
237 43 : Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
238 43 : return DIJKSTRA_STATE_NOT_REQUIRED;
239 270 : }
240 :
241 : // returns true if at least one inclusion or exclusion zone is enabled
242 270 : bool AP_OADijkstra::some_fences_enabled() const
243 : {
244 270 : const AC_Fence *fence = AC_Fence::get_singleton();
245 270 : if (fence == nullptr) {
246 0 : return false;
247 : }
248 270 : if ((fence->polyfence().get_inclusion_polygon_count() == 0) &&
249 270 : (fence->polyfence().get_exclusion_polygon_count() == 0) &&
250 0 : (fence->polyfence().get_exclusion_circle_count() == 0)) {
251 0 : return false;
252 : }
253 270 : return ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) > 0);
254 : }
255 :
256 : // return error message for a given error id
257 0 : const char* AP_OADijkstra::get_error_msg(AP_OADijkstra_Error error_id) const
258 : {
259 0 : switch (error_id) {
260 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_NONE:
261 0 : return "no error";
262 : break;
263 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY:
264 0 : return "out of memory";
265 : break;
266 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS:
267 0 : return "overlapping polygon points";
268 : break;
269 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_FAILED_TO_BUILD_INNER_POLYGON:
270 0 : return "failed to build inner polygon";
271 : break;
272 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES:
273 0 : return "overlapping polygon lines";
274 : break;
275 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED:
276 0 : return "fence disabled";
277 : break;
278 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS:
279 0 : return "too many fence points";
280 : break;
281 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_NO_POSITION_ESTIMATE:
282 0 : return "no position estimate";
283 : break;
284 0 : case AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH:
285 0 : return "could not find path";
286 : break;
287 : }
288 :
289 : // we should never reach here but just in case
290 0 : return "unknown error";
291 : }
292 :
293 0 : void AP_OADijkstra::report_error(AP_OADijkstra_Error error_id)
294 : {
295 : // report errors to GCS every 5 seconds
296 0 : uint32_t now_ms = AP_HAL::millis();
297 0 : if ((error_id != AP_OADijkstra_Error::DIJKSTRA_ERROR_NONE) &&
298 0 : ((error_id != _error_last_id) || ((now_ms - _error_last_report_ms) > OA_DIJKSTRA_ERROR_REPORTING_INTERVAL_MS))) {
299 0 : const char* error_msg = get_error_msg(error_id);
300 : (void)error_msg; // in case !HAL_GCS_ENABLED
301 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Dijkstra: %s", error_msg);
302 0 : _error_last_id = error_id;
303 0 : _error_last_report_ms = now_ms;
304 : }
305 0 : }
306 :
307 : // check if polygon fence has been updated since we created the inner fence. returns true if changed
308 270 : bool AP_OADijkstra::check_inclusion_polygon_updated() const
309 : {
310 : // exit immediately if polygon fence is not enabled
311 270 : const AC_Fence *fence = AC_Fence::get_singleton();
312 270 : if (fence == nullptr) {
313 0 : return false;
314 : }
315 270 : return (_inclusion_polygon_update_ms != fence->polyfence().get_inclusion_polygon_update_ms());
316 : }
317 :
318 : // create polygons inside the existing inclusion polygons
319 : // returns true on success. returns false on failure and err_id is updated
320 1 : bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
321 : {
322 1 : const AC_Fence *fence = AC_Fence::get_singleton();
323 :
324 1 : if (fence == nullptr) {
325 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
326 0 : return false;
327 : }
328 :
329 : // skip unnecessary retry to build inclusion polygon if previous fence points have not changed
330 1 : if (_inclusion_polygon_update_ms == fence->polyfence().get_inclusion_polygon_update_ms()) {
331 0 : return false;
332 : }
333 :
334 1 : _inclusion_polygon_update_ms = fence->polyfence().get_inclusion_polygon_update_ms();
335 :
336 : // clear all points
337 1 : _inclusion_polygon_numpoints = 0;
338 :
339 : // return immediately if no polygons
340 1 : const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
341 :
342 : // iterate through polygons and create inner points
343 2 : for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
344 : uint16_t num_points;
345 1 : const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
346 :
347 : // for each point in inclusion polygon
348 : // Note: boundary is "unclosed" meaning the last point is *not* the same as the first
349 1 : uint16_t new_points = 0;
350 20 : for (uint16_t j = 0; j < num_points; j++) {
351 :
352 : // find points before and after current point (relative to current point)
353 19 : const uint16_t before_idx = (j == 0) ? num_points-1 : j-1;
354 19 : const uint16_t after_idx = (j == num_points-1) ? 0 : j+1;
355 19 : Vector2f before_pt = boundary[before_idx] - boundary[j];
356 19 : Vector2f after_pt = boundary[after_idx] - boundary[j];
357 :
358 : // if points are overlapping fail
359 19 : if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
360 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS;
361 0 : return false;
362 : }
363 :
364 : // scale points to be unit vectors
365 19 : before_pt.normalize();
366 19 : after_pt.normalize();
367 :
368 : // calculate intermediate point and scale to margin
369 19 : Vector2f intermediate_pt = after_pt + before_pt;
370 19 : intermediate_pt.normalize();
371 19 : intermediate_pt *= margin_cm;
372 :
373 : // find final point which is outside the inside polygon
374 19 : Vector2f temp_point = boundary[j] + intermediate_pt;
375 19 : if (Polygon_outside(temp_point, boundary, num_points)) {
376 7 : intermediate_pt *= -1.0;
377 7 : temp_point = boundary[j] + intermediate_pt;
378 7 : if (Polygon_outside(temp_point, boundary, num_points)) {
379 : // could not find a point on either side that was outside the exclusion polygon so fail
380 : // this may happen if the exclusion polygon has overlapping lines
381 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
382 0 : return false;
383 : }
384 : }
385 :
386 : // don't add points in corners
387 19 : if (fabsf(wrap_PI(intermediate_pt.angle() - before_pt.angle())) < M_PI_2) {
388 12 : continue;
389 : }
390 :
391 : // expand array if required
392 7 : if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + new_points + 1)) {
393 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
394 0 : return false;
395 : }
396 : // add point
397 7 : _inclusion_polygon_pts[_inclusion_polygon_numpoints + new_points] = temp_point;
398 7 : new_points++;
399 : }
400 :
401 : // update total number of points
402 1 : _inclusion_polygon_numpoints += new_points;
403 : }
404 1 : return true;
405 : }
406 :
407 : // check if exclusion polygons have been updated since create_exclusion_polygon_with_margin was run
408 : // returns true if changed
409 270 : bool AP_OADijkstra::check_exclusion_polygon_updated() const
410 : {
411 270 : const AC_Fence *fence = AC_Fence::get_singleton();
412 270 : if (fence == nullptr) {
413 0 : return false;
414 : }
415 270 : return (_exclusion_polygon_update_ms != fence->polyfence().get_exclusion_polygon_update_ms());
416 : }
417 :
418 : // create polygons around existing exclusion polygons
419 : // returns true on success. returns false on failure and err_id is updated
420 1 : bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
421 : {
422 1 : const AC_Fence *fence = AC_Fence::get_singleton();
423 :
424 1 : if (fence == nullptr) {
425 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
426 0 : return false;
427 : }
428 :
429 : // skip unnecessary retry to build exclusion polygon if previous fence points have not changed
430 1 : if (_exclusion_polygon_update_ms == fence->polyfence().get_exclusion_polygon_update_ms()) {
431 0 : return false;
432 : }
433 :
434 1 : _exclusion_polygon_update_ms = fence->polyfence().get_exclusion_polygon_update_ms();
435 :
436 :
437 : // clear all points
438 1 : _exclusion_polygon_numpoints = 0;
439 :
440 : // return immediately if no exclusion polygons
441 1 : const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
442 :
443 : // iterate through exclusion polygons and create outer points
444 2 : for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
445 : uint16_t num_points;
446 1 : const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
447 :
448 : // for each point in exclusion polygon
449 : // Note: boundary is "unclosed" meaning the last point is *not* the same as the first
450 1 : uint16_t new_points = 0;
451 5 : for (uint16_t j = 0; j < num_points; j++) {
452 :
453 : // find points before and after current point (relative to current point)
454 4 : const uint16_t before_idx = (j == 0) ? num_points-1 : j-1;
455 4 : const uint16_t after_idx = (j == num_points-1) ? 0 : j+1;
456 4 : Vector2f before_pt = boundary[before_idx] - boundary[j];
457 4 : Vector2f after_pt = boundary[after_idx] - boundary[j];
458 :
459 : // if points are overlapping fail
460 4 : if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
461 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS;
462 0 : return false;
463 : }
464 :
465 : // scale points to be unit vectors
466 4 : before_pt.normalize();
467 4 : after_pt.normalize();
468 :
469 : // calculate intermediate point and scale to margin
470 4 : Vector2f intermediate_pt = after_pt + before_pt;
471 4 : intermediate_pt.normalize();
472 4 : intermediate_pt *= margin_cm;
473 :
474 : // find final point which is outside the original polygon
475 4 : Vector2f temp_point = boundary[j] + intermediate_pt;
476 4 : if (!Polygon_outside(temp_point, boundary, num_points)) {
477 4 : intermediate_pt *= -1;
478 4 : temp_point = boundary[j] + intermediate_pt;
479 4 : if (!Polygon_outside(temp_point, boundary, num_points)) {
480 : // could not find a point on either side that was outside the exclusion polygon so fail
481 : // this may happen if the exclusion polygon has overlapping lines
482 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
483 0 : return false;
484 : }
485 : }
486 :
487 : // don't add points in corners
488 4 : if (fabsf(wrap_PI(intermediate_pt.angle() - before_pt.angle())) < M_PI_2) {
489 0 : continue;
490 : }
491 :
492 : // expand array if required
493 4 : if (!_exclusion_polygon_pts.expand_to_hold(_exclusion_polygon_numpoints + new_points + 1)) {
494 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
495 0 : return false;
496 : }
497 : // add point
498 4 : _exclusion_polygon_pts[_exclusion_polygon_numpoints + new_points] = temp_point;
499 4 : new_points++;
500 : }
501 :
502 : // update total number of points
503 1 : _exclusion_polygon_numpoints += new_points;
504 : }
505 1 : return true;
506 : }
507 :
508 : // check if exclusion circles have been updated since create_exclusion_circle_with_margin was run
509 : // returns true if changed
510 270 : bool AP_OADijkstra::check_exclusion_circle_updated() const
511 : {
512 : // exit immediately if fence is not enabled
513 270 : const AC_Fence *fence = AC_Fence::get_singleton();
514 270 : if (fence == nullptr) {
515 0 : return false;
516 : }
517 270 : return (_exclusion_circle_update_ms != fence->polyfence().get_exclusion_circle_update_ms());
518 : }
519 :
520 : // create polygons around existing exclusion circles
521 : // returns true on success. returns false on failure and err_id is updated
522 1 : bool AP_OADijkstra::create_exclusion_circle_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
523 : {
524 : // exit immediately if fence is not enabled
525 1 : const AC_Fence *fence = AC_Fence::get_singleton();
526 1 : if (fence == nullptr) {
527 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
528 0 : return false;
529 : }
530 :
531 : // clear all points
532 1 : _exclusion_circle_numpoints = 0;
533 :
534 : // unit length offsets for polygon points around circles
535 1 : const Vector2f unit_offsets[] = {
536 : {cosf(radians(30)), cosf(radians(30-90))}, // north-east
537 : {cosf(radians(90)), cosf(radians(90-90))}, // east
538 : {cosf(radians(150)), cosf(radians(150-90))},// south-east
539 : {cosf(radians(210)), cosf(radians(210-90))},// south-west
540 : {cosf(radians(270)), cosf(radians(270-90))},// west
541 : {cosf(radians(330)), cosf(radians(330-90))},// north-west
542 : };
543 1 : const uint8_t num_points_per_circle = ARRAY_SIZE(unit_offsets);
544 :
545 : // expand polygon point array if required
546 1 : const uint8_t num_exclusion_circles = fence->polyfence().get_exclusion_circle_count();
547 1 : if (!_exclusion_circle_pts.expand_to_hold(num_exclusion_circles * num_points_per_circle)) {
548 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
549 0 : return false;
550 : }
551 :
552 : // iterate through exclusion circles and create outer polygon points
553 2 : for (uint8_t i = 0; i < num_exclusion_circles; i++) {
554 1 : Vector2f circle_pos_cm;
555 : float radius;
556 1 : if (fence->polyfence().get_exclusion_circle(i, circle_pos_cm, radius)) {
557 : // scaler to ensure lines between points do not intersect circle
558 1 : const float scaler = (1.0f / cosf(radians(180.0f / (float)num_points_per_circle))) * ((radius * 100.0f) + margin_cm);
559 :
560 : // add points to array
561 7 : for (uint8_t j = 0; j < num_points_per_circle; j++) {
562 6 : _exclusion_circle_pts[_exclusion_circle_numpoints] = circle_pos_cm + (unit_offsets[j] * scaler);
563 6 : _exclusion_circle_numpoints++;
564 : }
565 : }
566 : }
567 :
568 : // record fence update time so we don't process these exclusion circles again
569 1 : _exclusion_circle_update_ms = fence->polyfence().get_exclusion_circle_update_ms();
570 :
571 1 : return true;
572 : }
573 :
574 : // returns total number of points across all fence types
575 1453 : uint16_t AP_OADijkstra::total_numpoints() const
576 : {
577 1453 : return _inclusion_polygon_numpoints + _exclusion_polygon_numpoints + _exclusion_circle_numpoints;
578 : }
579 :
580 : // get a single point across the total list of points from all fence types
581 903 : bool AP_OADijkstra::get_point(uint16_t index, Vector2f &point) const
582 : {
583 : // sanity check index
584 903 : if (index >= total_numpoints()) {
585 0 : return false;
586 : }
587 :
588 : // return an inclusion polygon point
589 903 : if (index < _inclusion_polygon_numpoints) {
590 402 : point = _inclusion_polygon_pts[index];
591 402 : return true;
592 : }
593 501 : index -= _inclusion_polygon_numpoints;
594 :
595 : // return an exclusion polygon point
596 501 : if (index < _exclusion_polygon_numpoints) {
597 219 : point = _exclusion_polygon_pts[index];
598 219 : return true;
599 : }
600 282 : index -= _exclusion_polygon_numpoints;
601 :
602 : // return an exclusion circle point
603 282 : if (index < _exclusion_circle_numpoints) {
604 282 : point = _exclusion_circle_pts[index];
605 282 : return true;
606 : }
607 :
608 : // we should never get here but just in case
609 0 : return false;
610 : }
611 :
612 : // returns true if line segment intersects polygon or circular fence
613 206 : bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f &seg_end) const
614 : {
615 : // return immediately if fence is not enabled
616 206 : const AC_Fence *fence = AC_Fence::get_singleton();
617 206 : if (fence == nullptr) {
618 0 : return false;
619 : }
620 :
621 : // determine if segment crosses any of the inclusion polygons
622 206 : uint16_t num_points = 0;
623 293 : for (uint8_t i = 0; i < fence->polyfence().get_inclusion_polygon_count(); i++) {
624 206 : const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
625 206 : if (boundary != nullptr) {
626 206 : Vector2f intersection;
627 206 : if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
628 119 : return true;
629 : }
630 : }
631 : }
632 :
633 : // determine if segment crosses any of the exclusion polygons
634 152 : for (uint8_t i = 0; i < fence->polyfence().get_exclusion_polygon_count(); i++) {
635 87 : const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
636 87 : if (boundary != nullptr) {
637 87 : Vector2f intersection;
638 87 : if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
639 22 : return true;
640 : }
641 : }
642 : }
643 :
644 : // determine if segment crosses any of the inclusion circles
645 65 : for (uint8_t i = 0; i < fence->polyfence().get_inclusion_circle_count(); i++) {
646 0 : Vector2f center_pos_cm;
647 : float radius;
648 0 : if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) {
649 : // intersects circle if either start or end is further from the center than the radius
650 0 : const float radius_cm_sq = sq(radius * 100.0f) ;
651 0 : if ((seg_start - center_pos_cm).length_squared() > radius_cm_sq) {
652 0 : return true;
653 : }
654 0 : if ((seg_end - center_pos_cm).length_squared() > radius_cm_sq) {
655 0 : return true;
656 : }
657 : }
658 : }
659 :
660 : // determine if segment crosses any of the exclusion circles
661 104 : for (uint8_t i = 0; i < fence->polyfence().get_exclusion_circle_count(); i++) {
662 65 : Vector2f center_pos_cm;
663 : float radius;
664 65 : if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) {
665 : // calculate distance between circle's center and segment
666 65 : const float dist_cm = Vector2f::closest_distance_between_line_and_point(seg_start, seg_end, center_pos_cm);
667 :
668 : // intersects if distance is less than radius
669 65 : if (dist_cm <= (radius * 100.0f)) {
670 26 : return true;
671 : }
672 : }
673 : }
674 :
675 : // if we got this far then no intersection
676 39 : return false;
677 : }
678 :
679 : // create visibility graph for all fence (with margin) points
680 : // returns true on success. returns false on failure and err_id is updated
681 : // requires these functions to have been run create_inclusion_polygon_with_margin, create_exclusion_polygon_with_margin, create_exclusion_circle_with_margin
682 1 : bool AP_OADijkstra::create_fence_visgraph(AP_OADijkstra_Error &err_id)
683 : {
684 : // exit immediately if fence is not enabled
685 1 : const AC_Fence *fence = AC_Fence::get_singleton();
686 1 : if (fence == nullptr) {
687 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
688 0 : return false;
689 : }
690 :
691 : // fail if more fence points than algorithm can handle
692 1 : if (total_numpoints() >= OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) {
693 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS;
694 0 : return false;
695 : }
696 :
697 : // clear fence points visibility graph
698 1 : _fence_visgraph.clear();
699 :
700 : // calculate distance from each point to all other points
701 17 : for (uint8_t i = 0; i < total_numpoints() - 1; i++) {
702 16 : Vector2f start_seg;
703 16 : if (get_point(i, start_seg)) {
704 152 : for (uint8_t j = i + 1; j < total_numpoints(); j++) {
705 136 : Vector2f end_seg;
706 136 : if (get_point(j, end_seg)) {
707 : // if line segment does not intersect with any inclusion or exclusion zones add to visgraph
708 136 : if (!intersects_fence(start_seg, end_seg)) {
709 35 : if (!_fence_visgraph.add_item({AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i},
710 : {AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, j},
711 70 : (start_seg - end_seg).length())) {
712 : // failure to add a point can only be caused by out-of-memory
713 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
714 0 : return false;
715 : }
716 : }
717 : }
718 : }
719 : }
720 : }
721 :
722 1 : return true;
723 : }
724 :
725 : // updates visibility graph for a given position which is an offset (in cm) from the ekf origin
726 : // to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
727 : // requires create_inclusion_polygon_with_margin to have been run
728 : // returns true on success
729 4 : bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position, Vector2f extra_position)
730 : {
731 : // clear visibility graph
732 4 : visgraph.clear();
733 :
734 : // calculate distance from position to all inclusion/exclusion fence points
735 72 : for (uint8_t i = 0; i < total_numpoints(); i++) {
736 68 : Vector2f seg_end;
737 68 : if (get_point(i, seg_end)) {
738 68 : if (!intersects_fence(position, seg_end)) {
739 : // line segment does not intersect with fences so add to visgraph
740 4 : if (!visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i}, (position - seg_end).length())) {
741 0 : return false;
742 : }
743 : }
744 : }
745 : }
746 :
747 : // add extra point to visibility graph if it doesn't intersect with polygon fence or exclusion polygons
748 4 : if (add_extra_position) {
749 2 : if (!intersects_fence(position, extra_position)) {
750 0 : if (!visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, (position - extra_position).length())) {
751 0 : return false;
752 : }
753 : }
754 : }
755 :
756 4 : return true;
757 : }
758 :
759 : // update total distance for all nodes visible from current node
760 : // curr_node_idx is an index into the _short_path_data array
761 23 : void AP_OADijkstra::update_visible_node_distances(node_index curr_node_idx)
762 : {
763 : // sanity check
764 23 : if (curr_node_idx >= _short_path_data_numpoints) {
765 0 : return;
766 : }
767 :
768 : // get current node for convenience
769 23 : const ShortPathNode &curr_node = _short_path_data[curr_node_idx];
770 :
771 : // for each visibility graph
772 23 : const AP_OAVisGraph* visgraphs[] = {&_fence_visgraph, &_destination_visgraph};
773 69 : for (uint8_t v=0; v<ARRAY_SIZE(visgraphs); v++) {
774 :
775 : // skip if empty
776 46 : const AP_OAVisGraph &curr_visgraph = *visgraphs[v];
777 46 : if (curr_visgraph.num_items() == 0) {
778 0 : continue;
779 : }
780 :
781 : // search visibility graph for items visible from current_node
782 874 : for (uint16_t i = 0; i < curr_visgraph.num_items(); i++) {
783 828 : const AP_OAVisGraph::VisGraphItem &item = curr_visgraph[i];
784 : // match if current node's id matches either of the id's in the graph (i.e. either end of the vector)
785 828 : if ((curr_node.id == item.id1) || (curr_node.id == item.id2)) {
786 125 : AP_OAVisGraph::OAItemID matching_id = (curr_node.id == item.id1) ? item.id2 : item.id1;
787 : // find item's id in node array
788 : node_index item_node_idx;
789 125 : if (find_node_from_id(matching_id, item_node_idx)) {
790 : // if current node's distance + distance to item is less than item's current distance, update item's distance
791 125 : const float dist_to_item_via_current_node = _short_path_data[curr_node_idx].distance_cm + item.distance_cm;
792 125 : if (dist_to_item_via_current_node < _short_path_data[item_node_idx].distance_cm) {
793 : // update item's distance and set "distance_from_idx" to current node's index
794 30 : _short_path_data[item_node_idx].distance_cm = dist_to_item_via_current_node;
795 30 : _short_path_data[item_node_idx].distance_from_idx = curr_node_idx;
796 : }
797 : }
798 : }
799 : }
800 : }
801 : }
802 :
803 : // find a node's index into _short_path_data array from it's id (i.e. id type and id number)
804 : // returns true if successful and node_idx is updated
805 154 : bool AP_OADijkstra::find_node_from_id(const AP_OAVisGraph::OAItemID &id, node_index &node_idx) const
806 : {
807 154 : switch (id.id_type) {
808 0 : case AP_OAVisGraph::OATYPE_SOURCE:
809 : // source node is always the first node
810 0 : if (_short_path_data_numpoints > 0) {
811 0 : node_idx = 0;
812 0 : return true;
813 : }
814 0 : break;
815 29 : case AP_OAVisGraph::OATYPE_DESTINATION:
816 : // destination is always the 2nd node
817 29 : if (_short_path_data_numpoints > 1) {
818 29 : node_idx = 1;
819 29 : return true;
820 : }
821 0 : break;
822 125 : case AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT:
823 : // intermediate nodes start from 3rd node
824 125 : if (_short_path_data_numpoints > id.id_num + 2) {
825 125 : node_idx = id.id_num + 2;
826 125 : return true;
827 : }
828 0 : break;
829 : }
830 :
831 : // could not find node
832 0 : return false;
833 : }
834 :
835 : // find index of node with lowest tentative distance (ignore visited nodes)
836 : // returns true if successful and node_idx argument is updated
837 25 : bool AP_OADijkstra::find_closest_node_idx(node_index &node_idx) const
838 : {
839 25 : node_index lowest_idx = 0;
840 25 : float lowest_dist = FLT_MAX;
841 :
842 : // scan through all nodes looking for closest
843 500 : for (node_index i=0; i<_short_path_data_numpoints; i++) {
844 475 : const ShortPathNode &node = _short_path_data[i];
845 475 : if (node.visited || is_equal(_short_path_data[i].distance_cm, FLT_MAX)) {
846 : // if node is already visited OR cannot be reached yet, we can't use it
847 375 : continue;
848 : }
849 : // figure out the pos of this node
850 100 : Vector2f node_pos;
851 100 : float dist_with_heuristics = FLT_MAX;
852 100 : if (convert_node_to_point(node.id, node_pos)) {
853 : // heuristics is is simple Euclidean distance from the node to the destination
854 : // This should be admissible, therefore optimal path is guaranteed
855 100 : const float heuristics = (node_pos-_path_destination).length();
856 100 : dist_with_heuristics = node.distance_cm + heuristics;
857 : } else {
858 : // shouldn't happen
859 0 : return false;
860 : }
861 100 : if (dist_with_heuristics < lowest_dist) {
862 : // for NOW, this is the closest node
863 48 : lowest_idx = i;
864 48 : lowest_dist = dist_with_heuristics;
865 : }
866 : }
867 :
868 25 : if (lowest_dist < FLT_MAX) {
869 : // found the closest node
870 25 : node_idx = lowest_idx;
871 25 : return true;
872 : }
873 0 : return false;
874 : }
875 :
876 : // calculate shortest path from origin to destination
877 : // returns true on success. returns false on failure and err_id is updated
878 : // requires these functions to have been run: create_inclusion_polygon_with_margin, create_exclusion_polygon_with_margin, create_exclusion_circle_with_margin, create_polygon_fence_visgraph
879 : // resulting path is stored in _shortest_path array as vector offsets from EKF origin
880 2 : bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &destination, AP_OADijkstra_Error &err_id)
881 : {
882 : // convert origin and destination to offsets from EKF origin
883 4 : if (!origin.get_vector_xy_from_origin_NE_cm(_path_source) ||
884 2 : !destination.get_vector_xy_from_origin_NE_cm(_path_destination)) {
885 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_NO_POSITION_ESTIMATE;
886 0 : return false;
887 : }
888 :
889 : // create visgraphs of origin and destination to fence points
890 2 : if (!update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, _path_source, true, _path_destination)) {
891 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
892 0 : return false;
893 : }
894 2 : if (!update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, _path_destination)) {
895 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
896 0 : return false;
897 : }
898 :
899 : // expand _short_path_data if necessary
900 2 : if (!_short_path_data.expand_to_hold(2 + total_numpoints())) {
901 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
902 0 : return false;
903 : }
904 :
905 : // add origin and destination (node_type, id, visited, distance_from_idx, distance_cm) to short_path_data array
906 2 : _short_path_data[0] = {{AP_OAVisGraph::OATYPE_SOURCE, 0}, false, 0, 0};
907 2 : _short_path_data[1] = {{AP_OAVisGraph::OATYPE_DESTINATION, 0}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
908 2 : _short_path_data_numpoints = 2;
909 :
910 : // add all inclusion and exclusion fence points to short_path_data array (node_type, id, visited, distance_from_idx, distance_cm)
911 36 : for (uint8_t i=0; i<total_numpoints(); i++) {
912 34 : _short_path_data[_short_path_data_numpoints++] = {{AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
913 : }
914 :
915 : // start algorithm from source point
916 2 : node_index current_node_idx = 0;
917 :
918 : // update nodes visible from source point
919 4 : for (uint16_t i = 0; i < _source_visgraph.num_items(); i++) {
920 : node_index node_idx;
921 2 : if (find_node_from_id(_source_visgraph[i].id2, node_idx)) {
922 2 : _short_path_data[node_idx].distance_cm = _source_visgraph[i].distance_cm;
923 2 : _short_path_data[node_idx].distance_from_idx = current_node_idx;
924 : } else {
925 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
926 0 : return false;
927 : }
928 : }
929 : // mark source node as visited
930 2 : _short_path_data[current_node_idx].visited = true;
931 :
932 : // move current_node_idx to node with lowest distance
933 25 : while (find_closest_node_idx(current_node_idx)) {
934 : node_index dest_node;
935 : // See if this next "closest" node is actually the destination
936 25 : if (find_node_from_id({AP_OAVisGraph::OATYPE_DESTINATION,0}, dest_node) && current_node_idx == dest_node) {
937 : // We have discovered destination.. Don't bother with the rest of the graph
938 2 : break;
939 : }
940 : // update distances to all neighbours of current node
941 23 : update_visible_node_distances(current_node_idx);
942 :
943 : // mark current node as visited
944 23 : _short_path_data[current_node_idx].visited = true;
945 : }
946 :
947 : // extract path starting from destination
948 2 : bool success = false;
949 : node_index nidx;
950 2 : if (!find_node_from_id({AP_OAVisGraph::OATYPE_DESTINATION,0}, nidx)) {
951 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
952 0 : return false;
953 : }
954 2 : _path_numpoints = 0;
955 : while (true) {
956 14 : if (!_path.expand_to_hold(_path_numpoints + 1)) {
957 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
958 0 : return false;
959 : }
960 : // fail if newest node has invalid distance_from_index
961 28 : if ((_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) ||
962 14 : (_short_path_data[nidx].distance_cm >= FLT_MAX)) {
963 0 : break;
964 : } else {
965 : // add node's id to path array
966 14 : _path[_path_numpoints] = _short_path_data[nidx].id;
967 14 : _path_numpoints++;
968 :
969 : // we are done if node is the source
970 14 : if (_short_path_data[nidx].id.id_type == AP_OAVisGraph::OATYPE_SOURCE) {
971 2 : success = true;
972 2 : break;
973 : } else {
974 : // follow node's "distance_from_idx" to previous node on path
975 12 : nidx = _short_path_data[nidx].distance_from_idx;
976 : }
977 : }
978 : }
979 : // report error in case path not found
980 2 : if (!success) {
981 0 : err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
982 : }
983 :
984 2 : return success;
985 : }
986 :
987 : // return point from final path as an offset (in cm) from the ekf origin
988 630 : bool AP_OADijkstra::get_shortest_path_point(uint8_t point_num, Vector2f& pos) const
989 : {
990 630 : if ((_path_numpoints == 0) || (point_num >= _path_numpoints)) {
991 0 : return false;
992 : }
993 :
994 : // get id from path
995 630 : AP_OAVisGraph::OAItemID id = _path[_path_numpoints - point_num - 1];
996 :
997 630 : return convert_node_to_point(id, pos);
998 : }
999 :
1000 : // find the position of a node as an offset (in cm) from the ekf origin
1001 730 : bool AP_OADijkstra::convert_node_to_point(const AP_OAVisGraph::OAItemID& id, Vector2f& pos) const
1002 : {
1003 : // convert id to a position offset from EKF origin
1004 730 : switch (id.id_type) {
1005 45 : case AP_OAVisGraph::OATYPE_SOURCE:
1006 45 : pos = _path_source;
1007 45 : return true;
1008 2 : case AP_OAVisGraph::OATYPE_DESTINATION:
1009 2 : pos = _path_destination;
1010 2 : return true;
1011 683 : case AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT:
1012 683 : return get_point(id.id_num, pos);
1013 : }
1014 :
1015 : // we should never reach here but just in case
1016 0 : return false;
1017 : }
1018 : #endif // AP_FENCE_ENABLED
1019 :
1020 :
1021 : #endif // AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
