zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_Avoidance: OA_PathPlanner returns which planner was used 

this replaces get_bendy_type
zr-v5.1
Randy Mackay 4 years ago
parent
1536249b33
commit
f98efd7ef8
2 changed files with 48 additions and 18 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 47
      libraries/AC_Avoidance/AP_OAPathPlanner.cpp
  2. 19
      libraries/AC_Avoidance/AP_OAPathPlanner.h

47
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
Unescape View File

@ -111,15 +111,6 @@ void AP_OAPathPlanner::init() @@ -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() @@ -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 @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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
Unescape View File

@ -39,13 +39,23 @@ public: @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save