Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Added new OA type (Dijkstra + BendyRuler fusion)

Browse Source
This commit is contained in:
Rishabh 2020-08-24 12:34:35 +05:30 committed by Randy Mackay
parent 80bee19bc9
commit caf5bfed59
5 changed files with 93 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -79,7 +79,7 @@ AP_OABendyRuler::AP_OABendyRuler()
// run background task to find best path and update avoidance_results
// returns true and updates origin_new and destination_new if a best path has been found
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new)
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, bool proximity_only)
{
// bendy ruler always sets origin to current_loc
origin_new = current_loc;
@ -114,20 +114,20 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
switch (get_type()) {
case OABendyType::OA_BENDY_VERTICAL:
#if VERTICAL_ENABLED
ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
break;
#endif
case OABendyType::OA_BENDY_HORIZONTAL:
default:
ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
}
return ret;
}
// Search for path in the horizontal directions
bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest )
bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only)
{
// check OA_BEARING_INC definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC_XY == 0, "check 360 is a multiple of OA_BEARING_INC");
@ -157,7 +157,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
test_loc.offset_bearing(bearing_test, lookahead_step1_dist);
// calculate margin from obstacles for this scenario
float margin = calc_avoidance_margin(current_loc, test_loc);
float margin = calc_avoidance_margin(current_loc, test_loc, proximity_only);
if (margin > best_margin) {
best_margin_bearing = bearing_test;
best_margin = margin;
@ -184,7 +184,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
test_loc2.offset_bearing(bearing_test2, distance2);
// calculate minimum margin to fence and obstacles for this scenario
float margin2 = calc_avoidance_margin(test_loc, test_loc2);
float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only);
if (margin2 > _margin_max) {
// if the chosen direction is directly towards the destination avoidance can be turned off
// i == 0 && j == 0 implies no deviation from bearing to destination
@ -192,7 +192,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
float final_bearing = bearing_test;
float final_margin = margin;
// check if we need ignore test_bearing and continue on previous bearing
const bool ignore_bearing_change = resist_bearing_change(destination, current_loc, active, bearing_test, lookahead_step1_dist, margin, _destination_prev,_bearing_prev, final_bearing, final_margin);
const bool ignore_bearing_change = resist_bearing_change(destination, current_loc, active, bearing_test, lookahead_step1_dist, margin, _destination_prev,_bearing_prev, final_bearing, final_margin, proximity_only);
// all good, now project in the chosen direction by the full distance
destination_new = current_loc;
@ -230,7 +230,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
}
// Search for path in the vertical directions
bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest)
bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest, bool proximity_only)
{
// check OA_BEARING_INC_VERTICAL definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL");
@ -254,7 +254,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
test_loc.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, lookahead_step1_dist);
// calculate margin from obstacles for this scenario
float margin = calc_avoidance_margin(current_loc, test_loc);
float margin = calc_avoidance_margin(current_loc, test_loc, proximity_only);
if (margin > best_margin) {
best_margin_pitch = pitch_delta;
@ -282,18 +282,18 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2 ,distance2);
// calculate minimum margin to fence and obstacles for this scenario
float margin2 = calc_avoidance_margin(test_loc, test_loc2);
float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only);
if (margin2 > _margin_max) {
// if the chosen direction is directly towards the destination we might turn off avoidance
// i == 0 && j == 0 implies no deviation from bearing to destination
bool active = (i != 0 || j != 0);
if (!active) {
// do a sub test to confirm if we should really turn of BendyRuler
// do a sub test for proximity obstacles to confirm if we should really turn of BendyRuler
const float sub_test_pitch_step2[] {-90.0f, 90.0f};
for (uint8_t k = 0; k < ARRAY_SIZE(sub_test_pitch_step2); k++) {
Location test_loc_sub_test = test_loc;
test_loc_sub_test.offset_bearing_and_pitch(bearing_to_dest2, sub_test_pitch_step2[k], _margin_max);
float margin_sub_test = calc_avoidance_margin(test_loc, test_loc_sub_test);
float margin_sub_test = calc_avoidance_margin(test_loc, test_loc_sub_test, true);
if (margin_sub_test < _margin_max) {
// BendyRuler will remain active
active = true;
@ -361,7 +361,7 @@ Therefore, this method attempts to avoid changing direction of the vehicle by mo
unless the new margin is atleast _bendy_ratio times better than the margin with previously calculated bearing.
We return true if we have resisted the change and will follow the last calculated bearing.
*/
bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin) const
bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin, bool proximity_only) const
{
bool resisted_change = false;
// see if there was a change in destination, if so, do not resist changing bearing
@ -378,7 +378,7 @@ bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const L
// check margin in last bearing's direction
Location test_loc_previous_bearing = current_loc;
test_loc_previous_bearing.offset_bearing(wrap_180(prev_bearing), lookahead_step1_dist);
float previous_bearing_margin = calc_avoidance_margin(current_loc,test_loc_previous_bearing);
float previous_bearing_margin = calc_avoidance_margin(current_loc,test_loc_previous_bearing, proximity_only);
if (margin < (_bendy_ratio * previous_bearing_margin)) {
// don't change direction abruptly. If margin difference is not significant, follow the last direction
@ -400,11 +400,21 @@ bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const L
}
// calculate minimum distance between a segment and any obstacle
float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end) const
float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const
{
float margin_min = FLT_MAX;
float latest_margin;
if (calc_margin_from_object_database(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
if (proximity_only) {
// only need margin from proximity data
return margin_min;
}
if (calc_margin_from_circular_fence(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
@ -418,10 +428,6 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
}
#endif
if (calc_margin_from_object_database(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
if (calc_margin_from_inclusion_and_exclusion_polygons(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
@ -649,6 +655,9 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
if (!start.get_vector_from_origin_NEU(start_NEU) || !end.get_vector_from_origin_NEU(end_NEU)) {
return false;
}
if (start_NEU == end_NEU) {
return false;
}
// check each obstacle's distance from segment
float smallest_margin = FLT_MAX;

10
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -21,7 +21,7 @@ public:
// 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);
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, bool proximity_only);
enum class OABendyType {
OA_BENDY_DISABLED = 0,
@ -33,20 +33,20 @@ public:
OABendyType get_type() const;
// search for path in XY direction
bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest);
bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only);
// search for path in the Vertical directions
bool search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest);
bool search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest, bool proximity_only);
static const struct AP_Param::GroupInfo var_info[];
private:
// calculate minimum distance between a path and any obstacle
float calc_avoidance_margin(const Location &start, const Location &end) const;
float calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const;
// determine if BendyRuler should accept the new bearing or try and resist it. Returns true if bearing is not changed
bool resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin) const;
bool resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin, bool proximity_only) const;
// calculate minimum distance between a path and the circular fence (centered on home)
// on success returns true and updates margin

3
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -22,6 +22,9 @@ 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); }
// trigger Dijkstra's to recalculate shortest path based on current location
void recalculate_path() { _shortest_path_ok = false; }
// update return status enum
enum AP_OADijkstra_State : uint8_t {
DIJKSTRA_STATE_NOT_REQUIRED = 0,

57
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -37,7 +37,7 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
// @Param: TYPE
// @DisplayName: Object Avoidance Path Planning algorithm to use
// @Description: Enabled/disable path planning around obstacles
// @Values: 0:Disabled,1:BendyRuler,2:Dijkstra
// @Values: 0:Disabled,1:BendyRuler,2:Dijkstra,3:Dijkstra with BendyRuler
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_OAPathPlanner, _type, OA_PATHPLAN_DISABLED, AP_PARAM_FLAG_ENABLE),
@ -101,6 +101,15 @@ void AP_OAPathPlanner::init()
_oadijkstra = new AP_OADijkstra();
}
break;
case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra();
}
if (_oabendyruler == nullptr) {
_oabendyruler = new AP_OABendyRuler();
AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
}
break;
}
_oadatabase.init();
@ -136,6 +145,12 @@ bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
return false;
}
break;
case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
if(_oadijkstra == nullptr || _oabendyruler == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "OA requires reboot");
return false;
}
break;
}
return true;
}
@ -267,12 +282,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)) {
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, false)) {
res = OA_SUCCESS;
}
break;
case OA_PATHPLAN_DIJKSTRA:
case OA_PATHPLAN_DIJKSTRA: {
if (_oadijkstra == nullptr) {
continue;
}
@ -292,6 +307,42 @@ void AP_OAPathPlanner::avoidance_thread()
break;
}
case OA_PATHPLAN_DJIKSTRA_BENDYRULER: {
if ((_oabendyruler == nullptr) || _oadijkstra == 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, 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;
break;
} else {
// cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position
if (proximity_only == false) {
_oadijkstra->recalculate_path();
}
// only use proximity avoidance now for BendyRuler
proximity_only = true;
}
_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);
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;
}
}
{
// give the main thread the avoidance result
WITH_SEMAPHORE(_rsem);

4
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -51,7 +51,8 @@ public:
enum OAPathPlanTypes {
OA_PATHPLAN_DISABLED = 0,
OA_PATHPLAN_BENDYRULER = 1,
OA_PATHPLAN_DIJKSTRA = 2
OA_PATHPLAN_DIJKSTRA = 2,
OA_PATHPLAN_DJIKSTRA_BENDYRULER = 3,
};
// enumeration for _OPTION parameter
@ -104,6 +105,7 @@ private:
AP_OADatabase _oadatabase; // Database of dynamic objects to avoid
uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran
bool proximity_only = true;
static AP_OAPathPlanner *_singleton;
};