aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OAPathPlanner: report errors to caller

Browse Source
This commit is contained in:
Randy Mackay 2019-06-26 16:15:41 +09:00
parent 104164d2e4
commit 951ded1f51
5 changed files with 64 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -22,6 +22,7 @@ public:
void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); }
// run background task to find best path and update avoidance_results
// returns true and populates origin_new and destination_new if OA is required. returns false if OA is not required
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
private:

18
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -28,18 +28,18 @@ AP_OADijkstra::AP_OADijkstra() :
}
// calculate a destination to avoid the polygon fence
// returns true if avoidance is required and updates origin_new and destination_new
bool AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
// 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)
{
// require ekf origin to have been set
struct Location ekf_origin {};
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
return DIJKSTRA_STATE_NOT_REQUIRED;
}
// no avoidance required if fence is disabled
if (!polygon_fence_enabled()) {
return false;
return DIJKSTRA_STATE_NOT_REQUIRED;
}
// check for fence updates
@ -53,7 +53,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
if (!_polyfence_with_margin_ok) {
_polyfence_visgraph_ok = false;
_shortest_path_ok = false;
return false;
return DIJKSTRA_STATE_ERROR;
}
}
@ -62,7 +62,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
_polyfence_visgraph_ok = create_polygon_fence_visgraph();
if (!_polyfence_visgraph_ok) {
_shortest_path_ok = false;
return false;
return DIJKSTRA_STATE_ERROR;
}
}
@ -76,7 +76,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
if (!_shortest_path_ok) {
_shortest_path_ok = calc_shortest_path(current_loc, destination);
if (!_shortest_path_ok) {
return false;
return DIJKSTRA_STATE_ERROR;
}
// start from 2nd point on path (first is the original origin)
_path_idx_returned = 1;
@ -109,10 +109,10 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
if (near_oa_wp || past_oa_wp) {
_path_idx_returned++;
}
return true;
return DIJKSTRA_STATE_SUCCESS;
}
return false;
return DIJKSTRA_STATE_ERROR;
}
// returns true if polygon fence is enabled

11
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -22,9 +22,16 @@ public:
// set fence margin (in meters) used when creating "safe positions" within the polygon fence
void set_fence_margin(float margin) { _polyfence_margin = MAX(margin, 0.0f); }
// update return status enum
enum AP_OADijkstra_State : uint8_t {
DIJKSTRA_STATE_NOT_REQUIRED = 0,
DIJKSTRA_STATE_ERROR,
DIJKSTRA_STATE_SUCCESS
};
// calculate a destination to avoid the polygon fence
// returns true if avoidance is required and target destination is placed in result_loc
bool update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new);
// 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);
private:

49
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -114,7 +114,7 @@ bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_loc with an intermediate location
bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
Location &result_origin,
@ -122,7 +122,7 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
{
// exit immediately if disabled
if (_type == OA_PATHPLAN_DISABLED) {
return false;
return OA_NOT_REQUIRED;
}
WITH_SEMAPHORE(_rsem);
@ -134,7 +134,7 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OAPathPlanner::avoidance_thread, void),
"avoidance",
8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
return false;
return OA_ERROR;
}
_thread_created = true;
}
@ -148,10 +148,14 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
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 results have not timed out
const bool timed_out = now - avoidance_result.result_time_ms > OA_TIMEOUT_MS;
// return results from background thread's latest checks
if (destination.lat == avoidance_result.destination.lat &&
destination.lng == avoidance_result.destination.lng &&
now - avoidance_result.result_time_ms < OA_TIMEOUT_MS) {
if (destination_matches && !timed_out) {
// we have a result from the thread
result_origin = avoidance_result.origin_new;
result_destination = avoidance_result.destination_new;
@ -160,12 +164,16 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
_logged_time_ms = avoidance_result.result_time_ms;
AP::logger().Write_OA(_type, destination, result_destination);
}
return avoidance_result.avoidance_needed;
return avoidance_result.ret_state;
}
// do not performance avoidance because background thread's results were
// run against a different destination or they are simply not required
return false;
// if timeout then path planner is taking too long to respond
if (timed_out) {
return OA_ERROR;
}
// background thread is working on a new destination
return OA_PROCESSING;
}
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request
@ -195,7 +203,7 @@ void AP_OAPathPlanner::avoidance_thread()
}
// run background task looking for best alternative destination
bool res = false;
OA_RetState res = OA_NOT_REQUIRED;
switch (_type) {
case OA_PATHPLAN_DISABLED:
continue;
@ -204,14 +212,27 @@ void AP_OAPathPlanner::avoidance_thread()
continue;
}
_oabendyruler->set_config(_lookahead, _margin_max);
res = _oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new);
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new)) {
res = OA_SUCCESS;
}
break;
case OA_PATHPLAN_DIJKSTRA:
if (_oadijkstra == nullptr) {
continue;
}
_oadijkstra->set_fence_margin(_margin_max);
res = _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, origin_new, destination_new);
switch (dijkstra_state) {
case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
res = OA_NOT_REQUIRED;
break;
case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
res = OA_ERROR;
break;
case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
res = OA_SUCCESS;
break;
}
break;
}
@ -222,7 +243,7 @@ void AP_OAPathPlanner::avoidance_thread()
avoidance_result.origin_new = res ? origin_new : avoidance_result.origin_new;
avoidance_result.destination_new = res ? destination_new : avoidance_result.destination;
avoidance_result.result_time_ms = AP_HAL::millis();
avoidance_result.avoidance_needed = res;
avoidance_result.ret_state = res;
}
}
}

12
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -30,9 +30,17 @@ public:
/// returns true if all pre-takeoff checks have completed successfully
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// object avoidance processing return status enum
enum OA_RetState : uint8_t {
OA_NOT_REQUIRED = 0, // object avoidance is not required
OA_PROCESSING, // still calculating alternative path
OA_ERROR, // error during calculation
OA_SUCCESS // success
};
// 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
bool mission_avoidance(const Location &current_loc,
OA_RetState mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
Location &result_origin,
@ -67,7 +75,7 @@ private:
Location origin_new; // intermediate origin. The start of line segment that vehicle should follow
Location destination_new; // intermediate destination vehicle should move towards
uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent)
bool avoidance_needed; // true if the vehicle should move along the path from origin_new to destination_new
OA_RetState ret_state; // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new
} avoidance_result;
// parameters