Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: Dijkstra's returns control when clear of obstacles

This commit is contained in:
Randy Mackay 2023-12-14 10:36:14 +09:00
parent 5aeabc5779
commit cb1853b9bb
2 changed files with 67 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -40,19 +40,30 @@ AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) :
}
// calculate a destination to avoid fences
// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required
AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
// returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required
// next_destination_new will be non-zero if there is a next destination
// dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear
AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current_loc,
const Location &destination,
const Location &next_destination,
Location& origin_new,
Location& destination_new,
Location& next_destination_new,
bool& dest_to_next_dest_clear)
{
WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());
// avoidance is not required if no fences
if (!some_fences_enabled()) {
dest_to_next_dest_clear = _dest_to_next_dest_clear = true;
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
// no avoidance required if destination is same as current location
if (current_loc.same_latlon_as(destination)) {
// we do not check path to next destination so conservatively set to false
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
@ -83,6 +94,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_inclusion_polygon_with_margin_ok) {
_inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_inclusion_polygon_with_margin_ok) {
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
@ -93,6 +105,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_exclusion_polygon_with_margin_ok) {
_exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_exclusion_polygon_with_margin_ok) {
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
@ -103,6 +116,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_exclusion_circle_with_margin_ok) {
_exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_exclusion_circle_with_margin_ok) {
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
@ -114,6 +128,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_polyfence_visgraph_ok = create_fence_visgraph(error_id);
if (!_polyfence_visgraph_ok) {
_shortest_path_ok = false;
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
@ -133,9 +148,10 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
}
}
// rebuild path if destination has changed
if (!destination.same_latlon_as(_destination_prev)) {
// rebuild path if destination or next_destination has changed
if (!destination.same_latlon_as(_destination_prev) || !next_destination.same_latlon_as(_next_destination_prev)) {
_destination_prev = destination;
_next_destination_prev = next_destination;
_shortest_path_ok = false;
}
@ -143,17 +159,28 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_shortest_path_ok) {
_shortest_path_ok = calc_shortest_path(current_loc, destination, error_id);
if (!_shortest_path_ok) {
dest_to_next_dest_clear = _dest_to_next_dest_clear = false;
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
// start from 2nd point on path (first is the original origin)
_path_idx_returned = 1;
// check if path from destination to next_destination intersects with a fence
_dest_to_next_dest_clear = false;
if (!next_destination.is_zero()) {
Vector2f seg_start, seg_end;
if (destination.get_vector_xy_from_origin_NE(seg_start) && next_destination.get_vector_xy_from_origin_NE(seg_end)) {
_dest_to_next_dest_clear = !intersects_fence(seg_start, seg_end);
}
}
}
// path has been created, return latest point
Vector2f dest_pos;
if (get_shortest_path_point(_path_idx_returned, dest_pos)) {
uint8_t path_length = get_shortest_path_numpoints() > 0 ? (get_shortest_path_numpoints() - 1) : 0;
if ((_path_idx_returned < path_length) && get_shortest_path_point(_path_idx_returned, dest_pos)) {
// for the first point return origin as current_loc
Vector2f origin_pos;
@ -172,6 +199,23 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
destination_new.lat = temp_loc.lat;
destination_new.lng = temp_loc.lng;
// if present also provide next destination if present to allow smooth cornering
next_destination_new.zero();
Vector2f next_dest_pos;
if ((_path_idx_returned + 1 < path_length) && get_shortest_path_point(_path_idx_returned + 1, next_dest_pos)) {
// convert offset from ekf origin to Location
Location next_loc(Vector3f{next_dest_pos.x, next_dest_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN);
next_destination_new = destination;
next_destination_new.lat = next_loc.lat;
next_destination_new.lng = next_loc.lng;
} else {
// return destination as next_destination
next_destination_new = destination;
}
// path to next destination clear state is still valid from previous calcs (was calced along with shortest path)
dest_to_next_dest_clear = _dest_to_next_dest_clear;
// check if we should advance to next point for next iteration
const bool near_oa_wp = current_loc.get_distance(destination_new) <= 2.0f;
const bool past_oa_wp = current_loc.past_interval_finish_line(origin_new, destination_new);
@ -179,11 +223,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_path_idx_returned++;
}
// log success
Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new);
Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, get_shortest_path_numpoints(), destination, destination_new);
return DIJKSTRA_STATE_SUCCESS;
}
// we have reached the destination so avoidance is no longer required
// path to next destination clear state is still valid from previous calcs
dest_to_next_dest_clear = _dest_to_next_dest_clear;
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}

17
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -30,8 +30,16 @@ public:
};
// calculate a destination to avoid the polygon fence
// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required
AP_OADijkstra_State update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new);
// returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required
// next_destination_new will be non-zero if there is a next destination
// dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear
AP_OADijkstra_State update(const Location &current_loc,
const Location &destination,
const Location &next_destination,
Location& origin_new,
Location& destination_new,
Location& next_destination_new,
bool& dest_to_next_dest_clear);
private:
@ -124,7 +132,9 @@ private:
bool _shortest_path_ok;
Location _destination_prev; // destination of previous iterations (used to determine if path should be re-calculated)
Location _next_destination_prev;// next_destination of previous iterations (used to determine if path should be re-calculated)
uint8_t _path_idx_returned; // index into _path array which gives location vehicle should be currently moving towards
bool _dest_to_next_dest_clear; // true if path from dest to next_dest is clear (i.e. does not intersects a fence)
// inclusion polygon (with margin) related variables
float _polyfence_margin = 10; // margin around polygon defaults to 10m but is overriden with set_fence_margin
@ -181,6 +191,9 @@ private:
Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin)
Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin)
// return number of points on path
uint8_t get_shortest_path_numpoints() const { return _path_numpoints; }
// return point from final path as an offset (in cm) from the ekf origin
bool get_shortest_path_point(uint8_t point_num, Vector2f& pos) const;