Much better mission->kml conversion support

ChangeLog.md

@@ -10,6 +10,8 @@ Note: This file only contains high level features or important fixes.
 
 ### 4.0.6 - Not yet released
 
+* Plan: Much better conversion of missions to KML for 3d visualization/verification of missions
+
 ### 4.0.5 - Stable
 
 * Solo: Fix mission upload failures

qgroundcontrol.pro

@@ -587,7 +587,7 @@ HEADERS += \
     src/MissionManager/FixedWingLandingComplexItem.h \
     src/MissionManager/GeoFenceController.h \
     src/MissionManager/GeoFenceManager.h \
-    src/MissionManager/KML.h \
+    src/MissionManager/KMLPlanDomDocument.h \
     src/MissionManager/MissionCommandList.h \
     src/MissionManager/MissionCommandTree.h \
     src/MissionManager/MissionCommandUIInfo.h \
@@ -795,7 +795,7 @@ SOURCES += \
     src/MissionManager/FixedWingLandingComplexItem.cc \
     src/MissionManager/GeoFenceController.cc \
     src/MissionManager/GeoFenceManager.cc \
-    src/MissionManager/KML.cc \
+    src/MissionManager/KMLPlanDomDocument.cc \
     src/MissionManager/MissionCommandList.cc \
     src/MissionManager/MissionCommandTree.cc \
     src/MissionManager/MissionCommandUIInfo.cc \

src/MissionManager/KML.cc

-/****************************************************************************
- *
- * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
- *
- * QGroundControl is licensed according to the terms in the file
- * COPYING.md in the root of the source code directory.
- *
- ****************************************************************************/
-
-#include "KML.h"
-
-#include <QDomDocument>
-#include <QStringList>
-
-const QString Kml::_version("version=\"1.0\"");
-const QString Kml::_encoding("encoding=\"UTF-8\"");
-const QString Kml::_opengis("http://www.opengis.net/kml/2.2");
-const QString Kml::_qgckml("QGC KML");
-
-Kml::Kml()
-{
-    //create header
-    createHeader();
-    //name
-    createTextElement(_docEle, "name", _qgckml);
-    //open
-    createTextElement(_docEle, "open", "1");
-    //create style
-    createStyles();
-}
-
-void Kml::points(const QStringList& points)
-{
-    //create placemark
-    QDomElement placemark = _domDocument.createElement("Placemark");
-    _docEle.appendChild(placemark);
-    createTextElement(placemark, "styleUrl", "yellowLineGreenPoly");
-    createTextElement(placemark, "name", "Absolute");
-    createTextElement(placemark, "visibility", "0");
-    createTextElement(placemark, "description", "Transparent purple line");
-
-    QStringList latLonAlt = points[0].split(",");
-    QStringList lookAtList({latLonAlt[0], latLonAlt[1], "0" \
-        , "-100", "45", "2500"});
-    createLookAt(placemark, lookAtList);
-
-    //Add linestring
-    QDomElement lineString = _domDocument.createElement("LineString");
-    placemark.appendChild(lineString);
-
-    //extruder
-    createTextElement(lineString, "extruder", "1");
-    createTextElement(lineString, "tessellate", "1");
-    createTextElement(lineString, "altitudeMode", "absolute");
-    QString coordinates;
-    for(const auto& point : points) {
-        coordinates += point + "\n";
-    }
-    createTextElement(lineString, "coordinates", coordinates);
-}
-
-void Kml::save(QDomDocument& document)
-{
-    document = _domDocument;
-}
-
-void Kml::createHeader()
-{
-    QDomProcessingInstruction header = _domDocument.createProcessingInstruction("xml", _version + " " + _encoding);
-    _domDocument.appendChild(header);
-    QDomElement kml = _domDocument.createElement("kml");
-    kml.setAttribute("xmlns", _opengis);
-    _docEle = _domDocument.createElement("Document");
-    kml.appendChild(_docEle);
-    _domDocument.appendChild(kml);
-}
-
-void Kml::createStyles()
-{
-    QDomElement style = _domDocument.createElement("Style");
-    style.setAttribute("id", "yellowLineGreenPoly");
-    createStyleLine(style, "7f00ffff", "4", "7f00ff00");
-    _docEle.appendChild(style);
-}
-
-void Kml::createLookAt(QDomElement& placemark, const QStringList &lookAtList)
-{
-    QDomElement lookAt = _domDocument.createElement("LookAt");
-    placemark.appendChild(lookAt);
-    createTextElement(lookAt, "longitude", lookAtList[0]);
-    createTextElement(lookAt, "latitude", lookAtList[1]);
-    createTextElement(lookAt, "altitude", lookAtList[2]);
-    createTextElement(lookAt, "heading", lookAtList[3]);
-    createTextElement(lookAt, "tilt", lookAtList[4]);
-    createTextElement(lookAt, "range", lookAtList[5]);
-}
-
-void Kml::createTextElement(QDomElement& domEle, const QString& elementName, const QString& textElement)
-{
-    // <elementName>textElement</elementName>
-    auto element = _domDocument.createElement(elementName);
-    element.appendChild(_domDocument.createTextNode(textElement));
-    domEle.appendChild(element);
-}
-
-void Kml::createStyleLine(QDomElement& domEle, const QString& lineColor, const QString& lineWidth, const QString& polyColor)
-{
-    /*
-    <LineStyle>
-        <color>7f00ffff</color>
-        <width>4</width>
-    </LineStyle>
-    <PolyStyle>
-        <color>7f00ff00</color>
-    </PolyStyle>
-    */
-    auto lineStyle = _domDocument.createElement("LineStyle");
-    auto polyStyle = _domDocument.createElement("PolyStyle");
-    domEle.appendChild(lineStyle);
-    domEle.appendChild(polyStyle);
-    createTextElement(lineStyle, "color", lineColor);
-    createTextElement(lineStyle, "width", lineWidth);
-    createTextElement(polyStyle, "color", polyColor);
-}
-
-Kml::~Kml()
-{
-}

