diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 95ad4a1b07..48595b4fb4 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -186,12 +186,17 @@ AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplann } // provides an alternative target location if path planning around obstacles is required -// returns true and updates result_loc with an intermediate location +// returns true and updates result_origin, result_destination, result_next_destination with an intermediate path +// result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras) +// path_planner_used updated with which path planner produced the result AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, + const Location &next_destination, Location &result_origin, Location &result_destination, + Location &result_next_destination, + bool &result_dest_to_next_dest_clear, OAPathPlannerUsed &path_planner_used) { // exit immediately if disabled or thread is not running from a failed init @@ -212,20 +217,25 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location avoidance_request.current_loc = current_loc; avoidance_request.origin = origin; avoidance_request.destination = destination; + avoidance_request.next_destination = next_destination; avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector(); avoidance_request.request_time_ms = now; - // check result's destination matches our request - const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng); + // check result's destination and next_destination matches our request + // e.g. check this result was using our current inputs and not from an old request + const bool destination_matches = destination.same_latlon_as(avoidance_result.destination); + const bool next_destination_matches = next_destination.same_latlon_as(avoidance_result.next_destination); // check results have not timed out const bool timed_out = (now - avoidance_result.result_time_ms > OA_TIMEOUT_MS) && (now - _activated_ms > OA_TIMEOUT_MS); // return results from background thread's latest checks - if (destination_matches && !timed_out) { + if (destination_matches && next_destination_matches && !timed_out) { // we have a result from the thread result_origin = avoidance_result.origin_new; result_destination = avoidance_result.destination_new; + result_next_destination = avoidance_result.next_destination_new; + result_dest_to_next_dest_clear = avoidance_result.dest_to_next_dest_clear; path_planner_used = avoidance_result.path_planner_used; return avoidance_result.ret_state; } @@ -270,8 +280,11 @@ void AP_OAPathPlanner::avoidance_thread() _oadatabase.update(); + // values returned by path planners Location origin_new; Location destination_new; + Location next_destination_new; + bool dest_to_next_dest_clear = false; { WITH_SEMAPHORE(_rsem); if (now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) { @@ -282,9 +295,10 @@ void AP_OAPathPlanner::avoidance_thread() // copy request to avoid conflict with main thread avoidance_request2 = avoidance_request; - // store passed in origin and destination so we can return it if object avoidance is not required + // store passed in origin, destination and next_destination so we can return it if object avoidance is not required origin_new = avoidance_request.origin; destination_new = avoidance_request.destination; + next_destination_new = avoidance_request.next_destination; } // run background task looking for best alternative destination @@ -313,7 +327,13 @@ void AP_OAPathPlanner::avoidance_thread() continue; } _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); + const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, + avoidance_request2.destination, + avoidance_request2.next_destination, + origin_new, + destination_new, + next_destination_new, + dest_to_next_dest_clear); switch (dijkstra_state) { case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED: res = OA_NOT_REQUIRED; @@ -354,7 +374,13 @@ void AP_OAPathPlanner::avoidance_thread() } #if AP_FENCE_ENABLED _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); + const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, + avoidance_request2.destination, + avoidance_request2.next_destination, + origin_new, + destination_new, + next_destination_new, + dest_to_next_dest_clear); switch (dijkstra_state) { case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED: res = OA_NOT_REQUIRED; @@ -376,9 +402,18 @@ void AP_OAPathPlanner::avoidance_thread() { // give the main thread the avoidance result WITH_SEMAPHORE(_rsem); + + // place the destination and next destination used into the result (used by the caller to verify the result matches their request) avoidance_result.destination = avoidance_request2.destination; + avoidance_result.next_destination = avoidance_request2.next_destination; + avoidance_result.dest_to_next_dest_clear = dest_to_next_dest_clear; + + // fill the result structure with the intermediate path avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new; avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination; + avoidance_result.next_destination_new = (res == OA_SUCCESS) ? next_destination_new : avoidance_result.next_destination; + + // create new avoidance result.dest_to_next_dest_clear field. fill in with results from dijkstras or leave as unknown avoidance_result.result_time_ms = AP_HAL::millis(); avoidance_result.path_planner_used = path_planner_used; avoidance_result.ret_state = res; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index 3e3e49d370..b6f57b16e6 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -48,13 +48,17 @@ public: }; // provides an alternative target location if path planning around obstacles is required - // returns true and updates result_origin and result_destination with an intermediate path + // returns true and updates result_origin, result_destination and result_next_destination with an intermediate path + // result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras) // path_planner_used updated with which path planner produced the result OA_RetState mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, + const Location &next_destination, Location &result_origin, Location &result_destination, + Location &result_next_destination, + bool &result_dest_to_next_dest_clear, OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED; // enumerations for _TYPE parameter @@ -90,6 +94,7 @@ private: Location current_loc; Location origin; Location destination; + Location next_destination; Vector2f ground_speed_vec; uint32_t request_time_ms; } avoidance_request, avoidance_request2; @@ -97,8 +102,11 @@ private: // an avoidance result from the avoidance thread struct { Location destination; // destination vehicle is trying to get to (also used to verify the result matches a recent request) + Location next_destination; // next destination vehicle is trying to get to (also used to verify the result matches a recent request) Location origin_new; // intermediate origin. The start of line segment that vehicle should follow Location destination_new; // intermediate destination vehicle should move towards + Location next_destination_new; // intermediate next destination vehicle should move towards + bool dest_to_next_dest_clear; // true if the path from destination_new to next_destination_new is clear and does not require path planning (only supported by Dijkstras) uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent) OAPathPlannerUsed path_planner_used; // path planner that produced the result OA_RetState ret_state; // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new