Include planned home position in MissionImportData

examples/move_mission_to_vehicle/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(fly_qgc_mission)
+
+add_executable(move_mission_to_vehicle
+        main.cpp
+        )
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(move_mission_to_vehicle
+        MAVSDK::mavsdk
+        )
+
+if(NOT MSVC)
+    add_compile_options(fly_qgc_mission PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(fly_qgc_mission PRIVATE -WX -W2)
+endif()

examples/move_mission_to_vehicle/main.cpp

@@ -0,0 +1,156 @@
+/**
+ * brief Demonstrates how to move a mission loaded from a .plan file such that the planned home
+ * position if present or the first waypoint is colocated with the vehicle position.
+ *
+ * Steps to run this example:
+ * 1.(a) Create a Mission in QGroundControl and save them to a file (.plan), or
+ *   (b) Use a pre-created sample mission plan in
+ *       "src/plugins/mission_raw/test_plans/qgroundcontrol_sample.plan".
+ * 2. Run the example by passing the path of the QGC mission plan as argument.
+ *
+ * Example description:
+ * 1. Imports QGC mission items from .plan file.
+ * 2. Translates mission such that it starts at the vehicle location.
+ * 3. Uploads mission items to vehicle.
+ */
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+
+#include <chrono>
+#include <functional>
+#include <future>
+#include <iostream>
+#include <memory>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url> <mission_plan_path>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 3) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk;
+
+    const ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    std::cout << "Waiting to discover system...\n";
+    auto prom = std::promise<std::shared_ptr<System>>{};
+    auto fut = prom.get_future();
+
+    // We wait for new systems to be discovered, once we find one that has an
+    // autopilot, we decide to use it.
+    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
+      auto system = mavsdk.systems().back();
+
+      if (system->has_autopilot()) {
+          std::cout << "Discovered autopilot\n";
+
+          // Unsubscribe again as we only want to find one system.
+          mavsdk.subscribe_on_new_system(nullptr);
+          prom.set_value(system);
+      }
+    });
+
+    // We usually receive heartbeats at 1Hz, therefore we should find a
+    // system after around 3 seconds max, surely.
+    if (fut.wait_for(std::chrono::seconds(3)) == std::future_status::timeout) {
+        std::cerr << "No autopilot found, exiting.\n";
+        return 1;
+    }
+
+    // Get discovered system now.
+    auto system = fut.get();
+
+    auto action = Action{system};
+    auto mission_raw = MissionRaw{system};
+    auto telemetry = Telemetry{system};
+
+    while (!telemetry.health_all_ok()) {
+        std::cout << "Waiting for system to be ready\n";
+        sleep_for(seconds(1));
+    }
+
+    std::cout << "System ready\n";
+
+    std::cout << "Importing mission from mission plan: " << argv[2] << '\n';
+    std::pair<MissionRaw::Result, MissionRaw::MissionImportData> import_res =
+        mission_raw.import_qgroundcontrol_mission(argv[2]);
+    if (import_res.first != MissionRaw::Result::Success) {
+        std::cerr << "Failed to import mission items: " << import_res.first;
+        return 1;
+    }
+
+    if (import_res.second.mission_items.size() == 0) {
+        std::cerr << "No missions! Exiting...\n";
+        return 1;
+    }
+    std::cout << "Found " << import_res.second.mission_items.size()
+              << " mission items in the given QGC plan.\n";
+
+
+    const auto position = telemetry.position();
+
+    double offset_x, offset_y = 0.0;
+
+    if (import_res.second.has_planned_home) {
+        std::cout << "Found planned home position, moving mission to vehicle position.\n";
+        offset_x = import_res.second.planned_home_position.x -
+                        static_cast<int32_t>(1e7 * position.latitude_deg);
+        offset_y = import_res.second.planned_home_position.y -
+                        static_cast<int32_t>(1e7 * position.longitude_deg);
+
+    } else {
+        std::cout << "No planned home position, move mission to first waypoint.\n";
+
+        for (auto &item : import_res.second.mission_items) {
+            if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
+                offset_x = item.x - static_cast<int32_t>(1e7 * position.latitude_deg);
+                offset_y = item.y - static_cast<int32_t>(1e7 * position.longitude_deg);
+                break;
+            }
+        }
+    }
+
+    for (auto &item : import_res.second.mission_items) {
+        if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
+            item.x -= offset_x;
+            item.y -= offset_y;
+        }
+
+    }
+
+    std::cout << "Uploading mission...";
+    const MissionRaw::Result upload_result =
+        mission_raw.upload_mission(import_res.second.mission_items);
+    if (upload_result != MissionRaw::Result::Success) {
+        std::cerr << "Failed uploading mission: " << upload_result << '\n';
+        return 1;
+    }
+    std::cout << "Mission uploaded.\n";
+
+
+    return 0;
+}
+