src/MissionManager/KMLPlanDomDocument.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "KMLPlanDomDocument.h"
+#include "QGCPalette.h"
+#include "QGCApplication.h"
+#include "MissionCommandTree.h"
+#include "MissionCommandUIInfo.h"
+#include "FactMetaData.h"
+
+#include <QDomDocument>
+#include <QStringList>
+
+const char* KMLPlanDomDocument::_missionLineStyleName = "MissionLineStyle";
+const char* KMLPlanDomDocument::_ballonStyleName =      "BalloonStyle";
+
+KMLPlanDomDocument::KMLPlanDomDocument()
+{
+    QDomProcessingInstruction header = createProcessingInstruction(QStringLiteral("xml"), QStringLiteral("version=\"1.0\" encoding=\"UTF-8\""));
+    appendChild(header);
+
+    QDomElement kmlElement = createElement(QStringLiteral("kml"));
+    kmlElement.setAttribute(QStringLiteral("xmlns"), "http://www.opengis.net/kml/2.2");
+
+    _documentElement = createElement(QStringLiteral("Document"));
+    kmlElement.appendChild(_documentElement);
+    appendChild(kmlElement);
+
+    _addTextElement(_documentElement, "name", QStringLiteral("%1 Plan KML").arg(qgcApp()->applicationName()));
+    _addTextElement(_documentElement, "open", "1");
+
+    _addStyles();
+}
+
+QString KMLPlanDomDocument::_kmlCoordString(const QGeoCoordinate& coord)
+{
+    return QStringLiteral("%1,%2,%3").arg(QString::number(coord.longitude(), 'f', 7)).arg(QString::number(coord.latitude(), 'f', 7)).arg(QString::number(coord.altitude(), 'f', 2));
+}
+
+void KMLPlanDomDocument::addMissionItems(Vehicle* vehicle, QList<MissionItem*> rgMissionItems)
+{
+    if (rgMissionItems.count() == 0) {
+        return;
+    }
+
+    QDomElement itemFolderElement = createElement("Folder");
+    _documentElement.appendChild(itemFolderElement);
+
+    _addTextElement(itemFolderElement, "name", "Items");
+
+    QDomElement flightPathElement = createElement("Placemark");
+    _documentElement.appendChild(flightPathElement);
+
+    _addTextElement(flightPathElement, "styleUrl",     QStringLiteral("#%1").arg(_missionLineStyleName));
+    _addTextElement(flightPathElement, "name",         "Flight Path");
+    _addTextElement(flightPathElement, "visibility",   "1");
+    _addLookAt(flightPathElement, rgMissionItems[0]->coordinate());
+
+    // Build up the mission trajectory line coords
+    QList<QGeoCoordinate> rgFlightCoords;
+    QGeoCoordinate homeCoord = rgMissionItems[0]->coordinate();
+    for (const MissionItem* item : rgMissionItems) {
+        const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, item->command());
+        if (uiInfo) {
+            double altAdjustment = item->frame() == MAV_FRAME_GLOBAL ? 0 : homeCoord.altitude(); // Used to convert to amsl
+            if (uiInfo->isTakeoffCommand() && !vehicle->fixedWing()) {
+                // These takeoff items go straight up from home position to specified altitude
+                QGeoCoordinate coord = homeCoord;
+                coord.setAltitude(item->param7() + altAdjustment);
+                rgFlightCoords += coord;
+            }
+            if (uiInfo->specifiesCoordinate()) {
+                QGeoCoordinate coord = item->coordinate();
+                coord.setAltitude(coord.altitude() + altAdjustment); // convert to amsl
+
+                if (!uiInfo->isStandaloneCoordinate()) {
+                    // Flight path goes through this item
+                    rgFlightCoords += coord;
+                }
+
+                // Add a place mark for each WP
+
+                QDomElement wpPlacemarkElement = createElement("Placemark");
+                _addTextElement(wpPlacemarkElement, "name",     QStringLiteral("%1 %2").arg(QString::number(item->sequenceNumber())).arg(item->command() == MAV_CMD_NAV_WAYPOINT ? "" : uiInfo->friendlyName()));
+                _addTextElement(wpPlacemarkElement, "styleUrl", QStringLiteral("#%1").arg(_ballonStyleName));
+
+                QDomElement wpPointElement = createElement("Point");
+                _addTextElement(wpPointElement, "altitudeMode", "absolute");
+                _addTextElement(wpPointElement, "coordinates",  _kmlCoordString(coord));
+                _addTextElement(wpPointElement, "extrude",      "1");
+
+                QDomElement descriptionElement = createElement("description");
+                QString htmlString;
+                htmlString += QStringLiteral("Index: %1\n").arg(item->sequenceNumber());
+                htmlString += uiInfo->friendlyName() + "\n";
+                htmlString += QStringLiteral("Alt AMSL: %1 %2\n").arg(QString::number(FactMetaData::metersToAppSettingsDistanceUnits(coord.altitude()).toDouble(), 'f', 2)).arg(FactMetaData::appSettingsDistanceUnitsString());
+                htmlString += QStringLiteral("Alt Rel: %1 %2\n").arg(QString::number(FactMetaData::metersToAppSettingsDistanceUnits(coord.altitude() - homeCoord.altitude()).toDouble(), 'f', 2)).arg(FactMetaData::appSettingsDistanceUnitsString());
+                htmlString += QStringLiteral("Lat: %1\n").arg(QString::number(coord.latitude(), 'f', 7));
+                htmlString += QStringLiteral("Lon: %1\n").arg(QString::number(coord.longitude(), 'f', 7));
+                QDomCDATASection cdataSection = createCDATASection(htmlString);
+                descriptionElement.appendChild(cdataSection);
+
+                wpPlacemarkElement.appendChild(descriptionElement);
+                wpPlacemarkElement.appendChild(wpPointElement);
+                itemFolderElement.appendChild(wpPlacemarkElement);
+            }
+        }
+    }
+
+    // Create a LineString element from the coords
+
+    QDomElement lineStringElement = createElement("LineString");
+    flightPathElement.appendChild(lineStringElement);
+
+    _addTextElement(lineStringElement, "extruder",      "1");
+    _addTextElement(lineStringElement, "tessellate",    "1");
+    _addTextElement(lineStringElement, "altitudeMode",  "absolute");
+
+    QString coordString;
+    for (const QGeoCoordinate& coord : rgFlightCoords) {
+        coordString += QStringLiteral("%1\n").arg(_kmlCoordString(coord));
+    }
+    _addTextElement(lineStringElement, "coordinates", coordString);
+}
+
+QString KMLPlanDomDocument::_kmlColorString (const QColor& color)
+{
+    return QStringLiteral("ff%1%2%3").arg(color.blue(), 2, 16, QChar('0')).arg(color.green(), 2, 16, QChar('0')).arg(color.red(), 2, 16, QChar('0'));
+}
+
+void KMLPlanDomDocument::_addStyles(void)
+{
+    QGCPalette palette;
+
+    QDomElement styleElement1 = createElement("Style");
+    styleElement1.setAttribute("id", _missionLineStyleName);
+    QDomElement lineStyleElement = createElement("LineStyle");
+    _addTextElement(lineStyleElement, "color", _kmlColorString(palette.mapMissionTrajectory()));
+    _addTextElement(lineStyleElement, "width", "4");
+    styleElement1.appendChild(lineStyleElement);
+
+    QDomElement styleElement2 = createElement("Style");
+    styleElement2.setAttribute("id", _ballonStyleName);
+    QDomElement balloonStyleElement = createElement("BalloonStyle");
+    _addTextElement(balloonStyleElement, "text", "$[description]");
+    styleElement2.appendChild(balloonStyleElement);
+
+    _documentElement.appendChild(styleElement1);
+    _documentElement.appendChild(styleElement2);
+}
+
+void KMLPlanDomDocument::_addTextElement(QDomElement &element, const QString &name, const QString &value)
+{
+    QDomElement textElement = createElement(name);
+    textElement.appendChild(createTextNode(value));
+    element.appendChild(textElement);
+}
+
+void KMLPlanDomDocument::_addLookAt(QDomElement& element, const QGeoCoordinate& coord)
+{
+    QDomElement lookAtElement = createElement("LookAt");
+    _addTextElement(lookAtElement, "latitude",  QString::number(coord.latitude(), 'f', 7));
+    _addTextElement(lookAtElement, "longitude", QString::number(coord.longitude(), 'f', 7));
+    _addTextElement(lookAtElement, "altitude",  QString::number(coord.longitude(), 'f', 2));
+    _addTextElement(lookAtElement, "heading",   "-100");
+    _addTextElement(lookAtElement, "tilt",      "45");
+    _addTextElement(lookAtElement, "range",     "2500");
+    element.appendChild(lookAtElement);
+}

