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

AC_Avoidance: OA_PathPlanner returns which planner was used 

this replaces get_bendy_type
This commit is contained in:
Randy Mackay 2021-03-24 11:33:04 +09:00
parent 980909deb5
commit 0b26bace7f
2 changed files with 48 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -111,15 +111,6 @@ void AP_OAPathPlanner::init()
start_thread();
}
// return type of BendyRuler in use
AP_OABendyRuler::OABendyType AP_OAPathPlanner::get_bendy_type() const
{
if (_oabendyruler == nullptr) {
return AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED;
}
return _oabendyruler->get_type();
}
// pre-arm checks that algorithms have been initialised successfully
bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
@ -173,13 +164,30 @@ bool AP_OAPathPlanner::start_thread()
return true;
}
// helper function to map OABendyType to OAPathPlannerUsed
AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type)
{
switch (bendy_type) {
case AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL:
return OAPathPlannerUsed::BendyRulerHorizontal;
case AP_OABendyRuler::OABendyType::OA_BENDY_VERTICAL:
return OAPathPlannerUsed::BendyRulerVertical;
default:
case AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED:
return OAPathPlannerUsed::None;
}
}
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_loc with an intermediate location
AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
Location &result_origin,
Location &result_destination)
Location &result_destination,
OAPathPlannerUsed &path_planner_used)
{
// exit immediately if disabled or thread is not running from a failed init
if (_type == OA_PATHPLAN_DISABLED || !_thread_created) {
@ -207,6 +215,7 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
// we have a result from the thread
result_origin = avoidance_result.origin_new;
result_destination = avoidance_result.destination_new;
path_planner_used = avoidance_result.path_planner_used;
return avoidance_result.ret_state;
}
@ -269,18 +278,23 @@ void AP_OAPathPlanner::avoidance_thread()
// run background task looking for best alternative destination
OA_RetState res = OA_NOT_REQUIRED;
OAPathPlannerUsed path_planner_used = OAPathPlannerUsed::None;
switch (_type) {
case OA_PATHPLAN_DISABLED:
continue;
case OA_PATHPLAN_BENDYRULER:
case OA_PATHPLAN_BENDYRULER: {
if (_oabendyruler == nullptr) {
continue;
}
_oabendyruler->set_config(_margin_max);
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, false)) {
AP_OABendyRuler::OABendyType bendy_type;
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, false)) {
res = OA_SUCCESS;
}
path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
break;
}
case OA_PATHPLAN_DIJKSTRA: {
if (_oadijkstra == nullptr) {
@ -299,6 +313,7 @@ void AP_OAPathPlanner::avoidance_thread()
res = OA_SUCCESS;
break;
}
path_planner_used = OAPathPlannerUsed::Dijkstras;
break;
}
@ -307,10 +322,12 @@ void AP_OAPathPlanner::avoidance_thread()
continue;
}
_oabendyruler->set_config(_margin_max);
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, proximity_only)) {
AP_OABendyRuler::OABendyType bendy_type;
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, proximity_only)) {
// detected a obstacle by vehicle's proximity sensor. Switch avoidance to BendyRuler till obstacle is out of the way
proximity_only = false;
res = OA_SUCCESS;
path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
break;
} else {
// cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position
@ -333,10 +350,11 @@ void AP_OAPathPlanner::avoidance_thread()
res = OA_SUCCESS;
break;
}
path_planner_used = OAPathPlannerUsed::Dijkstras;
break;
}
}
} // switch
{
// give the main thread the avoidance result
@ -345,6 +363,7 @@ 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.result_time_ms = AP_HAL::millis();
avoidance_result.path_planner_used = path_planner_used;
avoidance_result.ret_state = res;
}
}

19
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -39,13 +39,23 @@ public:
OA_SUCCESS // success
};
// path planner responsible for a particular result
enum OAPathPlannerUsed : uint8_t {
None = 0,
BendyRulerHorizontal,
BendyRulerVertical,
Dijkstras
};
// 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
// 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,
Location &result_origin,
Location &result_destination) WARN_IF_UNUSED;
Location &result_destination,
OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED;
// enumerations for _TYPE parameter
enum OAPathPlanTypes {
@ -63,9 +73,6 @@ public:
uint16_t get_options() const { return _options;}
// helper function to return type of BendyRuler in use. This is used by AC_WPNav_OA
AP_OABendyRuler::OABendyType get_bendy_type() const;
static const struct AP_Param::GroupInfo var_info[];
private:
@ -74,6 +81,9 @@ private:
void avoidance_thread();
bool start_thread();
// helper function to map OABendyType to OAPathPlannerUsed
OAPathPlannerUsed map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type);
// an avoidance request from the navigation code
struct avoidance_info {
Location current_loc;
@ -89,6 +99,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)
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
} avoidance_result;