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

AC_Avoidance: path planner accepts next destination

This commit is contained in:
Randy Mackay 2023-12-14 15:19:51 +09:00
parent 82984577d2
commit 5aeabc5779
2 changed files with 51 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -186,12 +186,17 @@ 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_loc with an intermediate location
// 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 (only supported by Dijkstras)
// 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,
const Location &destination,
const Location &next_destination,
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
@ -212,20 +217,25 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
avoidance_request.current_loc = current_loc;
avoidance_request.origin = origin;
avoidance_request.destination = destination;
avoidance_request.next_destination = next_destination;
avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector();
avoidance_request.request_time_ms = now;
// check result's destination matches our request
const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng);
// check result's destination and next_destination matches our request
// e.g. check this result was using our current inputs and not from an old request
const bool destination_matches = destination.same_latlon_as(avoidance_result.destination);
const bool next_destination_matches = next_destination.same_latlon_as(avoidance_result.next_destination);
// check results have not timed out
const bool timed_out = (now - avoidance_result.result_time_ms > OA_TIMEOUT_MS) && (now - _activated_ms > OA_TIMEOUT_MS);
// return results from background thread's latest checks
if (destination_matches && !timed_out) {
if (destination_matches && next_destination_matches && !timed_out) {
// we have a result from the thread
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;
}
@ -270,8 +280,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) {
@ -282,9 +295,10 @@ void AP_OAPathPlanner::avoidance_thread()
// copy request to avoid conflict with main thread
avoidance_request2 = avoidance_request;
// store passed in origin and destination so we can return it if object avoidance is not required
// store passed in origin, destination and next_destination so we can return it if object avoidance is not required
origin_new = avoidance_request.origin;
destination_new = avoidance_request.destination;
next_destination_new = avoidance_request.next_destination;
}
// run background task looking for best alternative destination
@ -313,7 +327,13 @@ void AP_OAPathPlanner::avoidance_thread()
continue;
}
_oadijkstra->set_fence_margin(_margin_max);
const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
avoidance_request2.destination,
avoidance_request2.next_destination,
origin_new,
destination_new,
next_destination_new,
dest_to_next_dest_clear);
switch (dijkstra_state) {
case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
res = OA_NOT_REQUIRED;
@ -354,7 +374,13 @@ void AP_OAPathPlanner::avoidance_thread()
}
#if AP_FENCE_ENABLED
_oadijkstra->set_fence_margin(_margin_max);
const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
avoidance_request2.destination,
avoidance_request2.next_destination,
origin_new,
destination_new,
next_destination_new,
dest_to_next_dest_clear);
switch (dijkstra_state) {
case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
res = OA_NOT_REQUIRED;
@ -376,9 +402,18 @@ void AP_OAPathPlanner::avoidance_thread()
{
// give the main thread the avoidance result
WITH_SEMAPHORE(_rsem);
// place the destination and next destination used into the result (used by the caller to verify the result matches their request)
avoidance_result.destination = avoidance_request2.destination;
avoidance_result.next_destination = avoidance_request2.next_destination;
avoidance_result.dest_to_next_dest_clear = dest_to_next_dest_clear;
// fill the result structure with the intermediate path
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;
// 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;

10
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -48,13 +48,17 @@ public:
};
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_origin and result_destination with an intermediate path
// 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 (only supported by Dijkstras)
// path_planner_used updated with which path planner produced the result
OA_RetState mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
const Location &next_destination,
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
@ -90,6 +94,7 @@ private:
Location current_loc;
Location origin;
Location destination;
Location next_destination;
Vector2f ground_speed_vec;
uint32_t request_time_ms;
} avoidance_request, avoidance_request2;
@ -97,8 +102,11 @@ private:
// an avoidance result from the avoidance thread
struct {
Location destination; // destination vehicle is trying to get to (also used to verify the result matches a recent request)
Location next_destination; // next destination vehicle is trying to get to (also used to verify the result matches a recent request)
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 (only supported by Dijkstras)
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