AP_OAPathPlanner: support get path length requests

libraries/AC_Avoidance/AP_OAPathPlanner.cpp

@@ -253,6 +253,46 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
     return OA_PROCESSING;
 }
 
+// get path length to arbitrary Location
+AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::get_path_length(const Location &origin, const Location &destination, float &path_length)
+{
+    WITH_SEMAPHORE(path_length_sem);
+
+    // get current system time
+    const uint32_t now_ms = AP_HAL::millis();
+
+    // copy path length request to buffer for avoidance thread
+    path_length_request = {
+        origin : origin,
+        destination : destination,
+        request_time_ms : now_ms
+    };
+
+    // record the first time we made a path length request
+    if (path_length_first_ms == 0) {
+        path_length_first_ms = now_ms;
+    }
+
+    // check results buffer for replies that match our request and has not timed out
+    const bool destination_matches = destination.same_latlon_as(path_length_reply.destination);
+    const bool timed_out = (now_ms - path_length_reply.result_time_ms > OA_TIMEOUT_MS) &&
+                           (now_ms - path_length_first_ms > OA_TIMEOUT_MS);
+
+    // return results from background thread's latest calculations
+    if (destination_matches && !timed_out) {
+        path_length = path_length_reply.path_length_m;
+        return path_length_reply.ret_state;
+    }
+
+    // if timeout then path planner is taking too long to respond
+    if (timed_out) {
+        return OA_ERROR;
+    }
+
+    // background thread is still calculating the path length
+    return OA_PROCESSING;
+}
+
 // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
 void AP_OAPathPlanner::avoidance_thread()
 {
@@ -288,6 +328,9 @@ void AP_OAPathPlanner::avoidance_thread()
 
         // handle avoidance requests
         handle_avoidance_requests();
+
+        // handle path length requests
+        handle_path_length_requests();
     }
 }
 
@@ -436,6 +479,31 @@ void AP_OAPathPlanner::handle_avoidance_requests()
     }
 }
 
+// handle path length requests in the avoidance thread
+void AP_OAPathPlanner::handle_path_length_requests()
+{
+    // return immediately if dijkstras is not enabled
+    if (_oadijkstra == nullptr) {
+        return;
+    }
+
+    // check if request is recent
+    if (AP_HAL::millis() - path_length_request.request_time_ms > OA_TIMEOUT_MS) {
+        return;
+    }
+
+    // ask dijkstras to calculate the path and path length
+    if (_oadijkstra->get_path_length(path_length_request.origin, path_length_request.destination, path_length_reply.path_length_m)) {
+        path_length_reply.ret_state = OA_SUCCESS;
+    } else {
+        path_length_reply.ret_state = OA_ERROR;
+    }
+    path_length_reply.destination = path_length_request.destination;
+    path_length_reply.result_time_ms = AP_HAL::millis();
+
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "handle_path_length_requests: %.1fm", (double)path_length_reply.path_length_m);
+}
+
 // singleton instance
 AP_OAPathPlanner *AP_OAPathPlanner::_singleton;
 

libraries/AC_Avoidance/AP_OAPathPlanner.h

@@ -83,6 +83,9 @@ class AP_OAPathPlanner {
 
     uint16_t get_options() const { return _options;}
 
+    // get path length to arbitrary Location
+    OA_RetState get_path_length(const Location &origin, const Location &destination, float &path_length) WARN_IF_UNUSED;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -94,6 +97,9 @@ class AP_OAPathPlanner {
     // handle avoidance requests in the avoidance thread
     void handle_avoidance_requests();
 
+    // handle path length requests in the avoidance thread
+    void handle_path_length_requests();
+
     // helper function to map OABendyType to OAPathPlannerUsed
     OAPathPlannerUsed map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type);
 
@@ -120,6 +126,22 @@ class AP_OAPathPlanner {
         OA_RetState ret_state;      // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new
     } avoidance_result;
 
+    // request and reply for path length (using Dijkstras)
+    struct PathLengthRequest {
+        Location origin;
+        Location destination;
+        uint32_t request_time_ms;
+    } path_length_request, path_length_request2;
+    HAL_Semaphore path_length_sem;  // semaphore for multi-thread use of path_length_request
+    uint32_t path_length_first_ms;  // system time that the first path length request was made.  Used to avoid timeout error on first run
+
+    struct PathLengthReply {
+        Location destination;       // destination (used to compare against request)
+        OA_RetState ret_state;      // OA_SUCCESS if the path length could be calculated
+        float path_length_m;        // path length in meters
+        uint32_t result_time_ms;    // system time the result was calculated (used to verify the result is recent)
+    } path_length_reply = {ret_state : OA_PROCESSING};
+
     // parameters
     AP_Int8 _type;                  // avoidance algorithm to be used
     AP_Float _margin_max;           // object avoidance will ignore objects more than this many meters from vehicle