src/MissionManager/KML.h → src/MissionManager/KMLPlanDomDocument.h

@@ -7,36 +7,33 @@
  *
  ****************************************************************************/
 
-#ifndef KML_H
-#define KML_H
+#pragma once
 
 #include <QDomDocument>
 #include <QDomElement>
+#include <QGeoCoordinate>
 
-class Kml
+class MissionItem;
+class Vehicle;
+
+/// Used to convert a Plan to a KML document
+class KMLPlanDomDocument : public QDomDocument
 {
 
 public:
-    Kml();
-    ~Kml();
+    KMLPlanDomDocument();
 
-    void points(const QStringList& points);
-    void polygon(const QStringList& points);
-    void save(QDomDocument& document);
+    void addMissionItems(Vehicle* vehicle, QList<MissionItem*> rgMissionItems);
 
 private:
-    void createHeader();
-    void createLookAt(QDomElement& placemark, const QStringList &lookAtList);
-    void createStyles();
-    void createStyleLine(QDomElement& domEle, const QString& lineColor, const QString& lineWidth, const QString& polyColor);
-    void createTextElement(QDomElement& domEle, const QString& elementName, const QString& textElement);
-
-    QDomDocument _domDocument;
-    QDomElement _docEle;
-    static const QString _encoding;
-    static const QString _opengis;
-    static const QString _qgckml;
-    static const QString _version;
-};
+    void    _addStyles      (void);
+    QString _kmlColorString (const QColor& color);
+    void    _addTextElement (QDomElement& element, const QString& name, const QString& value);
+    QString _kmlCoordString (const QGeoCoordinate& coord);
+    void    _addLookAt(QDomElement& element, const QGeoCoordinate& coord);
 