src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h

@@ -124,6 +124,9 @@ class MissionRaw : public PluginBase {
         std::vector<MissionItem> mission_items{}; /**< @brief Mission items */
         std::vector<MissionItem> geofence_items{}; /**< @brief Geofence items */
         std::vector<MissionItem> rally_items{}; /**< @brief Rally items */
+        bool has_planned_home{}; /**< @brief Whether it contains a planned home position element or
+                                    not */
+        MissionItem planned_home_position{}; /**< @brief Planned home position */
     };
 
     /**

src/mavsdk/plugins/mission_raw/mission_import.cpp

@@ -24,12 +24,16 @@ MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopil
     }
 
     auto maybe_mission_items = import_mission(root, autopilot);
-    if (!maybe_mission_items.has_value()) {
+    if (!maybe_mission_items.first.has_value()) {
         return {MissionRaw::Result::FailedToParseQgcPlan, {}};
     }
 
     MissionRaw::MissionImportData import_data;
-    import_data.mission_items = maybe_mission_items.value();
+    import_data.mission_items = maybe_mission_items.first.value();
+    import_data.planned_home_position = maybe_mission_items.second.value();
+    import_data.has_planned_home = maybe_mission_items.second.has_value();
+
+
     return {MissionRaw::Result::Success, import_data};
 }
 
@@ -46,14 +50,16 @@ bool MissionImport::check_overall_version(const Json::Value& root)
     return true;
 }
 
-std::optional<std::vector<MissionRaw::MissionItem>>
+std::pair<
+    std::optional<std::vector<MissionRaw::MissionItem>>,
+    std::optional<MissionRaw::MissionItem>>
 MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopilot)
 {
     // We need a mission part.
     const auto mission = root["mission"];
     if (mission.empty()) {
         LogErr() << "No mission found in .plan.";
-        return std::nullopt;
+        return std::make_pair(std::nullopt, std::nullopt);
     }
 
     // Check the mission version.
@@ -62,11 +68,36 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil
     if (mission_version.empty() || mission_version.asInt() != supported_mission_version) {
         LogErr() << "mission version for .plan not supported, found version: "
                  << mission_version.asInt() << ", supported: " << supported_mission_version;
-        return std::nullopt;
+        return std::make_pair(std::nullopt, std::nullopt);
     }
 
     std::vector<MissionRaw::MissionItem> mission_items;
 
+    const auto home = mission["plannedHomePosition"];
+    std::optional<MissionRaw::MissionItem> home_item;
+
+    if (!home.empty()) {
+        if (home.isArray() && home.size() != 3) {
+            LogErr() << "Unknown plannedHomePosition format";
+            return std::make_pair(std::nullopt, std::nullopt);
+        }
+
+        home_item = MissionRaw::MissionItem{
+            0,
+            MAV_FRAME_GLOBAL_INT,
+            MAV_CMD_NAV_WAYPOINT,
+            0, // current
+            1, // autocontinue
+            0.0f,
+            0.0f,
+            0.0f,
+            0.0f,
+            static_cast<int32_t>(std::round(home[0].asDouble() * 1e7)),
+            static_cast<int32_t>(std::round(home[1].asDouble() * 1e7)),
+            home[2].asFloat(),
+            MAV_MISSION_TYPE_MISSION};
+    }
+
     // Go through items
     for (const auto& json_item : mission["items"]) {
         const auto type = json_item["type"];
@@ -76,7 +107,7 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil
             if (maybe_item.has_value()) {
                 mission_items.push_back(maybe_item.value());
             } else {
-                return std::nullopt;
+                return std::make_pair(std::nullopt, home_item);
             }
 
         } else if (!type.isNull() && type.asString() == "ComplexItem") {
@@ -85,12 +116,12 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil
                 mission_items.insert(
                     mission_items.end(), maybe_items.value().begin(), maybe_items.value().end());
             } else {
-                return std::nullopt;
+                return std::make_pair(std::nullopt, home_item);
             }
 
         } else {
             LogErr() << "Type " << type.asString() << " not understood.";
-            return std::nullopt;
+            return std::make_pair(std::nullopt, home_item);
         }
     }
 
@@ -106,35 +137,12 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil
     }
 
     // Add home position at 0 for ArduPilot
-    if (autopilot == Sender::Autopilot::ArduPilot) {
-        const auto home = mission["plannedHomePosition"];
-        if (!home.empty()) {
-            if (home.isArray() && home.size() != 3) {
-                LogErr() << "Unknown plannedHomePosition format";
-                return std::nullopt;
-            }
-
-            mission_items.insert(
-                mission_items.begin(),
-                MissionRaw::MissionItem{
-                    0,
-                    MAV_FRAME_GLOBAL_INT,
-                    MAV_CMD_NAV_WAYPOINT,
-                    0, // current
-                    1, // autocontinue
-                    0.0f,
-                    0.0f,
-                    0.0f,
-                    0.0f,
-                    static_cast<int32_t>(std::round(home[0].asDouble() * 1e7)),
-                    static_cast<int32_t>(std::round(home[1].asDouble() * 1e7)),
-                    home[2].asFloat(),
-                    MAV_MISSION_TYPE_MISSION});
-        }
+    if (autopilot == Sender::Autopilot::ArduPilot && home_item.has_value()) {
+        mission_items.insert(mission_items.begin(), home_item.value());
     }
 
     // Returning an empty vector is ok here if there were really no mission items.
-    return {mission_items};
+    return std::make_pair(mission_items, home_item);
 }
 
 std::optional<MissionRaw::MissionItem>

src/mavsdk/plugins/mission_raw/mission_import.h

@@ -16,7 +16,9 @@ class MissionImport {
 
 private:
     static bool check_overall_version(const Json::Value& root);
-    static std::optional<std::vector<MissionRaw::MissionItem>>
+    static std::pair<
+        std::optional<std::vector<MissionRaw::MissionItem>>,
+        std::optional<MissionRaw::MissionItem>>
     import_mission(const Json::Value& root, SystemImpl::Autopilot autopilot);
     static std::optional<MissionRaw::MissionItem>
     import_simple_mission_item(const Json::Value& json_item);

src/mavsdk/plugins/mission_raw/mission_raw.cpp

@@ -170,7 +170,8 @@ std::ostream& operator<<(std::ostream& str, MissionRaw::MissionItem const& missi
 bool operator==(const MissionRaw::MissionImportData& lhs, const MissionRaw::MissionImportData& rhs)
 {
     return (rhs.mission_items == lhs.mission_items) && (rhs.geofence_items == lhs.geofence_items) &&
-           (rhs.rally_items == lhs.rally_items);
+           (rhs.rally_items == lhs.rally_items) && (rhs.has_planned_home == lhs.has_planned_home) &&
+           (rhs.planned_home_position == lhs.planned_home_position);
 }
 
 std::ostream&
@@ -199,6 +200,8 @@ operator<<(std::ostream& str, MissionRaw::MissionImportData const& mission_impor
         str << *it;
         str << (it + 1 != mission_import_data.rally_items.end() ? ", " : "]\n");
     }
+    str << "    has_planned_home: " << mission_import_data.has_planned_home << '\n';
+    str << "    planned_home_position: " << mission_import_data.planned_home_position << '\n';
     str << '}';
     return str;
 }