Merge pull request #2804 from PavelGuzenfeld/fix/2403-download-geofence

Implement download_geofence mirroring download_mission

Author: julianoes

c/src/cmavsdk/plugins/geofence/geofence.cpp

@@ -378,6 +378,46 @@ mavsdk_geofence_upload_geofence(
     return translate_result(ret_value);
 }
 
+// DownloadGeofence async
+void mavsdk_geofence_download_geofence_async(
+    mavsdk_geofence_t geofence,
+    mavsdk_geofence_download_geofence_callback_t callback,
+    void* user_data)
+{
+    auto wrapper = reinterpret_cast<mavsdk_geofence_wrapper*>(geofence);
+
+    wrapper->cpp_plugin->download_geofence_async(
+        [callback, user_data](
+            mavsdk::Geofence::Result result,
+            mavsdk::Geofence::GeofenceData value) {
+                if (callback) {
+                    callback(
+                        translate_result(result),
+                        translate_geofence_data_to_c(value),
+                        user_data);
+                }
+        });
+}
+
+
+// DownloadGeofence sync
+mavsdk_geofence_result_t
+mavsdk_geofence_download_geofence(
+    mavsdk_geofence_t geofence,
+    mavsdk_geofence_geofence_data_t* geofence_data_out)
+{
+    auto wrapper = reinterpret_cast<mavsdk_geofence_wrapper*>(geofence);
+
+    auto result_pair = wrapper->cpp_plugin->download_geofence(
+);
+
+    if (geofence_data_out != nullptr) {
+        *geofence_data_out = translate_geofence_data_to_c(result_pair.second);
+    }
+
+    return translate_result(result_pair.first);
+}
+
 // ClearGeofence async
 void mavsdk_geofence_clear_geofence_async(
     mavsdk_geofence_t geofence,

c/src/cmavsdk/plugins/geofence/geofence.h

@@ -300,6 +300,7 @@ CMAVSDK_EXPORT void mavsdk_geofence_byte_buffer_destroy(uint8_t** buffer);
 
 // ===== Callback Typedefs =====
 typedef void (*mavsdk_geofence_upload_geofence_callback_t)(const mavsdk_geofence_result_t result, void* user_data);
+typedef void (*mavsdk_geofence_download_geofence_callback_t)(const mavsdk_geofence_result_t result, const mavsdk_geofence_geofence_data_t geofence_data, void* user_data);
 typedef void (*mavsdk_geofence_clear_geofence_callback_t)(const mavsdk_geofence_result_t result, void* user_data);
 
 // ===== Geofence Creation/Destruction =====
@@ -343,6 +344,37 @@ mavsdk_geofence_upload_geofence(
     mavsdk_geofence_geofence_data_t geofence_data);
 
 
+/**
+ * @brief Download geofences from the vehicle.
+ * 
+ *  Downloads polygon and circular geofences from the vehicle.
+ *
+ * @param geofence The geofence instance.
+ *
+ * @param callback Function to call when new data is available.
+ * @param user_data User data to pass to the callback.
+ */
+CMAVSDK_EXPORT void mavsdk_geofence_download_geofence_async(
+    mavsdk_geofence_t geofence,
+    mavsdk_geofence_download_geofence_callback_t callback,
+    void* user_data);
+
+
+/**
+ * @brief Get the current download geofence (blocking).
+ *
+ * This function blocks until a value is available.
+ *
+ * @param telemetry The telemetry instance.
+ * @param download_geofence_out Pointer to store the result.
+ */
+CMAVSDK_EXPORT
+mavsdk_geofence_result_t
+mavsdk_geofence_download_geofence(
+    mavsdk_geofence_t geofence,
+    mavsdk_geofence_geofence_data_t* geofence_data_out);
+
+
 /**
  * @brief Clear all geofences saved on the vehicle.
  *

cpp/docs/en/cpp/api_reference/classmavsdk_1_1_geofence.md

@@ -26,6 +26,7 @@ Type | Description
 enum [FenceType](#classmavsdk_1_1_geofence_1a51196ded958aa9d4b3f25cde95da48e5) | [Geofence](classmavsdk_1_1_geofence.md) types.
 enum [Result](#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642) | Possible results returned for geofence requests.
 std::function< void([Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642))> [ResultCallback](#classmavsdk_1_1_geofence_1af9662e645781e4e64ed8b7c65d3d9309) | Callback type for asynchronous [Geofence](classmavsdk_1_1_geofence.md) calls.
+std::function< void([Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642), [GeofenceData](structmavsdk_1_1_geofence_1_1_geofence_data.md))> [DownloadGeofenceCallback](#classmavsdk_1_1_geofence_1a1fcce00cf17acb7e6f9cc3120f5b1e93) | Callback type for download_geofence_async.
 
 ## Public Member Functions
 
@@ -38,6 +39,8 @@ Type | Name | Description
 &nbsp; | [Geofence](#classmavsdk_1_1_geofence_1a60e1f1a3123050c73980cba61b4b4009) (const [Geofence](classmavsdk_1_1_geofence.md) & other) | Copy constructor.
 void | [upload_geofence_async](#classmavsdk_1_1_geofence_1a1f0c6431b86b77c29f19eafb4e55dcdb) ([GeofenceData](structmavsdk_1_1_geofence_1_1_geofence_data.md) geofence_data, const [ResultCallback](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1af9662e645781e4e64ed8b7c65d3d9309) callback) | Upload geofences.
 [Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642) | [upload_geofence](#classmavsdk_1_1_geofence_1ab2e825f0955e7a320117a21d649bab09) ([GeofenceData](structmavsdk_1_1_geofence_1_1_geofence_data.md) geofence_data)const | Upload geofences.
+void | [download_geofence_async](#classmavsdk_1_1_geofence_1aedb57c09762303d17bf911cb46bc3403) (const [DownloadGeofenceCallback](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1a1fcce00cf17acb7e6f9cc3120f5b1e93) callback) | Download geofences from the vehicle.
+std::pair< [Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642), [Geofence::GeofenceData](structmavsdk_1_1_geofence_1_1_geofence_data.md) > | [download_geofence](#classmavsdk_1_1_geofence_1a6470bac1295b64eed9fc03c380bfbe24) () const | Download geofences from the vehicle.
 void | [clear_geofence_async](#classmavsdk_1_1_geofence_1a6947151765b621a93d35885599812752) (const [ResultCallback](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1af9662e645781e4e64ed8b7c65d3d9309) callback) | Clear all geofences saved on the vehicle.
 [Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642) | [clear_geofence](#classmavsdk_1_1_geofence_1a54b2a696e8aebae6916116adb92c03c3) () const | Clear all geofences saved on the vehicle.
 const [Geofence](classmavsdk_1_1_geofence.md) & | [operator=](#classmavsdk_1_1_geofence_1a174d03979e425bf8440dfe9bbabaf7d2) (const [Geofence](classmavsdk_1_1_geofence.md) &)=delete | Equality operator (object is not copyable).
@@ -117,6 +120,16 @@ using mavsdk::Geofence::ResultCallback =  std::function<void(Result)>
 Callback type for asynchronous [Geofence](classmavsdk_1_1_geofence.md) calls.
 
 
+### typedef DownloadGeofenceCallback {#classmavsdk_1_1_geofence_1a1fcce00cf17acb7e6f9cc3120f5b1e93}
+
+```cpp
+using mavsdk::Geofence::DownloadGeofenceCallback =  std::function<void(Result, GeofenceData)>
+```
+
+
+Callback type for download_geofence_async.
+
+
 ## Member Enumeration Documentation
 
 
@@ -190,6 +203,40 @@ This function is blocking. See 'upload_geofence_async' for the non-blocking coun
 
 &emsp;[Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642) - Result of request.
 
+### download_geofence_async() {#classmavsdk_1_1_geofence_1aedb57c09762303d17bf911cb46bc3403}
+```cpp
+void mavsdk::Geofence::download_geofence_async(const DownloadGeofenceCallback callback)
+```
+
+
+Download geofences from the vehicle.
+
+Downloads polygon and circular geofences from the vehicle.
+
+
+This function is non-blocking. See 'download_geofence' for the blocking counterpart.
+
+**Parameters**
+
+* const [DownloadGeofenceCallback](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1a1fcce00cf17acb7e6f9cc3120f5b1e93) **callback** - 
+
+### download_geofence() {#classmavsdk_1_1_geofence_1a6470bac1295b64eed9fc03c380bfbe24}
+```cpp
+std::pair< Result, Geofence::GeofenceData > mavsdk::Geofence::download_geofence() const
+```
+
+
+Download geofences from the vehicle.
+
+Downloads polygon and circular geofences from the vehicle.
+
+
+This function is blocking. See 'download_geofence_async' for the non-blocking counterpart.
+
+**Returns**
+
+&emsp;std::pair< [Result](classmavsdk_1_1_geofence.md#classmavsdk_1_1_geofence_1ab64d6e3b9aeb9b6d5e45ae8a843a2642), [Geofence::GeofenceData](structmavsdk_1_1_geofence_1_1_geofence_data.md) > - Result of request.
+
 ### clear_geofence_async() {#classmavsdk_1_1_geofence_1a6947151765b621a93d35885599812752}
 ```cpp
 void mavsdk::Geofence::clear_geofence_async(const ResultCallback callback)

cpp/src/mavsdk/plugins/geofence/geofence.cpp

@@ -33,6 +33,16 @@ Geofence::Result Geofence::upload_geofence(GeofenceData geofence_data) const
     return _impl->upload_geofence(geofence_data);
 }
 
+void Geofence::download_geofence_async(const DownloadGeofenceCallback callback)
+{
+    _impl->download_geofence_async(callback);
+}
+
+std::pair<Geofence::Result, Geofence::GeofenceData> Geofence::download_geofence() const
+{
+    return _impl->download_geofence();
+}
+
 void Geofence::clear_geofence_async(const ResultCallback callback)
 {
     _impl->clear_geofence_async(callback);

cpp/src/mavsdk/plugins/geofence/geofence_impl.cpp

@@ -56,6 +56,101 @@ void GeofenceImpl::upload_geofence_async(
         });
 }
 
+std::pair<Geofence::Result, Geofence::GeofenceData> GeofenceImpl::download_geofence()
+{
+    auto prom = std::promise<std::pair<Geofence::Result, Geofence::GeofenceData>>();
+    auto fut = prom.get_future();
+
+    download_geofence_async([&prom](Geofence::Result result, Geofence::GeofenceData geofence_data) {
+        prom.set_value(std::make_pair(result, geofence_data));
+    });
+    return fut.get();
+}
+
+void GeofenceImpl::download_geofence_async(const Geofence::DownloadGeofenceCallback& callback)
+{
+    if (_last_download.lock()) {
+        _system_impl->call_user_callback([callback]() {
+            if (callback) {
+                callback(Geofence::Result::Busy, Geofence::GeofenceData{});
+            }
+        });
+        return;
+    }
+
+    _last_download = _system_impl->mission_transfer_client().download_items_async(
+        MAV_MISSION_TYPE_FENCE,
+        _system_impl->get_system_id(),
+        [this, callback](
+            MavlinkMissionTransferClient::Result result,
+            std::vector<MavlinkMissionTransferClient::ItemInt> items) {
+            auto result_pair = (result == MavlinkMissionTransferClient::Result::Success) ?
+                                   disassemble_items(items) :
+                                   std::make_pair(convert_result(result), Geofence::GeofenceData{});
+            _system_impl->call_user_callback([callback, result_pair]() {
+                if (callback) {
+                    callback(result_pair.first, result_pair.second);
+                }
+            });
+        });
+}
+
+std::pair<Geofence::Result, Geofence::GeofenceData>
+GeofenceImpl::disassemble_items(const std::vector<MavlinkMissionTransferClient::ItemInt>& items)
+{
+    Geofence::GeofenceData geofence_data;
+
+    size_t i = 0;
+    while (i < items.size()) {
+        const auto& item = items[i];
+
+        if (item.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
+            item.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
+            Geofence::Polygon polygon;
+            polygon.fence_type = (item.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) ?
+                                     Geofence::FenceType::Inclusion :
+                                     Geofence::FenceType::Exclusion;
+
+            const auto vertex_count = static_cast<size_t>(item.param1);
+            if (vertex_count == 0 || i + vertex_count > items.size()) {
+                LogErr() << "Invalid polygon vertex count: " << vertex_count;
+                return {Geofence::Result::InvalidArgument, {}};
+            }
+
+            for (size_t j = 0; j < vertex_count; ++j) {
+                const auto& vertex_item = items[i + j];
+                Geofence::Point point;
+                point.latitude_deg = vertex_item.x * 1e-7;
+                point.longitude_deg = vertex_item.y * 1e-7;
+                polygon.points.push_back(point);
+            }
+
+            geofence_data.polygons.push_back(polygon);
+            i += vertex_count;
+
+        } else if (
+            item.command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION ||
+            item.command == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
+            Geofence::Circle circle;
+            circle.fence_type = (item.command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) ?
+                                    Geofence::FenceType::Inclusion :
+                                    Geofence::FenceType::Exclusion;
+            circle.point.latitude_deg = item.x * 1e-7;
+            circle.point.longitude_deg = item.y * 1e-7;
+            circle.radius = item.param1;
+
+            geofence_data.circles.push_back(circle);
+            ++i;
+
+        } else {
+            LogWarn() << "Unknown geofence item command: " << item.command;
+            ++i;
+        }
+    }
+
+    return {Geofence::Result::Success, geofence_data};
+}
+
 Geofence::Result GeofenceImpl::clear_geofence()
 {
     auto prom = std::promise<Geofence::Result>();

cpp/src/mavsdk/plugins/geofence/geofence_impl.h

@@ -28,6 +28,10 @@ class GeofenceImpl : public PluginImplBase {
     void upload_geofence_async(
         const Geofence::GeofenceData& geofence_data, const Geofence::ResultCallback& callback);
 
+    std::pair<Geofence::Result, Geofence::GeofenceData> download_geofence();
+
+    void download_geofence_async(const Geofence::DownloadGeofenceCallback& callback);
+
     Geofence::Result clear_geofence();
 
     void clear_geofence_async(const Geofence::ResultCallback& callback);
@@ -40,7 +44,12 @@ class GeofenceImpl : public PluginImplBase {
     std::vector<MavlinkMissionTransferClient::ItemInt>
     assemble_items(const Geofence::GeofenceData& geofence_data);
 
+    static std::pair<Geofence::Result, Geofence::GeofenceData>
+    disassemble_items(const std::vector<MavlinkMissionTransferClient::ItemInt>& items);
+
     static Geofence::Result convert_result(MavlinkMissionTransferClient::Result result);
+
+    std::weak_ptr<MavlinkMissionTransferClient::WorkItem> _last_download{};
 };
 
 } // namespace mavsdk

cpp/src/mavsdk/plugins/geofence/include/plugins/geofence/geofence.h

@@ -213,6 +213,31 @@ class Geofence : public PluginBase {
      */
     Result upload_geofence(GeofenceData geofence_data) const;
 
+    /**
+     * @brief Callback type for download_geofence_async.
+     */
+    using DownloadGeofenceCallback = std::function<void(Result, GeofenceData)>;
+
+    /**
+     * @brief Download geofences from the vehicle.
+     *
+     * Downloads polygon and circular geofences from the vehicle.
+     *
+     * This function is non-blocking. See 'download_geofence' for the blocking counterpart.
+     */
+    void download_geofence_async(const DownloadGeofenceCallback callback);
+
+    /**
+     * @brief Download geofences from the vehicle.
+     *
+     * Downloads polygon and circular geofences from the vehicle.
+     *
+     * This function is blocking. See 'download_geofence_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    std::pair<Result, Geofence::GeofenceData> download_geofence() const;
+
     /**
      * @brief Clear all geofences saved on the vehicle.
      *