diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 4c26fc921c..8960f8b3b8 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -19,12 +19,52 @@ #include #include +const float OA_BENDYRULER_LOOKAHEAD_DEFAULT = 15.0f; +const float OA_BENDYRULER_RATIO_DEFAULT = 1.5f; +const int16_t OA_BENDYRULER_ANGLE_DEFAULT = 75; + const int16_t OA_BENDYRULER_BEARING_INC = 5; // check every 5 degrees around vehicle const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length const float OA_BENDYRULER_LOOKAHEAD_STEP2_MIN = 2.0f; // step2 checks at least this many meters past step1's location const float OA_BENDYRULER_LOOKAHEAD_PAST_DEST = 2.0f; // lookahead length will be at least this many meters past the destination const float OA_BENDYRULER_LOW_SPEED_SQUARED = (0.2f * 0.2f); // when ground course is below this speed squared, vehicle's heading will be used +const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = { + + // @Param: LOOKAHEAD + // @DisplayName: Object Avoidance look ahead distance maximum + // @Description: Object Avoidance will look this many meters ahead of vehicle + // @Units: m + // @Range: 1 100 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("LOOKAHEAD", 1, AP_OABendyRuler, _lookahead, OA_BENDYRULER_LOOKAHEAD_DEFAULT), + + // @Param: CONT_RATIO + // @DisplayName: Obstacle Avoidance margin ratio for BendyRuler to change bearing significantly + // @Description: BendyRuler will avoid changing bearing unless ratio of previous margin from obstacle (or fence) to present calculated margin is atleast this much. + // @Range: 1.1 2 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("CONT_RATIO", 2, AP_OABendyRuler, _bendy_ratio, OA_BENDYRULER_RATIO_DEFAULT), + + // @Param: CONT_ANGLE + // @DisplayName: BendyRuler's bearing change resistance threshold angle + // @Description: BendyRuler will resist changing current bearing if the change in bearing is over this angle + // @Range: 20 180 + // @Increment: 5 + // @User: Standard + AP_GROUPINFO("CONT_ANGLE", 3, AP_OABendyRuler, _bendy_angle, OA_BENDYRULER_ANGLE_DEFAULT), + + AP_GROUPEND +}; + +AP_OABendyRuler::AP_OABendyRuler() +{ + AP_Param::setup_object_defaults(this, var_info); + _bearing_prev = FLT_MAX; +} + // 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) @@ -36,6 +76,9 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin const float bearing_to_dest = current_loc.get_bearing_to(destination) * 0.01f; const float distance_to_dest = current_loc.get_distance(destination); + // make sure user has set a meaningful value for _lookahead + _lookahead = MAX(_lookahead,1.0f); + // lookahead distance is adjusted dynamically based on avoidance results _current_lookahead = constrain_float(_current_lookahead, _lookahead * 0.5f, _lookahead); @@ -112,13 +155,18 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin // calculate minimum margin to fence and obstacles for this scenario float margin2 = calc_avoidance_margin(test_loc, test_loc2); if (margin2 > _margin_max) { + // if the chosen direction is directly towards the destination avoidance can be turned off + const bool active = (i != 0 || j != 0); + 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); + // all good, now project in the chosen direction by the full distance destination_new = current_loc; - destination_new.offset_bearing(bearing_test, distance_to_dest); + destination_new.offset_bearing(final_bearing, distance_to_dest); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f); - // if the chosen direction is directly towards the destination turn off avoidance - const bool active = (i != 0 || j != 0); - AP::logger().Write_OABendyRuler(active, bearing_to_dest, margin, destination, destination_new); + AP::logger().Write_OABendyRuler(active, bearing_to_dest, ignore_bearing_change, final_margin, destination, destination_new); return active; } } @@ -144,13 +192,59 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin destination_new.offset_bearing(chosen_bearing, distance_to_dest); // log results - AP::logger().Write_OABendyRuler(true, chosen_bearing, best_margin, destination, destination_new); + AP::logger().Write_OABendyRuler(true, chosen_bearing, false, best_margin, destination, destination_new); return true; } +/* +This function is called when BendyRuler has found a bearing which is obstacles free at atleast lookahead_step1_dist and then lookahead_step2_dist from the present location +In many situations, this new bearing can be either left or right of the obstacle, and BendyRuler can have a tough time deciding between the two. +It has the tendency to move the vehicle back and forth, if the margin obtained is even slightly better in the newer iteration. +Therefore, this method attempts to avoid changing direction of the vehicle by more than _bendy_angle degrees, +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 resisted_change = false; + // see if there was a change in destination, if so, do not resist changing bearing + bool dest_change = false; + if (!destination.same_latlon_as(prev_dest)) { + dest_change = true; + prev_dest = destination; + } + + // check if we need to resist the change in direction of the vehicle. If we have a clear path to destination, go there any how + if (active && !dest_change && is_positive(_bendy_ratio)) { + // check the change in bearing between freshly calculated and previous stored BendyRuler bearing + if ((fabsf(wrap_180(prev_bearing-bearing_test)) > _bendy_angle) && (!is_equal(prev_bearing,FLT_MAX))) { + // 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); + + if (margin < (_bendy_ratio * previous_bearing_margin)) { + // don't change direction abruptly. If margin difference is not significant, follow the last direction + final_bearing = prev_bearing; + final_margin = previous_bearing_margin; + resisted_change = true; + } + } + } else { + // reset stored bearing if BendyRuler is not active or if WP has changed for unnecessary resistance to path change + prev_bearing = FLT_MAX; + } + if (!resisted_change) { + // we are not resisting the change, hence store BendyRuler's presently calculated bearing for future iterations + prev_bearing = bearing_test; + } + + return resisted_change; +} + // calculate minimum distance between a segment and any obstacle -float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end) +float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end) const { float margin_min = FLT_MAX; @@ -177,7 +271,7 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati // calculate minimum distance between a path and the circular fence (centered on home) // on success returns true and updates margin -bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) +bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const { // exit immediately if polygon fence is not enabled const AC_Fence *fence = AC_Fence::get_singleton(); @@ -203,7 +297,7 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con // calculate minimum distance between a path and all inclusion and exclusion polygons // on success returns true and updates margin -bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) +bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const { const AC_Fence *fence = AC_Fence::get_singleton(); if (fence == nullptr) { @@ -269,7 +363,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo // calculate minimum distance between a path and all inclusion and exclusion circles // on success returns true and updates margin -bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) +bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const { // exit immediately if fence is not enabled const AC_Fence *fence = AC_Fence::get_singleton(); @@ -345,7 +439,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc // calculate minimum distance between a path and proximity sensor obstacles // on success returns true and updates margin -bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin) +bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const { // exit immediately if db is empty AP_OADatabase *oaDb = AP::oadatabase(); diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.h b/libraries/AC_Avoidance/AP_OABendyRuler.h index 649dc34b31..1305e29ea7 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.h +++ b/libraries/AC_Avoidance/AP_OABendyRuler.h @@ -10,45 +10,55 @@ */ class AP_OABendyRuler { public: - - AP_OABendyRuler() {} + AP_OABendyRuler(); /* Do not allow copies */ AP_OABendyRuler(const AP_OABendyRuler &other) = delete; AP_OABendyRuler &operator=(const AP_OABendyRuler&) = delete; // send configuration info stored in front end parameters - void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); } + void set_config(float margin_max) { _margin_max = MAX(margin_max, 0.0f); } // 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); + 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); + float calc_avoidance_margin(const Location &start, const Location &end) 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; // calculate minimum distance between a path and the circular fence (centered on home) // on success returns true and updates margin - bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin); + bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const; // calculate minimum distance between a path and all inclusion and exclusion polygons // on success returns true and updates margin - bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin); + bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const; // calculate minimum distance between a path and all inclusion and exclusion circles // on success returns true and updates margin - bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin); + bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const; // calculate minimum distance between a path and proximity sensor obstacles // on success returns true and updates margin - bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin); + bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const; - // configuration parameters - float _lookahead; // object avoidance will look this many meters ahead of vehicle + // OA common parameters float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle + + // BendyRuler parameters + AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle + AP_Float _bendy_ratio; // object avoidance will avoid major directional change if change in margin ratio is less than this param + AP_Int16 _bendy_angle; // object avoidance will try avoding change in direction over this much angle // internal variables used by background thread float _current_lookahead; // distance (in meters) ahead of the vehicle we are looking for obstacles + float _bearing_prev; // stored bearing in degrees + Location _destination_prev; // previous destination, to check if there has been a change in destination }; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 9e2a65c8ed..9fb6b0a752 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -24,7 +24,6 @@ extern const AP_HAL::HAL &hal; // parameter defaults -const float OA_LOOKAHEAD_DEFAULT = 15; const float OA_MARGIN_MAX_DEFAULT = 5; #if APM_BUILD_TYPE(APM_BUILD_Rover) const int16_t OA_OPTIONS_DEFAULT = 1; @@ -42,14 +41,8 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = { // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_OAPathPlanner, _type, OA_PATHPLAN_DISABLED, AP_PARAM_FLAG_ENABLE), - // @Param: LOOKAHEAD - // @DisplayName: Object Avoidance look ahead distance maximum - // @Description: Object Avoidance will look this many meters ahead of vehicle - // @Units: m - // @Range: 1 100 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("LOOKAHEAD", 2, AP_OAPathPlanner, _lookahead, OA_LOOKAHEAD_DEFAULT), + // Note: Do not use Index "2" for any new parameter + // It was being used by _LOOKAHEAD which was later moved to AP_OABendyRuler // @Param: MARGIN_MAX // @DisplayName: Object Avoidance wide margin distance @@ -73,6 +66,10 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT), #endif + + // @Group: BR_ + // @Path: AP_OABendyRuler.cpp + AP_SUBGROUPPTR(_oabendyruler, "BR_", 6, AP_OAPathPlanner, AP_OABendyRuler), AP_GROUPEND }; @@ -96,6 +93,7 @@ void AP_OAPathPlanner::init() case OA_PATHPLAN_BENDYRULER: if (_oabendyruler == nullptr) { _oabendyruler = new AP_OABendyRuler(); + AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info); } break; case OA_PATHPLAN_DIJKSTRA: @@ -259,7 +257,7 @@ void AP_OAPathPlanner::avoidance_thread() if (_oabendyruler == nullptr) { continue; } - _oabendyruler->set_config(_lookahead, _margin_max); + _oabendyruler->set_config(_margin_max); if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new)) { res = OA_SUCCESS; } diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index a7aae2a684..f750dd5969 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -90,9 +90,9 @@ private: // parameters AP_Int8 _type; // avoidance algorithm to be used - AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle AP_Float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle AP_Int16 _options; // Bitmask for options while recovering from Object Avoidance + // internal variables used by front end HAL_Semaphore _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result bool _thread_created; // true once background thread has been created