diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 8a0913c63b..0083f64795 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -40,19 +40,30 @@ AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) : } // calculate a destination to avoid fences -// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required -AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new) +// returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required +// next_destination_new will be non-zero if there is a next destination +// dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear +AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_loc, + const Location &destination, + const Location &next_destination, + Location& origin_new, + Location& destination_new, + Location& next_destination_new, + bool& dest_to_next_dest_clear) { WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore()); // avoidance is not required if no fences if (!some_fences_enabled()) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = true; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } // no avoidance required if destination is same as current location if (current_loc.same_latlon_as(destination)) { + // we do not check path to next destination so conservatively set to false + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } @@ -83,6 +94,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_inclusion_polygon_with_margin_ok) { _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); if (!_inclusion_polygon_with_margin_ok) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; report_error(error_id); Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; @@ -93,6 +105,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_exclusion_polygon_with_margin_ok) { _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); if (!_exclusion_polygon_with_margin_ok) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; report_error(error_id); Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; @@ -103,6 +116,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_exclusion_circle_with_margin_ok) { _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id); if (!_exclusion_circle_with_margin_ok) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; report_error(error_id); Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; @@ -114,6 +128,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t _polyfence_visgraph_ok = create_fence_visgraph(error_id); if (!_polyfence_visgraph_ok) { _shortest_path_ok = false; + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; report_error(error_id); Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; @@ -133,9 +148,10 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t } } - // rebuild path if destination has changed - if (!destination.same_latlon_as(_destination_prev)) { + // rebuild path if destination or next_destination has changed + if (!destination.same_latlon_as(_destination_prev) || !next_destination.same_latlon_as(_next_destination_prev)) { _destination_prev = destination; + _next_destination_prev = next_destination; _shortest_path_ok = false; } @@ -143,17 +159,28 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_shortest_path_ok) { _shortest_path_ok = calc_shortest_path(current_loc, destination, error_id); if (!_shortest_path_ok) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; report_error(error_id); Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } // start from 2nd point on path (first is the original origin) _path_idx_returned = 1; + + // check if path from destination to next_destination intersects with a fence + _dest_to_next_dest_clear = false; + if (!next_destination.is_zero()) { + Vector2f seg_start, seg_end; + if (destination.get_vector_xy_from_origin_NE(seg_start) && next_destination.get_vector_xy_from_origin_NE(seg_end)) { + _dest_to_next_dest_clear = !intersects_fence(seg_start, seg_end); + } + } } // path has been created, return latest point Vector2f dest_pos; - if (get_shortest_path_point(_path_idx_returned, dest_pos)) { + uint8_t path_length = get_shortest_path_numpoints() > 0 ? (get_shortest_path_numpoints() - 1) : 0; + if ((_path_idx_returned < path_length) && get_shortest_path_point(_path_idx_returned, dest_pos)) { // for the first point return origin as current_loc Vector2f origin_pos; @@ -172,6 +199,23 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t destination_new.lat = temp_loc.lat; destination_new.lng = temp_loc.lng; + // if present also provide next destination if present to allow smooth cornering + next_destination_new.zero(); + Vector2f next_dest_pos; + if ((_path_idx_returned + 1 < path_length) && get_shortest_path_point(_path_idx_returned + 1, next_dest_pos)) { + // convert offset from ekf origin to Location + Location next_loc(Vector3f{next_dest_pos.x, next_dest_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN); + next_destination_new = destination; + next_destination_new.lat = next_loc.lat; + next_destination_new.lng = next_loc.lng; + } else { + // return destination as next_destination + next_destination_new = destination; + } + + // path to next destination clear state is still valid from previous calcs (was calced along with shortest path) + dest_to_next_dest_clear = _dest_to_next_dest_clear; + // check if we should advance to next point for next iteration const bool near_oa_wp = current_loc.get_distance(destination_new) <= 2.0f; const bool past_oa_wp = current_loc.past_interval_finish_line(origin_new, destination_new); @@ -179,11 +223,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t _path_idx_returned++; } // log success - Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new); + Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, get_shortest_path_numpoints(), destination, destination_new); return DIJKSTRA_STATE_SUCCESS; } // we have reached the destination so avoidance is no longer required + // path to next destination clear state is still valid from previous calcs + dest_to_next_dest_clear = _dest_to_next_dest_clear; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index b37b644c44..97c832a78a 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -30,8 +30,16 @@ public: }; // calculate a destination to avoid the polygon fence - // returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required - AP_OADijkstra_State update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new); + // returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required + // next_destination_new will be non-zero if there is a next destination + // dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear + AP_OADijkstra_State update(const Location ¤t_loc, + const Location &destination, + const Location &next_destination, + Location& origin_new, + Location& destination_new, + Location& next_destination_new, + bool& dest_to_next_dest_clear); private: @@ -124,7 +132,9 @@ private: bool _shortest_path_ok; Location _destination_prev; // destination of previous iterations (used to determine if path should be re-calculated) + Location _next_destination_prev;// next_destination of previous iterations (used to determine if path should be re-calculated) uint8_t _path_idx_returned; // index into _path array which gives location vehicle should be currently moving towards + bool _dest_to_next_dest_clear; // true if path from dest to next_dest is clear (i.e. does not intersects a fence) // inclusion polygon (with margin) related variables float _polyfence_margin = 10; // margin around polygon defaults to 10m but is overriden with set_fence_margin @@ -181,6 +191,9 @@ private: Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin) Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin) + // return number of points on path + uint8_t get_shortest_path_numpoints() const { return _path_numpoints; } + // return point from final path as an offset (in cm) from the ekf origin bool get_shortest_path_point(uint8_t point_num, Vector2f& pos) const;