From caf5bfed59eb94a85fcfb8c869c06b2d1adefaf4 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Mon, 24 Aug 2020 12:34:35 +0530 Subject: [PATCH] AC_Avoid: Added new OA type (Dijkstra + BendyRuler fusion) --- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 47 ++++++++++------- libraries/AC_Avoidance/AP_OABendyRuler.h | 10 ++-- libraries/AC_Avoidance/AP_OADijkstra.h | 3 ++ libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 57 +++++++++++++++++++-- libraries/AC_Avoidance/AP_OAPathPlanner.h | 4 +- 5 files changed, 93 insertions(+), 28 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index dc186d05f3..e2e030d88a 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -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 ¤t_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 ¤t_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; diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.h b/libraries/AC_Avoidance/AP_OABendyRuler.h index 8aa668514c..7c4ebab675 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.h +++ b/libraries/AC_Avoidance/AP_OABendyRuler.h @@ -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 ¤t_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 ¤t_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 diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index 664146bbb9..056c8bb298 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -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, diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 1a28e6a2fc..e4b0a24174 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -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); diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index d34a5e0918..50af4587b5 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -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; };