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

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 &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;

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 &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

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,

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);

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;
 };