From 9b1778c6857ad632e1a99cb5a23c2a125ef4f7dc Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 11 Aug 2023 13:54:39 +0300 Subject: [PATCH 1/6] when importing a mission from a plan file, read the planned home position such that the mission can be properly moved to the vehicle location --- .../include/plugins/mission_raw/mission_raw.h | 1 + .../plugins/mission_raw/mission_import.cpp | 70 ++++++++++--------- .../plugins/mission_raw/mission_import.h | 2 +- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h index 7816f5656f..ce2c9ad8c5 100644 --- a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h +++ b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h @@ -124,6 +124,7 @@ class MissionRaw : public PluginBase { std::vector mission_items{}; /**< @brief Mission items */ std::vector geofence_items{}; /**< @brief Geofence items */ std::vector rally_items{}; /**< @brief Rally items */ + std::optional plannedHomePosition; }; /** diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index 17bff7240b..08a82a6990 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -24,12 +24,14 @@ 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.plannedHomePosition = maybe_mission_items.second.value(); + return {MissionRaw::Result::Success, import_data}; } @@ -46,14 +48,14 @@ bool MissionImport::check_overall_version(const Json::Value& root) return true; } -std::optional> +std::pair>, std::optional> 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 +64,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 mission_items; + const auto home = mission["plannedHomePosition"]; + std::optional 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(std::round(home[0].asDouble() * 1e7)), + static_cast(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 +103,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 +112,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 +133,14 @@ 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; - } - + if (autopilot == Sender::Autopilot::ArduPilot && home_item.has_value()) { 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(std::round(home[0].asDouble() * 1e7)), - static_cast(std::round(home[1].asDouble() * 1e7)), - home[2].asFloat(), - MAV_MISSION_TYPE_MISSION}); - } + 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 diff --git a/src/mavsdk/plugins/mission_raw/mission_import.h b/src/mavsdk/plugins/mission_raw/mission_import.h index 406428de81..35f0b52b87 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.h +++ b/src/mavsdk/plugins/mission_raw/mission_import.h @@ -16,7 +16,7 @@ class MissionImport { private: static bool check_overall_version(const Json::Value& root); - static std::optional> + static std::pair>, std::optional> import_mission(const Json::Value& root, SystemImpl::Autopilot autopilot); static std::optional import_simple_mission_item(const Json::Value& json_item); From b41a05bba6eb1081d377daab27d77637d4dc0ff6 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Tue, 15 Aug 2023 12:06:32 +0200 Subject: [PATCH 2/6] Generate from proto --- proto | 2 +- .../include/plugins/mission_raw/mission_raw.h | 4 +- .../plugins/mission_raw/mission_import.cpp | 10 +- .../plugins/mission_raw/mission_import.h | 4 +- .../plugins/mission_raw/mission_raw.cpp | 7 +- .../generated/mission_raw/mission_raw.pb.cc | 186 +++++++++++++----- .../generated/mission_raw/mission_raw.pb.h | 141 +++++++++++++ .../mission_raw/mission_raw_service_impl.h | 10 + 8 files changed, 302 insertions(+), 62 deletions(-) diff --git a/proto b/proto index 17d707685a..e25d8f07b5 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 17d707685a8cbe5e0b6c930850b5a2c75b85bbd8 +Subproject commit e25d8f07b5108d946d984a3e420ed2a6294a5842 diff --git a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h index ce2c9ad8c5..dba9764e8a 100644 --- a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h +++ b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h @@ -124,7 +124,9 @@ class MissionRaw : public PluginBase { std::vector mission_items{}; /**< @brief Mission items */ std::vector geofence_items{}; /**< @brief Geofence items */ std::vector rally_items{}; /**< @brief Rally items */ - std::optional plannedHomePosition; + bool has_planned_home_position{}; /**< @brief Whether it contains a planned home position + element or not */ + MissionItem planned_home_position{}; /**< @brief Planned home position */ }; /** diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index 08a82a6990..e3bf97c01c 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -30,7 +30,7 @@ MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopil MissionRaw::MissionImportData import_data; import_data.mission_items = maybe_mission_items.first.value(); - import_data.plannedHomePosition = maybe_mission_items.second.value(); + import_data.planned_home_position = maybe_mission_items.second.value(); return {MissionRaw::Result::Success, import_data}; } @@ -48,7 +48,9 @@ bool MissionImport::check_overall_version(const Json::Value& root) return true; } -std::pair>, std::optional> +std::pair< + std::optional>, + std::optional> MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopilot) { // We need a mission part. @@ -134,9 +136,7 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil // Add home position at 0 for ArduPilot if (autopilot == Sender::Autopilot::ArduPilot && home_item.has_value()) { - mission_items.insert( - mission_items.begin(), - home_item.value()); + mission_items.insert(mission_items.begin(), home_item.value()); } // Returning an empty vector is ok here if there were really no mission items. diff --git a/src/mavsdk/plugins/mission_raw/mission_import.h b/src/mavsdk/plugins/mission_raw/mission_import.h index 35f0b52b87..1a7eb1d052 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.h +++ b/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::pair>, std::optional> + static std::pair< + std::optional>, + std::optional> import_mission(const Json::Value& root, SystemImpl::Autopilot autopilot); static std::optional import_simple_mission_item(const Json::Value& json_item); diff --git a/src/mavsdk/plugins/mission_raw/mission_raw.cpp b/src/mavsdk/plugins/mission_raw/mission_raw.cpp index 0648f67e48..91a1037c03 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw.cpp @@ -170,7 +170,9 @@ 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_position == lhs.has_planned_home_position) && + (rhs.planned_home_position == lhs.planned_home_position); } std::ostream& @@ -199,6 +201,9 @@ 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_position: " << mission_import_data.has_planned_home_position + << '\n'; + str << " planned_home_position: " << mission_import_data.planned_home_position << '\n'; str << '}'; return str; } diff --git a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc index db807b9a85..a794fdb70d 100644 --- a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc +++ b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc @@ -318,7 +318,9 @@ constexpr MissionImportData::MissionImportData( ::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized) : mission_items_() , geofence_items_() - , rally_items_(){} + , rally_items_() + , planned_home_position_(nullptr) + , has_planned_home_position_(false){} struct MissionImportDataDefaultTypeInternal { constexpr MissionImportDataDefaultTypeInternal() : _instance(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized{}) {} @@ -509,6 +511,8 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_mission_5fraw_2fmission_5fraw_ PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, mission_items_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, geofence_items_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, rally_items_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, has_planned_home_position_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, planned_home_position_), ~0u, // no _has_bits_ PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionRawResult, _internal_metadata_), ~0u, // no _extensions_ @@ -543,7 +547,7 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB { 126, -1, sizeof(::mavsdk::rpc::mission_raw::MissionProgress)}, { 133, -1, sizeof(::mavsdk::rpc::mission_raw::MissionItem)}, { 151, -1, sizeof(::mavsdk::rpc::mission_raw::MissionImportData)}, - { 159, -1, sizeof(::mavsdk::rpc::mission_raw::MissionRawResult)}, + { 161, -1, sizeof(::mavsdk::rpc::mission_raw::MissionRawResult)}, }; static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = { @@ -624,67 +628,70 @@ const char descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto[] PRO "ocontinue\030\005 \001(\r\022\016\n\006param1\030\006 \001(\002\022\016\n\006param" "2\030\007 \001(\002\022\016\n\006param3\030\010 \001(\002\022\016\n\006param4\030\t \001(\002\022" "\t\n\001x\030\n \001(\005\022\t\n\001y\030\013 \001(\005\022\t\n\001z\030\014 \001(\002\022\024\n\014miss" - "ion_type\030\r \001(\r\"\306\001\n\021MissionImportData\022:\n\r" + "ion_type\030\r \001(\r\"\255\002\n\021MissionImportData\022:\n\r" "mission_items\030\001 \003(\0132#.mavsdk.rpc.mission" "_raw.MissionItem\022;\n\016geofence_items\030\002 \003(\013" "2#.mavsdk.rpc.mission_raw.MissionItem\0228\n" "\013rally_items\030\003 \003(\0132#.mavsdk.rpc.mission_" - "raw.MissionItem\"\310\003\n\020MissionRawResult\022\?\n\006" - "result\030\001 \001(\0162/.mavsdk.rpc.mission_raw.Mi" - "ssionRawResult.Result\022\022\n\nresult_str\030\002 \001(" - "\t\"\336\002\n\006Result\022\022\n\016RESULT_UNKNOWN\020\000\022\022\n\016RESU" - "LT_SUCCESS\020\001\022\020\n\014RESULT_ERROR\020\002\022!\n\035RESULT" - "_TOO_MANY_MISSION_ITEMS\020\003\022\017\n\013RESULT_BUSY" - "\020\004\022\022\n\016RESULT_TIMEOUT\020\005\022\033\n\027RESULT_INVALID" - "_ARGUMENT\020\006\022\026\n\022RESULT_UNSUPPORTED\020\007\022\037\n\033R" - "ESULT_NO_MISSION_AVAILABLE\020\010\022\035\n\031RESULT_T" - "RANSFER_CANCELLED\020\t\022\"\n\036RESULT_FAILED_TO_" - "OPEN_QGC_PLAN\020\n\022#\n\037RESULT_FAILED_TO_PARS" - "E_QGC_PLAN\020\013\022\024\n\020RESULT_NO_SYSTEM\020\0142\223\013\n\021M" - "issionRawService\022n\n\rUploadMission\022,.mavs" - "dk.rpc.mission_raw.UploadMissionRequest\032" - "-.mavsdk.rpc.mission_raw.UploadMissionRe" - "sponse\"\000\022\204\001\n\023CancelMissionUpload\0222.mavsd" - "k.rpc.mission_raw.CancelMissionUploadReq" - "uest\0323.mavsdk.rpc.mission_raw.CancelMiss" - "ionUploadResponse\"\004\200\265\030\001\022t\n\017DownloadMissi" - "on\022..mavsdk.rpc.mission_raw.DownloadMiss" - "ionRequest\032/.mavsdk.rpc.mission_raw.Down" - "loadMissionResponse\"\000\022\212\001\n\025CancelMissionD" - "ownload\0224.mavsdk.rpc.mission_raw.CancelM" - "issionDownloadRequest\0325.mavsdk.rpc.missi" - "on_raw.CancelMissionDownloadResponse\"\004\200\265" - "\030\001\022k\n\014StartMission\022+.mavsdk.rpc.mission_" - "raw.StartMissionRequest\032,.mavsdk.rpc.mis" - "sion_raw.StartMissionResponse\"\000\022k\n\014Pause" - "Mission\022+.mavsdk.rpc.mission_raw.PauseMi" - "ssionRequest\032,.mavsdk.rpc.mission_raw.Pa" - "useMissionResponse\"\000\022k\n\014ClearMission\022+.m" - "avsdk.rpc.mission_raw.ClearMissionReques" - "t\032,.mavsdk.rpc.mission_raw.ClearMissionR" - "esponse\"\000\022\206\001\n\025SetCurrentMissionItem\0224.ma" - "vsdk.rpc.mission_raw.SetCurrentMissionIt" - "emRequest\0325.mavsdk.rpc.mission_raw.SetCu" - "rrentMissionItemResponse\"\000\022\210\001\n\030Subscribe" - "MissionProgress\0227.mavsdk.rpc.mission_raw" - ".SubscribeMissionProgressRequest\032/.mavsd" - "k.rpc.mission_raw.MissionProgressRespons" - "e\"\0000\001\022\211\001\n\027SubscribeMissionChanged\0226.mavs" - "dk.rpc.mission_raw.SubscribeMissionChang" - "edRequest\032..mavsdk.rpc.mission_raw.Missi" - "onChangedResponse\"\004\200\265\030\0000\001\022\234\001\n\033ImportQgro" - "undcontrolMission\022:.mavsdk.rpc.mission_r" - "aw.ImportQgroundcontrolMissionRequest\032;." - "mavsdk.rpc.mission_raw.ImportQgroundcont" - "rolMissionResponse\"\004\200\265\030\001B(\n\025io.mavsdk.mi" - "ssion_rawB\017MissionRawProtob\006proto3" + "raw.MissionItem\022!\n\031has_planned_home_posi" + "tion\030\004 \001(\010\022B\n\025planned_home_position\030\005 \001(" + "\0132#.mavsdk.rpc.mission_raw.MissionItem\"\310" + "\003\n\020MissionRawResult\022\?\n\006result\030\001 \001(\0162/.ma" + "vsdk.rpc.mission_raw.MissionRawResult.Re" + "sult\022\022\n\nresult_str\030\002 \001(\t\"\336\002\n\006Result\022\022\n\016R" + "ESULT_UNKNOWN\020\000\022\022\n\016RESULT_SUCCESS\020\001\022\020\n\014R" + "ESULT_ERROR\020\002\022!\n\035RESULT_TOO_MANY_MISSION" + "_ITEMS\020\003\022\017\n\013RESULT_BUSY\020\004\022\022\n\016RESULT_TIME" + "OUT\020\005\022\033\n\027RESULT_INVALID_ARGUMENT\020\006\022\026\n\022RE" + "SULT_UNSUPPORTED\020\007\022\037\n\033RESULT_NO_MISSION_" + "AVAILABLE\020\010\022\035\n\031RESULT_TRANSFER_CANCELLED" + "\020\t\022\"\n\036RESULT_FAILED_TO_OPEN_QGC_PLAN\020\n\022#" + "\n\037RESULT_FAILED_TO_PARSE_QGC_PLAN\020\013\022\024\n\020R" + "ESULT_NO_SYSTEM\020\0142\223\013\n\021MissionRawService\022" + "n\n\rUploadMission\022,.mavsdk.rpc.mission_ra" + "w.UploadMissionRequest\032-.mavsdk.rpc.miss" + "ion_raw.UploadMissionResponse\"\000\022\204\001\n\023Canc" + "elMissionUpload\0222.mavsdk.rpc.mission_raw" + ".CancelMissionUploadRequest\0323.mavsdk.rpc" + ".mission_raw.CancelMissionUploadResponse" + "\"\004\200\265\030\001\022t\n\017DownloadMission\022..mavsdk.rpc.m" + "ission_raw.DownloadMissionRequest\032/.mavs" + "dk.rpc.mission_raw.DownloadMissionRespon" + "se\"\000\022\212\001\n\025CancelMissionDownload\0224.mavsdk." + "rpc.mission_raw.CancelMissionDownloadReq" + "uest\0325.mavsdk.rpc.mission_raw.CancelMiss" + "ionDownloadResponse\"\004\200\265\030\001\022k\n\014StartMissio" + "n\022+.mavsdk.rpc.mission_raw.StartMissionR" + "equest\032,.mavsdk.rpc.mission_raw.StartMis" + "sionResponse\"\000\022k\n\014PauseMission\022+.mavsdk." + "rpc.mission_raw.PauseMissionRequest\032,.ma" + "vsdk.rpc.mission_raw.PauseMissionRespons" + "e\"\000\022k\n\014ClearMission\022+.mavsdk.rpc.mission" + "_raw.ClearMissionRequest\032,.mavsdk.rpc.mi" + "ssion_raw.ClearMissionResponse\"\000\022\206\001\n\025Set" + "CurrentMissionItem\0224.mavsdk.rpc.mission_" + "raw.SetCurrentMissionItemRequest\0325.mavsd" + "k.rpc.mission_raw.SetCurrentMissionItemR" + "esponse\"\000\022\210\001\n\030SubscribeMissionProgress\0227" + ".mavsdk.rpc.mission_raw.SubscribeMission" + "ProgressRequest\032/.mavsdk.rpc.mission_raw" + ".MissionProgressResponse\"\0000\001\022\211\001\n\027Subscri" + "beMissionChanged\0226.mavsdk.rpc.mission_ra" + "w.SubscribeMissionChangedRequest\032..mavsd" + "k.rpc.mission_raw.MissionChangedResponse" + "\"\004\200\265\030\0000\001\022\234\001\n\033ImportQgroundcontrolMission" + "\022:.mavsdk.rpc.mission_raw.ImportQgroundc" + "ontrolMissionRequest\032;.mavsdk.rpc.missio" + "n_raw.ImportQgroundcontrolMissionRespons" + "e\"\004\200\265\030\001B(\n\025io.mavsdk.mission_rawB\017Missio" + "nRawProtob\006proto3" ; static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_deps[1] = { &::descriptor_table_mavsdk_5foptions_2eproto, }; static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_once; const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_mission_5fraw_2fmission_5fraw_2eproto = { - false, false, 4074, descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto, "mission_raw/mission_raw.proto", + false, false, 4177, descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto, "mission_raw/mission_raw.proto", &descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_once, descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_deps, 1, 26, schemas, file_default_instances, TableStruct_mission_5fraw_2fmission_5fraw_2eproto::offsets, file_level_metadata_mission_5fraw_2fmission_5fraw_2eproto, file_level_enum_descriptors_mission_5fraw_2fmission_5fraw_2eproto, file_level_service_descriptors_mission_5fraw_2fmission_5fraw_2eproto, @@ -5491,8 +5498,13 @@ ::PROTOBUF_NAMESPACE_ID::Metadata MissionItem::GetMetadata() const { class MissionImportData::_Internal { public: + static const ::mavsdk::rpc::mission_raw::MissionItem& planned_home_position(const MissionImportData* msg); }; +const ::mavsdk::rpc::mission_raw::MissionItem& +MissionImportData::_Internal::planned_home_position(const MissionImportData* msg) { + return *msg->planned_home_position_; +} MissionImportData::MissionImportData(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned) : ::PROTOBUF_NAMESPACE_ID::Message(arena, is_message_owned), @@ -5511,10 +5523,20 @@ MissionImportData::MissionImportData(const MissionImportData& from) geofence_items_(from.geofence_items_), rally_items_(from.rally_items_) { _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_); + if (from._internal_has_planned_home_position()) { + planned_home_position_ = new ::mavsdk::rpc::mission_raw::MissionItem(*from.planned_home_position_); + } else { + planned_home_position_ = nullptr; + } + has_planned_home_position_ = from.has_planned_home_position_; // @@protoc_insertion_point(copy_constructor:mavsdk.rpc.mission_raw.MissionImportData) } inline void MissionImportData::SharedCtor() { +::memset(reinterpret_cast(this) + static_cast( + reinterpret_cast(&planned_home_position_) - reinterpret_cast(this)), + 0, static_cast(reinterpret_cast(&has_planned_home_position_) - + reinterpret_cast(&planned_home_position_)) + sizeof(has_planned_home_position_)); } MissionImportData::~MissionImportData() { @@ -5526,6 +5548,7 @@ MissionImportData::~MissionImportData() { inline void MissionImportData::SharedDtor() { GOOGLE_DCHECK(GetArenaForAllocation() == nullptr); + if (this != internal_default_instance()) delete planned_home_position_; } void MissionImportData::ArenaDtor(void* object) { @@ -5547,6 +5570,11 @@ void MissionImportData::Clear() { mission_items_.Clear(); geofence_items_.Clear(); rally_items_.Clear(); + if (GetArenaForAllocation() == nullptr && planned_home_position_ != nullptr) { + delete planned_home_position_; + } + planned_home_position_ = nullptr; + has_planned_home_position_ = false; _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(); } @@ -5592,6 +5620,20 @@ const char* MissionImportData::_InternalParse(const char* ptr, ::PROTOBUF_NAMESP } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr)); } else goto handle_unusual; continue; + // bool has_planned_home_position = 4; + case 4: + if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 32)) { + has_planned_home_position_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr); + CHK_(ptr); + } else goto handle_unusual; + continue; + // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; + case 5: + if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) { + ptr = ctx->ParseMessage(_internal_mutable_planned_home_position(), ptr); + CHK_(ptr); + } else goto handle_unusual; + continue; default: { handle_unusual: if ((tag == 0) || ((tag & 7) == 4)) { @@ -5645,6 +5687,20 @@ ::PROTOBUF_NAMESPACE_ID::uint8* MissionImportData::_InternalSerialize( InternalWriteMessage(3, this->_internal_rally_items(i), target, stream); } + // bool has_planned_home_position = 4; + if (this->_internal_has_planned_home_position() != 0) { + target = stream->EnsureSpace(target); + target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(4, this->_internal_has_planned_home_position(), target); + } + + // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; + if (this->_internal_has_planned_home_position()) { + target = stream->EnsureSpace(target); + target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite:: + InternalWriteMessage( + 5, _Internal::planned_home_position(this), target, stream); + } + if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) { target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray( _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream); @@ -5682,6 +5738,18 @@ size_t MissionImportData::ByteSizeLong() const { ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg); } + // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; + if (this->_internal_has_planned_home_position()) { + total_size += 1 + + ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize( + *planned_home_position_); + } + + // bool has_planned_home_position = 4; + if (this->_internal_has_planned_home_position() != 0) { + total_size += 1 + 1; + } + if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) { return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize( _internal_metadata_, total_size, &_cached_size_); @@ -5713,6 +5781,12 @@ void MissionImportData::MergeFrom(const MissionImportData& from) { mission_items_.MergeFrom(from.mission_items_); geofence_items_.MergeFrom(from.geofence_items_); rally_items_.MergeFrom(from.rally_items_); + if (from._internal_has_planned_home_position()) { + _internal_mutable_planned_home_position()->::mavsdk::rpc::mission_raw::MissionItem::MergeFrom(from._internal_planned_home_position()); + } + if (from._internal_has_planned_home_position() != 0) { + _internal_set_has_planned_home_position(from._internal_has_planned_home_position()); + } _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_); } @@ -5733,6 +5807,12 @@ void MissionImportData::InternalSwap(MissionImportData* other) { mission_items_.InternalSwap(&other->mission_items_); geofence_items_.InternalSwap(&other->geofence_items_); rally_items_.InternalSwap(&other->rally_items_); + ::PROTOBUF_NAMESPACE_ID::internal::memswap< + PROTOBUF_FIELD_OFFSET(MissionImportData, has_planned_home_position_) + + sizeof(MissionImportData::has_planned_home_position_) + - PROTOBUF_FIELD_OFFSET(MissionImportData, planned_home_position_)>( + reinterpret_cast(&planned_home_position_), + reinterpret_cast(&other->planned_home_position_)); } ::PROTOBUF_NAMESPACE_ID::Metadata MissionImportData::GetMetadata() const { diff --git a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h index 48a2544ec1..20806bc602 100644 --- a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h +++ b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h @@ -3846,6 +3846,8 @@ class MissionImportData final : kMissionItemsFieldNumber = 1, kGeofenceItemsFieldNumber = 2, kRallyItemsFieldNumber = 3, + kPlannedHomePositionFieldNumber = 5, + kHasPlannedHomePositionFieldNumber = 4, }; // repeated .mavsdk.rpc.mission_raw.MissionItem mission_items = 1; int mission_items_size() const; @@ -3901,6 +3903,33 @@ class MissionImportData final : const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem >& rally_items() const; + // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; + bool has_planned_home_position() const; + private: + bool _internal_has_planned_home_position() const; + public: + void clear_planned_home_position(); + const ::mavsdk::rpc::mission_raw::MissionItem& planned_home_position() const; + PROTOBUF_MUST_USE_RESULT ::mavsdk::rpc::mission_raw::MissionItem* release_planned_home_position(); + ::mavsdk::rpc::mission_raw::MissionItem* mutable_planned_home_position(); + void set_allocated_planned_home_position(::mavsdk::rpc::mission_raw::MissionItem* planned_home_position); + private: + const ::mavsdk::rpc::mission_raw::MissionItem& _internal_planned_home_position() const; + ::mavsdk::rpc::mission_raw::MissionItem* _internal_mutable_planned_home_position(); + public: + void unsafe_arena_set_allocated_planned_home_position( + ::mavsdk::rpc::mission_raw::MissionItem* planned_home_position); + ::mavsdk::rpc::mission_raw::MissionItem* unsafe_arena_release_planned_home_position(); + + // bool has_planned_home_position = 4; + void clear_has_planned_home_position(); + bool has_planned_home_position() const; + void set_has_planned_home_position(bool value); + private: + bool _internal_has_planned_home_position() const; + void _internal_set_has_planned_home_position(bool value); + public: + // @@protoc_insertion_point(class_scope:mavsdk.rpc.mission_raw.MissionImportData) private: class _Internal; @@ -3911,6 +3940,8 @@ class MissionImportData final : ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem > mission_items_; ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem > geofence_items_; ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem > rally_items_; + ::mavsdk::rpc::mission_raw::MissionItem* planned_home_position_; + bool has_planned_home_position_; mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; friend struct ::TableStruct_mission_5fraw_2fmission_5fraw_2eproto; }; @@ -5804,6 +5835,116 @@ MissionImportData::rally_items() const { return rally_items_; } +// bool has_planned_home_position = 4; +inline void MissionImportData::clear_has_planned_home_position() { + has_planned_home_position_ = false; +} +inline bool MissionImportData::_internal_has_planned_home_position() const { + return has_planned_home_position_; +} +inline bool MissionImportData::has_planned_home_position() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home_position) + return _internal_has_planned_home_position(); +} +inline void MissionImportData::_internal_set_has_planned_home_position(bool value) { + + has_planned_home_position_ = value; +} +inline void MissionImportData::set_has_planned_home_position(bool value) { + _internal_set_has_planned_home_position(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home_position) +} + +// .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; +inline bool MissionImportData::_internal_has_planned_home_position() const { + return this != internal_default_instance() && planned_home_position_ != nullptr; +} +inline bool MissionImportData::has_planned_home_position() const { + return _internal_has_planned_home_position(); +} +inline void MissionImportData::clear_planned_home_position() { + if (GetArenaForAllocation() == nullptr && planned_home_position_ != nullptr) { + delete planned_home_position_; + } + planned_home_position_ = nullptr; +} +inline const ::mavsdk::rpc::mission_raw::MissionItem& MissionImportData::_internal_planned_home_position() const { + const ::mavsdk::rpc::mission_raw::MissionItem* p = planned_home_position_; + return p != nullptr ? *p : reinterpret_cast( + ::mavsdk::rpc::mission_raw::_MissionItem_default_instance_); +} +inline const ::mavsdk::rpc::mission_raw::MissionItem& MissionImportData::planned_home_position() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.mission_raw.MissionImportData.planned_home_position) + return _internal_planned_home_position(); +} +inline void MissionImportData::unsafe_arena_set_allocated_planned_home_position( + ::mavsdk::rpc::mission_raw::MissionItem* planned_home_position) { + if (GetArenaForAllocation() == nullptr) { + delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(planned_home_position_); + } + planned_home_position_ = planned_home_position; + if (planned_home_position) { + + } else { + + } + // @@protoc_insertion_point(field_unsafe_arena_set_allocated:mavsdk.rpc.mission_raw.MissionImportData.planned_home_position) +} +inline ::mavsdk::rpc::mission_raw::MissionItem* MissionImportData::release_planned_home_position() { + + ::mavsdk::rpc::mission_raw::MissionItem* temp = planned_home_position_; + planned_home_position_ = nullptr; +#ifdef PROTOBUF_FORCE_COPY_IN_RELEASE + auto* old = reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(temp); + temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp); + if (GetArenaForAllocation() == nullptr) { delete old; } +#else // PROTOBUF_FORCE_COPY_IN_RELEASE + if (GetArenaForAllocation() != nullptr) { + temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp); + } +#endif // !PROTOBUF_FORCE_COPY_IN_RELEASE + return temp; +} +inline ::mavsdk::rpc::mission_raw::MissionItem* MissionImportData::unsafe_arena_release_planned_home_position() { + // @@protoc_insertion_point(field_release:mavsdk.rpc.mission_raw.MissionImportData.planned_home_position) + + ::mavsdk::rpc::mission_raw::MissionItem* temp = planned_home_position_; + planned_home_position_ = nullptr; + return temp; +} +inline ::mavsdk::rpc::mission_raw::MissionItem* MissionImportData::_internal_mutable_planned_home_position() { + + if (planned_home_position_ == nullptr) { + auto* p = CreateMaybeMessage<::mavsdk::rpc::mission_raw::MissionItem>(GetArenaForAllocation()); + planned_home_position_ = p; + } + return planned_home_position_; +} +inline ::mavsdk::rpc::mission_raw::MissionItem* MissionImportData::mutable_planned_home_position() { + ::mavsdk::rpc::mission_raw::MissionItem* _msg = _internal_mutable_planned_home_position(); + // @@protoc_insertion_point(field_mutable:mavsdk.rpc.mission_raw.MissionImportData.planned_home_position) + return _msg; +} +inline void MissionImportData::set_allocated_planned_home_position(::mavsdk::rpc::mission_raw::MissionItem* planned_home_position) { + ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArenaForAllocation(); + if (message_arena == nullptr) { + delete planned_home_position_; + } + if (planned_home_position) { + ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena = + ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper<::mavsdk::rpc::mission_raw::MissionItem>::GetOwningArena(planned_home_position); + if (message_arena != submessage_arena) { + planned_home_position = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage( + message_arena, planned_home_position, submessage_arena); + } + + } else { + + } + planned_home_position_ = planned_home_position; + // @@protoc_insertion_point(field_set_allocated:mavsdk.rpc.mission_raw.MissionImportData.planned_home_position) +} + // ------------------------------------------------------------------- // MissionRawResult diff --git a/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h b/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h index cf9274e355..20793dc67b 100644 --- a/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h +++ b/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h @@ -150,6 +150,11 @@ class MissionRawServiceImpl final : public rpc::mission_raw::MissionRawService:: ptr->CopyFrom(*translateToRpcMissionItem(elem).release()); } + rpc_obj->set_has_planned_home_position(mission_import_data.has_planned_home_position); + + rpc_obj->set_allocated_planned_home_position( + translateToRpcMissionItem(mission_import_data.planned_home_position).release()); + return rpc_obj; } @@ -173,6 +178,11 @@ class MissionRawServiceImpl final : public rpc::mission_raw::MissionRawService:: static_cast(elem))); } + obj.has_planned_home_position = mission_import_data.has_planned_home_position(); + + obj.planned_home_position = + translateFromRpcMissionItem(mission_import_data.planned_home_position()); + return obj; } From c84cc853c43ba05b9fb90b23a71da186aebab7bc Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 6 Sep 2023 17:09:47 +0300 Subject: [PATCH 3/6] started with example to demonstrate how to get planned home position from mission Signed-off-by: RomanBapst --- .../move_mission_to_vehicle/CMakeLists.txt | 22 +++ examples/move_mission_to_vehicle/main.cpp | 131 ++++++++++++++++++ 2 files changed, 153 insertions(+) create mode 100644 examples/move_mission_to_vehicle/CMakeLists.txt create mode 100644 examples/move_mission_to_vehicle/main.cpp diff --git a/examples/move_mission_to_vehicle/CMakeLists.txt b/examples/move_mission_to_vehicle/CMakeLists.txt new file mode 100644 index 0000000000..e89713931d --- /dev/null +++ b/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() diff --git a/examples/move_mission_to_vehicle/main.cpp b/examples/move_mission_to_vehicle/main.cpp new file mode 100644 index 0000000000..a54bb74f9f --- /dev/null +++ b/examples/move_mission_to_vehicle/main.cpp @@ -0,0 +1,131 @@ +/** + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +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 << " \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; + } + + auto system = mavsdk.first_autopilot(3.0); + if (!system) { + std::cerr << "Timed out waiting for system\n"; + return 1; + } + + auto action = Action{system.value()}; + auto mission_raw = MissionRaw{system.value()}; + auto telemetry = Telemetry{system.value()}; + + 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 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(); + REQUIRE(std::isfinite(position.latitude_deg)); + REQUIRE(std::isfinite(position.longitude_deg)); + + double offset_x, offset_y = 0.0; + + if (import_res.second.has_planned_home_position) { + std::cout << "Found planned home position, moving mission to vehicle position.\n"; + offset_x = import_res.second.planned_home_position.x - + static_cast(1e7 * position.latitude_deg); + offset_y = import_res.second.planned_home_position.y - + static_cast(1e7 * position.longitude_deg); + + } else { + std::cout << "No planned home position, move mission to first waypoint.\n"; + offset_x = import_res.second.mission_items[0].x - + static_cast(1e7 * position.latitude_deg); + offset_y = import_res.second.mission_items[0].y - + static_cast(1e7 * position.longitude_deg); + } + 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; +} + From 7010700af23763f343ace5faebabdce759244cea Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Wed, 6 Sep 2023 16:15:39 +0200 Subject: [PATCH 4/6] Rename to has_planned_home to avoid protoc conflict --- .../include/plugins/mission_raw/mission_raw.h | 4 +- .../plugins/mission_raw/mission_raw.cpp | 6 +- .../generated/mission_raw/mission_raw.pb.cc | 142 +++++++++--------- .../generated/mission_raw/mission_raw.pb.h | 42 +++--- .../mission_raw/mission_raw_service_impl.h | 4 +- 5 files changed, 98 insertions(+), 100 deletions(-) diff --git a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h index dba9764e8a..a5002cd7c1 100644 --- a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h +++ b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h @@ -124,8 +124,8 @@ class MissionRaw : public PluginBase { std::vector mission_items{}; /**< @brief Mission items */ std::vector geofence_items{}; /**< @brief Geofence items */ std::vector rally_items{}; /**< @brief Rally items */ - bool has_planned_home_position{}; /**< @brief Whether it contains a planned home position - element or not */ + bool has_planned_home{}; /**< @brief Whether it contains a planned home position element or + not */ MissionItem planned_home_position{}; /**< @brief Planned home position */ }; diff --git a/src/mavsdk/plugins/mission_raw/mission_raw.cpp b/src/mavsdk/plugins/mission_raw/mission_raw.cpp index 91a1037c03..28dc0d1667 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw.cpp @@ -170,8 +170,7 @@ 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.has_planned_home_position == lhs.has_planned_home_position) && + (rhs.rally_items == lhs.rally_items) && (rhs.has_planned_home == lhs.has_planned_home) && (rhs.planned_home_position == lhs.planned_home_position); } @@ -201,8 +200,7 @@ 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_position: " << mission_import_data.has_planned_home_position - << '\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; diff --git a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc index a794fdb70d..14fba4952e 100644 --- a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc +++ b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.cc @@ -320,7 +320,7 @@ constexpr MissionImportData::MissionImportData( , geofence_items_() , rally_items_() , planned_home_position_(nullptr) - , has_planned_home_position_(false){} + , has_planned_home_(false){} struct MissionImportDataDefaultTypeInternal { constexpr MissionImportDataDefaultTypeInternal() : _instance(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized{}) {} @@ -511,7 +511,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_mission_5fraw_2fmission_5fraw_ PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, mission_items_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, geofence_items_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, rally_items_), - PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, has_planned_home_position_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, has_planned_home_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionImportData, planned_home_position_), ~0u, // no _has_bits_ PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::mission_raw::MissionRawResult, _internal_metadata_), @@ -628,70 +628,70 @@ const char descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto[] PRO "ocontinue\030\005 \001(\r\022\016\n\006param1\030\006 \001(\002\022\016\n\006param" "2\030\007 \001(\002\022\016\n\006param3\030\010 \001(\002\022\016\n\006param4\030\t \001(\002\022" "\t\n\001x\030\n \001(\005\022\t\n\001y\030\013 \001(\005\022\t\n\001z\030\014 \001(\002\022\024\n\014miss" - "ion_type\030\r \001(\r\"\255\002\n\021MissionImportData\022:\n\r" + "ion_type\030\r \001(\r\"\244\002\n\021MissionImportData\022:\n\r" "mission_items\030\001 \003(\0132#.mavsdk.rpc.mission" "_raw.MissionItem\022;\n\016geofence_items\030\002 \003(\013" "2#.mavsdk.rpc.mission_raw.MissionItem\0228\n" "\013rally_items\030\003 \003(\0132#.mavsdk.rpc.mission_" - "raw.MissionItem\022!\n\031has_planned_home_posi" - "tion\030\004 \001(\010\022B\n\025planned_home_position\030\005 \001(" - "\0132#.mavsdk.rpc.mission_raw.MissionItem\"\310" - "\003\n\020MissionRawResult\022\?\n\006result\030\001 \001(\0162/.ma" - "vsdk.rpc.mission_raw.MissionRawResult.Re" - "sult\022\022\n\nresult_str\030\002 \001(\t\"\336\002\n\006Result\022\022\n\016R" - "ESULT_UNKNOWN\020\000\022\022\n\016RESULT_SUCCESS\020\001\022\020\n\014R" - "ESULT_ERROR\020\002\022!\n\035RESULT_TOO_MANY_MISSION" - "_ITEMS\020\003\022\017\n\013RESULT_BUSY\020\004\022\022\n\016RESULT_TIME" - "OUT\020\005\022\033\n\027RESULT_INVALID_ARGUMENT\020\006\022\026\n\022RE" - "SULT_UNSUPPORTED\020\007\022\037\n\033RESULT_NO_MISSION_" - "AVAILABLE\020\010\022\035\n\031RESULT_TRANSFER_CANCELLED" - "\020\t\022\"\n\036RESULT_FAILED_TO_OPEN_QGC_PLAN\020\n\022#" - "\n\037RESULT_FAILED_TO_PARSE_QGC_PLAN\020\013\022\024\n\020R" - "ESULT_NO_SYSTEM\020\0142\223\013\n\021MissionRawService\022" - "n\n\rUploadMission\022,.mavsdk.rpc.mission_ra" - "w.UploadMissionRequest\032-.mavsdk.rpc.miss" - "ion_raw.UploadMissionResponse\"\000\022\204\001\n\023Canc" - "elMissionUpload\0222.mavsdk.rpc.mission_raw" - ".CancelMissionUploadRequest\0323.mavsdk.rpc" - ".mission_raw.CancelMissionUploadResponse" - "\"\004\200\265\030\001\022t\n\017DownloadMission\022..mavsdk.rpc.m" - "ission_raw.DownloadMissionRequest\032/.mavs" - "dk.rpc.mission_raw.DownloadMissionRespon" - "se\"\000\022\212\001\n\025CancelMissionDownload\0224.mavsdk." - "rpc.mission_raw.CancelMissionDownloadReq" - "uest\0325.mavsdk.rpc.mission_raw.CancelMiss" - "ionDownloadResponse\"\004\200\265\030\001\022k\n\014StartMissio" - "n\022+.mavsdk.rpc.mission_raw.StartMissionR" - "equest\032,.mavsdk.rpc.mission_raw.StartMis" - "sionResponse\"\000\022k\n\014PauseMission\022+.mavsdk." - "rpc.mission_raw.PauseMissionRequest\032,.ma" - "vsdk.rpc.mission_raw.PauseMissionRespons" - "e\"\000\022k\n\014ClearMission\022+.mavsdk.rpc.mission" - "_raw.ClearMissionRequest\032,.mavsdk.rpc.mi" - "ssion_raw.ClearMissionResponse\"\000\022\206\001\n\025Set" - "CurrentMissionItem\0224.mavsdk.rpc.mission_" - "raw.SetCurrentMissionItemRequest\0325.mavsd" - "k.rpc.mission_raw.SetCurrentMissionItemR" - "esponse\"\000\022\210\001\n\030SubscribeMissionProgress\0227" - ".mavsdk.rpc.mission_raw.SubscribeMission" - "ProgressRequest\032/.mavsdk.rpc.mission_raw" - ".MissionProgressResponse\"\0000\001\022\211\001\n\027Subscri" - "beMissionChanged\0226.mavsdk.rpc.mission_ra" - "w.SubscribeMissionChangedRequest\032..mavsd" - "k.rpc.mission_raw.MissionChangedResponse" - "\"\004\200\265\030\0000\001\022\234\001\n\033ImportQgroundcontrolMission" - "\022:.mavsdk.rpc.mission_raw.ImportQgroundc" - "ontrolMissionRequest\032;.mavsdk.rpc.missio" - "n_raw.ImportQgroundcontrolMissionRespons" - "e\"\004\200\265\030\001B(\n\025io.mavsdk.mission_rawB\017Missio" - "nRawProtob\006proto3" + "raw.MissionItem\022\030\n\020has_planned_home\030\004 \001(" + "\010\022B\n\025planned_home_position\030\005 \001(\0132#.mavsd" + "k.rpc.mission_raw.MissionItem\"\310\003\n\020Missio" + "nRawResult\022\?\n\006result\030\001 \001(\0162/.mavsdk.rpc." + "mission_raw.MissionRawResult.Result\022\022\n\nr" + "esult_str\030\002 \001(\t\"\336\002\n\006Result\022\022\n\016RESULT_UNK" + "NOWN\020\000\022\022\n\016RESULT_SUCCESS\020\001\022\020\n\014RESULT_ERR" + "OR\020\002\022!\n\035RESULT_TOO_MANY_MISSION_ITEMS\020\003\022" + "\017\n\013RESULT_BUSY\020\004\022\022\n\016RESULT_TIMEOUT\020\005\022\033\n\027" + "RESULT_INVALID_ARGUMENT\020\006\022\026\n\022RESULT_UNSU" + "PPORTED\020\007\022\037\n\033RESULT_NO_MISSION_AVAILABLE" + "\020\010\022\035\n\031RESULT_TRANSFER_CANCELLED\020\t\022\"\n\036RES" + "ULT_FAILED_TO_OPEN_QGC_PLAN\020\n\022#\n\037RESULT_" + "FAILED_TO_PARSE_QGC_PLAN\020\013\022\024\n\020RESULT_NO_" + "SYSTEM\020\0142\223\013\n\021MissionRawService\022n\n\rUpload" + "Mission\022,.mavsdk.rpc.mission_raw.UploadM" + "issionRequest\032-.mavsdk.rpc.mission_raw.U" + "ploadMissionResponse\"\000\022\204\001\n\023CancelMission" + "Upload\0222.mavsdk.rpc.mission_raw.CancelMi" + "ssionUploadRequest\0323.mavsdk.rpc.mission_" + "raw.CancelMissionUploadResponse\"\004\200\265\030\001\022t\n" + "\017DownloadMission\022..mavsdk.rpc.mission_ra" + "w.DownloadMissionRequest\032/.mavsdk.rpc.mi" + "ssion_raw.DownloadMissionResponse\"\000\022\212\001\n\025" + "CancelMissionDownload\0224.mavsdk.rpc.missi" + "on_raw.CancelMissionDownloadRequest\0325.ma" + "vsdk.rpc.mission_raw.CancelMissionDownlo" + "adResponse\"\004\200\265\030\001\022k\n\014StartMission\022+.mavsd" + "k.rpc.mission_raw.StartMissionRequest\032,." + "mavsdk.rpc.mission_raw.StartMissionRespo" + "nse\"\000\022k\n\014PauseMission\022+.mavsdk.rpc.missi" + "on_raw.PauseMissionRequest\032,.mavsdk.rpc." + "mission_raw.PauseMissionResponse\"\000\022k\n\014Cl" + "earMission\022+.mavsdk.rpc.mission_raw.Clea" + "rMissionRequest\032,.mavsdk.rpc.mission_raw" + ".ClearMissionResponse\"\000\022\206\001\n\025SetCurrentMi" + "ssionItem\0224.mavsdk.rpc.mission_raw.SetCu" + "rrentMissionItemRequest\0325.mavsdk.rpc.mis" + "sion_raw.SetCurrentMissionItemResponse\"\000" + "\022\210\001\n\030SubscribeMissionProgress\0227.mavsdk.r" + "pc.mission_raw.SubscribeMissionProgressR" + "equest\032/.mavsdk.rpc.mission_raw.MissionP" + "rogressResponse\"\0000\001\022\211\001\n\027SubscribeMission" + "Changed\0226.mavsdk.rpc.mission_raw.Subscri" + "beMissionChangedRequest\032..mavsdk.rpc.mis" + "sion_raw.MissionChangedResponse\"\004\200\265\030\0000\001\022" + "\234\001\n\033ImportQgroundcontrolMission\022:.mavsdk" + ".rpc.mission_raw.ImportQgroundcontrolMis" + "sionRequest\032;.mavsdk.rpc.mission_raw.Imp" + "ortQgroundcontrolMissionResponse\"\004\200\265\030\001B(" + "\n\025io.mavsdk.mission_rawB\017MissionRawProto" + "b\006proto3" ; static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_deps[1] = { &::descriptor_table_mavsdk_5foptions_2eproto, }; static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_once; const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_mission_5fraw_2fmission_5fraw_2eproto = { - false, false, 4177, descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto, "mission_raw/mission_raw.proto", + false, false, 4168, descriptor_table_protodef_mission_5fraw_2fmission_5fraw_2eproto, "mission_raw/mission_raw.proto", &descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_once, descriptor_table_mission_5fraw_2fmission_5fraw_2eproto_deps, 1, 26, schemas, file_default_instances, TableStruct_mission_5fraw_2fmission_5fraw_2eproto::offsets, file_level_metadata_mission_5fraw_2fmission_5fraw_2eproto, file_level_enum_descriptors_mission_5fraw_2fmission_5fraw_2eproto, file_level_service_descriptors_mission_5fraw_2fmission_5fraw_2eproto, @@ -5528,15 +5528,15 @@ MissionImportData::MissionImportData(const MissionImportData& from) } else { planned_home_position_ = nullptr; } - has_planned_home_position_ = from.has_planned_home_position_; + has_planned_home_ = from.has_planned_home_; // @@protoc_insertion_point(copy_constructor:mavsdk.rpc.mission_raw.MissionImportData) } inline void MissionImportData::SharedCtor() { ::memset(reinterpret_cast(this) + static_cast( reinterpret_cast(&planned_home_position_) - reinterpret_cast(this)), - 0, static_cast(reinterpret_cast(&has_planned_home_position_) - - reinterpret_cast(&planned_home_position_)) + sizeof(has_planned_home_position_)); + 0, static_cast(reinterpret_cast(&has_planned_home_) - + reinterpret_cast(&planned_home_position_)) + sizeof(has_planned_home_)); } MissionImportData::~MissionImportData() { @@ -5574,7 +5574,7 @@ void MissionImportData::Clear() { delete planned_home_position_; } planned_home_position_ = nullptr; - has_planned_home_position_ = false; + has_planned_home_ = false; _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(); } @@ -5620,10 +5620,10 @@ const char* MissionImportData::_InternalParse(const char* ptr, ::PROTOBUF_NAMESP } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr)); } else goto handle_unusual; continue; - // bool has_planned_home_position = 4; + // bool has_planned_home = 4; case 4: if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 32)) { - has_planned_home_position_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr); + has_planned_home_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr); CHK_(ptr); } else goto handle_unusual; continue; @@ -5687,10 +5687,10 @@ ::PROTOBUF_NAMESPACE_ID::uint8* MissionImportData::_InternalSerialize( InternalWriteMessage(3, this->_internal_rally_items(i), target, stream); } - // bool has_planned_home_position = 4; - if (this->_internal_has_planned_home_position() != 0) { + // bool has_planned_home = 4; + if (this->_internal_has_planned_home() != 0) { target = stream->EnsureSpace(target); - target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(4, this->_internal_has_planned_home_position(), target); + target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(4, this->_internal_has_planned_home(), target); } // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; @@ -5745,8 +5745,8 @@ size_t MissionImportData::ByteSizeLong() const { *planned_home_position_); } - // bool has_planned_home_position = 4; - if (this->_internal_has_planned_home_position() != 0) { + // bool has_planned_home = 4; + if (this->_internal_has_planned_home() != 0) { total_size += 1 + 1; } @@ -5784,8 +5784,8 @@ void MissionImportData::MergeFrom(const MissionImportData& from) { if (from._internal_has_planned_home_position()) { _internal_mutable_planned_home_position()->::mavsdk::rpc::mission_raw::MissionItem::MergeFrom(from._internal_planned_home_position()); } - if (from._internal_has_planned_home_position() != 0) { - _internal_set_has_planned_home_position(from._internal_has_planned_home_position()); + if (from._internal_has_planned_home() != 0) { + _internal_set_has_planned_home(from._internal_has_planned_home()); } _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_); } @@ -5808,8 +5808,8 @@ void MissionImportData::InternalSwap(MissionImportData* other) { geofence_items_.InternalSwap(&other->geofence_items_); rally_items_.InternalSwap(&other->rally_items_); ::PROTOBUF_NAMESPACE_ID::internal::memswap< - PROTOBUF_FIELD_OFFSET(MissionImportData, has_planned_home_position_) - + sizeof(MissionImportData::has_planned_home_position_) + PROTOBUF_FIELD_OFFSET(MissionImportData, has_planned_home_) + + sizeof(MissionImportData::has_planned_home_) - PROTOBUF_FIELD_OFFSET(MissionImportData, planned_home_position_)>( reinterpret_cast(&planned_home_position_), reinterpret_cast(&other->planned_home_position_)); diff --git a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h index 20806bc602..d264d41ade 100644 --- a/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h +++ b/src/mavsdk_server/src/generated/mission_raw/mission_raw.pb.h @@ -3847,7 +3847,7 @@ class MissionImportData final : kGeofenceItemsFieldNumber = 2, kRallyItemsFieldNumber = 3, kPlannedHomePositionFieldNumber = 5, - kHasPlannedHomePositionFieldNumber = 4, + kHasPlannedHomeFieldNumber = 4, }; // repeated .mavsdk.rpc.mission_raw.MissionItem mission_items = 1; int mission_items_size() const; @@ -3921,13 +3921,13 @@ class MissionImportData final : ::mavsdk::rpc::mission_raw::MissionItem* planned_home_position); ::mavsdk::rpc::mission_raw::MissionItem* unsafe_arena_release_planned_home_position(); - // bool has_planned_home_position = 4; - void clear_has_planned_home_position(); - bool has_planned_home_position() const; - void set_has_planned_home_position(bool value); + // bool has_planned_home = 4; + void clear_has_planned_home(); + bool has_planned_home() const; + void set_has_planned_home(bool value); private: - bool _internal_has_planned_home_position() const; - void _internal_set_has_planned_home_position(bool value); + bool _internal_has_planned_home() const; + void _internal_set_has_planned_home(bool value); public: // @@protoc_insertion_point(class_scope:mavsdk.rpc.mission_raw.MissionImportData) @@ -3941,7 +3941,7 @@ class MissionImportData final : ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem > geofence_items_; ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::mavsdk::rpc::mission_raw::MissionItem > rally_items_; ::mavsdk::rpc::mission_raw::MissionItem* planned_home_position_; - bool has_planned_home_position_; + bool has_planned_home_; mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; friend struct ::TableStruct_mission_5fraw_2fmission_5fraw_2eproto; }; @@ -5835,24 +5835,24 @@ MissionImportData::rally_items() const { return rally_items_; } -// bool has_planned_home_position = 4; -inline void MissionImportData::clear_has_planned_home_position() { - has_planned_home_position_ = false; +// bool has_planned_home = 4; +inline void MissionImportData::clear_has_planned_home() { + has_planned_home_ = false; } -inline bool MissionImportData::_internal_has_planned_home_position() const { - return has_planned_home_position_; +inline bool MissionImportData::_internal_has_planned_home() const { + return has_planned_home_; } -inline bool MissionImportData::has_planned_home_position() const { - // @@protoc_insertion_point(field_get:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home_position) - return _internal_has_planned_home_position(); +inline bool MissionImportData::has_planned_home() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home) + return _internal_has_planned_home(); } -inline void MissionImportData::_internal_set_has_planned_home_position(bool value) { +inline void MissionImportData::_internal_set_has_planned_home(bool value) { - has_planned_home_position_ = value; + has_planned_home_ = value; } -inline void MissionImportData::set_has_planned_home_position(bool value) { - _internal_set_has_planned_home_position(value); - // @@protoc_insertion_point(field_set:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home_position) +inline void MissionImportData::set_has_planned_home(bool value) { + _internal_set_has_planned_home(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.mission_raw.MissionImportData.has_planned_home) } // .mavsdk.rpc.mission_raw.MissionItem planned_home_position = 5; diff --git a/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h b/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h index 20793dc67b..0ccee7e42c 100644 --- a/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h +++ b/src/mavsdk_server/src/plugins/mission_raw/mission_raw_service_impl.h @@ -150,7 +150,7 @@ class MissionRawServiceImpl final : public rpc::mission_raw::MissionRawService:: ptr->CopyFrom(*translateToRpcMissionItem(elem).release()); } - rpc_obj->set_has_planned_home_position(mission_import_data.has_planned_home_position); + rpc_obj->set_has_planned_home(mission_import_data.has_planned_home); rpc_obj->set_allocated_planned_home_position( translateToRpcMissionItem(mission_import_data.planned_home_position).release()); @@ -178,7 +178,7 @@ class MissionRawServiceImpl final : public rpc::mission_raw::MissionRawService:: static_cast(elem))); } - obj.has_planned_home_position = mission_import_data.has_planned_home_position(); + obj.has_planned_home = mission_import_data.has_planned_home(); obj.planned_home_position = translateFromRpcMissionItem(mission_import_data.planned_home_position()); From 8a2eb71139eb2ecec17322148ad2ace4958d8579 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Wed, 6 Sep 2023 19:26:53 +0200 Subject: [PATCH 5/6] Update proto submodule --- proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/proto b/proto index e25d8f07b5..a22cf87d8a 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit e25d8f07b5108d946d984a3e420ed2a6294a5842 +Subproject commit a22cf87d8aa069c336222b2615b3f743cd5e6438 From 9dd3bc074ef1f0ef8a8dc246ad490c894f584623 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 24 Oct 2023 12:02:55 +0200 Subject: [PATCH 6/6] fixes Signed-off-by: RomanBapst --- examples/move_mission_to_vehicle/main.cpp | 53 ++++++++++++++----- .../plugins/mission_raw/mission_import.cpp | 2 + 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/examples/move_mission_to_vehicle/main.cpp b/examples/move_mission_to_vehicle/main.cpp index a54bb74f9f..cfcbdc004f 100644 --- a/examples/move_mission_to_vehicle/main.cpp +++ b/examples/move_mission_to_vehicle/main.cpp @@ -55,15 +55,37 @@ int main(int argc, char** argv) return 1; } - auto system = mavsdk.first_autopilot(3.0); - if (!system) { - std::cerr << "Timed out waiting for system\n"; + std::cout << "Waiting to discover system...\n"; + auto prom = std::promise>{}; + 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; } - auto action = Action{system.value()}; - auto mission_raw = MissionRaw{system.value()}; - auto telemetry = Telemetry{system.value()}; + // 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"; @@ -88,13 +110,11 @@ int main(int argc, char** argv) << " mission items in the given QGC plan.\n"; - const auto position = telemetry->position(); - REQUIRE(std::isfinite(position.latitude_deg)); - REQUIRE(std::isfinite(position.longitude_deg)); + const auto position = telemetry.position(); double offset_x, offset_y = 0.0; - if (import_res.second.has_planned_home_position) { + 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(1e7 * position.latitude_deg); @@ -103,11 +123,16 @@ int main(int argc, char** argv) } else { std::cout << "No planned home position, move mission to first waypoint.\n"; - offset_x = import_res.second.mission_items[0].x - - static_cast(1e7 * position.latitude_deg); - offset_y = import_res.second.mission_items[0].y - - static_cast(1e7 * position.longitude_deg); + + for (auto &item : import_res.second.mission_items) { + if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT + offset_x = item.x - static_cast(1e7 * position.latitude_deg); + offset_y = item.y - static_cast(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; diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index e3bf97c01c..1899fbc64f 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -31,6 +31,8 @@ MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopil MissionRaw::MissionImportData import_data; 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}; }