-#endif
+    QDomElement _documentElement;
+
+    static const char* _missionLineStyleName;
+    static const char* _ballonStyleName;
+};

src/MissionManager/MissionController.cc

@@ -28,7 +28,7 @@
 #include "MissionSettingsItem.h"
 #include "QGCQGeoCoordinate.h"
 #include "PlanMasterController.h"
-#include "KML.h"
+#include "KMLPlanDomDocument.h"
 #include "QGCCorePlugin.h"
 #include "TakeoffMissionItem.h"
 #include "PlanViewSettings.h"
@@ -273,46 +273,14 @@ bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMission
     return endActionSet;
 }
 
-void MissionController::convertToKMLDocument(QDomDocument& document)
+void MissionController::addMissionToKML(KMLPlanDomDocument& planKML)
 {
     QObject*            deleteParent = new QObject();
     QList<MissionItem*> rgMissionItems;
 
     _convertToMissionItems(_visualItems, rgMissionItems, deleteParent);
-    if (rgMissionItems.count() == 0) {
-        return;
-    }
-
-    const double homePositionAltitude = _settingsItem->coordinate().altitude();
-
-    QString coord;
-    QStringList coords;
-    // Drop home position
-    bool dropPoint = true;
-    for(const auto& item : rgMissionItems) {
-        if(dropPoint) {
-            dropPoint = false;
-            continue;
-        }
-        const MissionCommandUIInfo* uiInfo = \
-                qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command());
-
-        if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
-            double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homePositionAltitude);
-            coord = QString::number(item->param6(),'f',7) \
-                    + "," \
-                    + QString::number(item->param5(),'f',7) \
-                    + "," \
-                    + QString::number(amslAltitude,'f',2);
-            coords.append(coord);
-        }
-    }
-
+    planKML.addMissionItems(_controllerVehicle, rgMissionItems);
     deleteParent->deleteLater();
