Skip to content

Commit

Permalink
AC_Avoidance: more messing about
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Dec 15, 2023
1 parent 545aec5 commit dbb0300
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 22 deletions.
25 changes: 12 additions & 13 deletions libraries/AC_Avoidance/AP_OADijkstra.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,14 @@ AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) :
// calculate a destination to avoid fences
// 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)
Location& next_destination_new,
bool& dest_to_next_dest_clear)
{
WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());

Expand Down Expand Up @@ -178,11 +180,6 @@ 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);

// debug
if (print_now) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: calc_shortest_path:%u", (unsigned)_shortest_path_ok);
}

if (!_shortest_path_ok) {
report_error(error_id);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Expand All @@ -192,23 +189,22 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_path_idx_returned = 1;

// check if path from destination to next_destination intersects with a fence
_dest_to_next_dest_intersects_fence = false;
_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_intersects_fence = intersects_fence(seg_start, seg_end);
_dest_to_next_dest_clear = !intersects_fence(seg_start, seg_end);
}
}
}

// debug
if (print_now) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: next intersects:%u", (unsigned)_dest_to_next_dest_intersects_fence);
// debug
if (print_now) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: calc_shortest_path:%u dest_to_next:%u", (unsigned)_shortest_path_ok, (unsigned)_dest_to_next_dest_clear);
}
}

// path has been created, return latest point
Vector2f dest_pos;
//uint8_t path_length = get_shortest_path_numpoints() + (_dest_to_next_dest_intersects_fence ? 0 : -1);
uint8_t path_length = get_shortest_path_numpoints() - 1;
if ((_path_idx_returned < path_length) && get_shortest_path_point(_path_idx_returned, dest_pos)) {

Expand Down Expand Up @@ -243,6 +239,9 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
next_destination_new = destination;
}

// indicate whether path from destination to next_destination is clear
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);
Expand Down
10 changes: 4 additions & 6 deletions libraries/AC_Avoidance/AP_OADijkstra.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,14 @@ class AP_OADijkstra {
// calculate a destination to avoid the polygon fence
// 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);

// return true if path from destionation to next_destination intersects a fence
// relies on update() having been called recently
bool dest_to_next_intersects_fence() const { return _dest_to_next_dest_intersects_fence; }
Location& next_destination_new,
bool& dest_to_next_dest_clear);

private:

Expand Down Expand Up @@ -136,7 +134,7 @@ class AP_OADijkstra {
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_intersects_fence; // true if path from dest to next_dest intersects a fence. relies on update() having been called recently
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
Expand Down
16 changes: 13 additions & 3 deletions libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,8 @@ AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplann
}

// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_origin, result_destination and result_next_destination with an intermediate path
// returns true and updates result_origin, result_destination, result_next_destination with an intermediate path
// result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear
// path_planner_used updated with which path planner produced the result
AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
const Location &origin,
Expand All @@ -195,6 +196,7 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
Location &result_origin,
Location &result_destination,
Location &result_next_destination,
bool &result_dest_to_next_dest_clear,
OAPathPlannerUsed &path_planner_used)
{
// exit immediately if disabled or thread is not running from a failed init
Expand Down Expand Up @@ -227,6 +229,7 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
result_origin = avoidance_result.origin_new;
result_destination = avoidance_result.destination_new;
result_next_destination = avoidance_result.next_destination_new;
result_dest_to_next_dest_clear = avoidance_result.dest_to_next_dest_clear;
path_planner_used = avoidance_result.path_planner_used;
return avoidance_result.ret_state;
}
Expand Down Expand Up @@ -271,9 +274,11 @@ void AP_OAPathPlanner::avoidance_thread()

_oadatabase.update();

// values returned by path planners
Location origin_new;
Location destination_new;
Location next_destination_new;
bool dest_to_next_dest_clear = false;
{
WITH_SEMAPHORE(_rsem);
if (now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) {
Expand Down Expand Up @@ -321,7 +326,8 @@ void AP_OAPathPlanner::avoidance_thread()
avoidance_request2.next_destination,
origin_new,
destination_new,
next_destination_new);
next_destination_new,
dest_to_next_dest_clear);
switch (dijkstra_state) {
case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
res = OA_NOT_REQUIRED;
Expand Down Expand Up @@ -367,7 +373,8 @@ void AP_OAPathPlanner::avoidance_thread()
avoidance_request2.next_destination,
origin_new,
destination_new,
next_destination_new);
next_destination_new,
dest_to_next_dest_clear);
switch (dijkstra_state) {
case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
res = OA_NOT_REQUIRED;
Expand Down Expand Up @@ -398,6 +405,9 @@ void AP_OAPathPlanner::avoidance_thread()
avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new;
avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination;
avoidance_result.next_destination_new = (res == OA_SUCCESS) ? next_destination_new : avoidance_result.next_destination;
avoidance_result.dest_to_next_dest_clear = (res == OA_SUCCESS) ? dest_to_next_dest_clear : avoidance_result.dest_to_next_dest_clear;

// create new avoidance result.dest_to_next_dest_clear field. fill in with results from dijkstras or leave as unknown
avoidance_result.result_time_ms = AP_HAL::millis();
avoidance_result.path_planner_used = path_planner_used;
avoidance_result.ret_state = res;
Expand Down
3 changes: 3 additions & 0 deletions libraries/AC_Avoidance/AP_OAPathPlanner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ class AP_OAPathPlanner {

// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_origin, result_destination and result_next_destination with an intermediate path
// result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear
// path_planner_used updated with which path planner produced the result
OA_RetState mission_avoidance(const Location &current_loc,
const Location &origin,
Expand All @@ -57,6 +58,7 @@ class AP_OAPathPlanner {
Location &result_origin,
Location &result_destination,
Location &result_next_destination,
bool &result_dest_to_next_dest_clear,
OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED;

// enumerations for _TYPE parameter
Expand Down Expand Up @@ -104,6 +106,7 @@ class AP_OAPathPlanner {
Location origin_new; // intermediate origin. The start of line segment that vehicle should follow
Location destination_new; // intermediate destination vehicle should move towards
Location next_destination_new; // intermediate next destination vehicle should move towards
bool dest_to_next_dest_clear; // true if the path from destination_new to next_destination_new is clear and does not require path planning
uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent)
OAPathPlannerUsed path_planner_used; // path planner that produced the result
OA_RetState ret_state; // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new
Expand Down

0 comments on commit dbb0300

Please sign in to comment.