-
-    Kml kml;
-    kml.points(coords);
-    kml.save(document);
 }
 
 void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)

src/MissionManager/MissionController.h

@@ -13,7 +13,7 @@
 #include "QmlObjectListModel.h"
 #include "Vehicle.h"
 #include "QGCLoggingCategory.h"
-
+#include "KMLPlanDomDocument.h"
 #include "QGCGeoBoundingCube.h"
 
 #include <QHash>
@@ -195,7 +195,7 @@ public:
     bool showPlanFromManagerVehicle (void) final;
 
     // Create KML file
-    void convertToKMLDocument(QDomDocument& document);
+    void addMissionToKML(KMLPlanDomDocument& planKML);
 
     // Property accessors
 

src/MissionManager/MissionSettingsTest.cc

@@ -9,7 +9,6 @@
 
 #include "MissionSettingsTest.h"
 #include "QGCApplication.h"
-#include "QGroundControlQmlGlobal.h"
 #include "SettingsManager.h"
 
 MissionSettingsTest::MissionSettingsTest(void)

src/MissionManager/PlanMasterController.cc

@@ -15,7 +15,7 @@
 #include "AppSettings.h"
 #include "JsonHelper.h"
 #include "MissionManager.h"
-#include "KML.h"
+#include "KMLPlanDomDocument.h"
 #include "SurveyPlanCreator.h"
 #include "StructureScanPlanCreator.h"
 #include "CorridorScanPlanCreator.h"
@@ -492,10 +492,10 @@ void PlanMasterController::saveToKml(const QString& filename)
     if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
         qgcApp()->showMessage(tr("KML save error %1 : %2").arg(filename).arg(file.errorString()));
     } else {
-        QDomDocument domDocument;
-        _missionController.convertToKMLDocument(domDocument);
+        KMLPlanDomDocument planKML;
+        _missionController.addMissionToKML(planKML);
         QTextStream stream(&file);
-        stream << domDocument.toString();
+        stream << planKML.toString();
         file.close();
     }
 }