diff --git a/UnitTest.qrc b/UnitTest.qrc index 61510fde340a034be48ab0aee7593c72c27ebf2d..2bfa5bbb8062a9838d5fd3351bc49e074e13939d 100644 --- a/UnitTest.qrc +++ b/UnitTest.qrc @@ -9,9 +9,10 @@ src/MissionManager/UnitTest/MavCmdInfoVTOL.json src/MissionManager/UnitTest/MissionPlanner.waypoints src/MissionManager/UnitTest/OldFileFormat.mission - src/MissionManager/UnitTest/GoodPolygon.kml - src/MissionManager/UnitTest/MissingPolygonNode.kml - src/MissionManager/UnitTest/BadXml.kml - src/MissionManager/UnitTest/BadCoordinatesNode.kml + src/MissionManager/UnitTest/PolygonAreaTest.kml + src/MissionManager/UnitTest/PolygonGood.kml + src/MissionManager/UnitTest/PolygonMissingNode.kml + src/MissionManager/UnitTest/PolygonBadXml.kml + src/MissionManager/UnitTest/PolygonBadCoordinatesNode.kml diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 1609e178c8b4a21dcb710a0d5633787d8ebe40d2..d50187981078161eb4f839211b2e2bbef0dded2a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -415,6 +415,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/FactSystem/FactSystemTestPX4.h \ src/FactSystem/ParameterManagerTest.h \ src/MissionManager/CameraSectionTest.h \ + src/MissionManager/CorridorScanComplexItemTest.h \ src/MissionManager/MissionCommandTreeTest.h \ src/MissionManager/MissionControllerManagerTest.h \ src/MissionManager/MissionControllerTest.h \ @@ -423,6 +424,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/MissionSettingsTest.h \ src/MissionManager/PlanMasterControllerTest.h \ src/MissionManager/QGCMapPolygonTest.h \ + src/MissionManager/QGCMapPolylineTest.h \ src/MissionManager/SectionTest.h \ src/MissionManager/SimpleMissionItemTest.h \ src/MissionManager/SpeedSectionTest.h \ @@ -452,6 +454,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/FactSystem/FactSystemTestPX4.cc \ src/FactSystem/ParameterManagerTest.cc \ src/MissionManager/CameraSectionTest.cc \ + src/MissionManager/CorridorScanComplexItemTest.cc \ src/MissionManager/MissionCommandTreeTest.cc \ src/MissionManager/MissionControllerManagerTest.cc \ src/MissionManager/MissionControllerTest.cc \ @@ -460,6 +463,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/MissionSettingsTest.cc \ src/MissionManager/PlanMasterControllerTest.cc \ src/MissionManager/QGCMapPolygonTest.cc \ + src/MissionManager/QGCMapPolylineTest.cc \ src/MissionManager/SectionTest.cc \ src/MissionManager/SimpleMissionItemTest.cc \ src/MissionManager/SpeedSectionTest.cc \ @@ -508,6 +512,7 @@ HEADERS += \ src/MissionManager/CameraSection.h \ src/MissionManager/CameraSpec.h \ src/MissionManager/ComplexMissionItem.h \ + src/MissionManager/CorridorScanComplexItem.h \ src/MissionManager/FixedWingLandingComplexItem.h \ src/MissionManager/GeoFenceController.h \ src/MissionManager/GeoFenceManager.h \ @@ -526,6 +531,7 @@ HEADERS += \ src/MissionManager/QGCFencePolygon.h \ src/MissionManager/QGCMapCircle.h \ src/MissionManager/QGCMapPolygon.h \ + src/MissionManager/QGCMapPolyline.h \ src/MissionManager/RallyPoint.h \ src/MissionManager/RallyPointController.h \ src/MissionManager/RallyPointManager.h \ @@ -700,6 +706,7 @@ SOURCES += \ src/MissionManager/CameraSection.cc \ src/MissionManager/CameraSpec.cc \ src/MissionManager/ComplexMissionItem.cc \ + src/MissionManager/CorridorScanComplexItem.cc \ src/MissionManager/FixedWingLandingComplexItem.cc \ src/MissionManager/GeoFenceController.cc \ src/MissionManager/GeoFenceManager.cc \ @@ -718,6 +725,7 @@ SOURCES += \ src/MissionManager/QGCFencePolygon.cc \ src/MissionManager/QGCMapCircle.cc \ src/MissionManager/QGCMapPolygon.cc \ + src/MissionManager/QGCMapPolyline.cc \ src/MissionManager/RallyPoint.cc \ src/MissionManager/RallyPointController.cc \ src/MissionManager/RallyPointManager.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 366817258be621af4112dfb5b2c4aab8ac277cb6..0a01d0ca96c32ca5853d42046061265d8734324d 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -18,6 +18,7 @@ src/AnalyzeView/AnalyzeView.qml src/ui/AppSettings.qml src/ui/preferences/BluetoothSettings.qml + src/PlanView/CorridorScanEditor.qml src/ViewWidgets/CustomCommandWidget.qml src/ui/preferences/DebugWindow.qml src/AutoPilotPlugins/Common/ESP8266Component.qml @@ -48,6 +49,7 @@ src/PlanView/CameraCalc.qml src/PlanView/CameraSection.qml src/QmlControls/ClickableColor.qml + src/PlanView/CorridorScanMapVisual.qml src/QmlControls/DropButton.qml src/QmlControls/EditPositionDialog.qml src/QmlControls/ExclusiveGroupItem.qml @@ -89,6 +91,7 @@ src/QmlControls/QGCMapLabel.qml src/MissionManager/QGCMapCircleVisuals.qml src/MissionManager/QGCMapPolygonVisuals.qml + src/MissionManager/QGCMapPolylineVisuals.qml src/QmlControls/QGCMouseArea.qml src/QmlControls/QGCMovableItem.qml src/QmlControls/QGCPipable.qml @@ -202,6 +205,7 @@ src/MissionManager/QGCMapCircle.Facts.json src/Settings/RTK.SettingsGroup.json src/MissionManager/Survey.SettingsGroup.json + src/MissionManager/CorridorScan.SettingsGroup.json src/MissionManager/StructureScan.SettingsGroup.json src/Settings/Units.SettingsGroup.json src/Settings/Video.SettingsGroup.json diff --git a/src/MissionManager/CameraCalc.h b/src/MissionManager/CameraCalc.h index 2ec3b97bffb89d0ef560a8d05670d55987e50c64..2445975c65b4f841ffb4ff6536c37bba834a4aa7 100644 --- a/src/MissionManager/CameraCalc.h +++ b/src/MissionManager/CameraCalc.h @@ -49,6 +49,14 @@ public: Fact* adjustedFootprintSide (void) { return &_adjustedFootprintSideFact; } Fact* adjustedFootprintFrontal (void) { return &_adjustedFootprintFrontalFact; } + const Fact* valueSetIsDistance (void) const { return &_valueSetIsDistanceFact; } + const Fact* distanceToSurface (void) const { return &_distanceToSurfaceFact; } + const Fact* imageDensity (void) const { return &_imageDensityFact; } + const Fact* frontalOverlap (void) const { return &_frontalOverlapFact; } + const Fact* sideOverlap (void) const { return &_sideOverlapFact; } + const Fact* adjustedFootprintSide (void) const { return &_adjustedFootprintSideFact; } + const Fact* adjustedFootprintFrontal (void) const { return &_adjustedFootprintFrontalFact; } + bool isManualCamera (void) { return cameraName() == manualCameraName(); } double imageFootprintSide (void) const { return _imageFootprintSide; } double imageFootprintFrontal (void) const { return _imageFootprintFrontal; } diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 10ae81c92314137999302b2ef05c67095a5d5336..bc27fe7f80856a77f750925fb21b734bcc5fab0a 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -47,9 +47,9 @@ public: static const char* jsonComplexItemTypeKey; signals: - void complexDistanceChanged (double complexDistance); + void complexDistanceChanged (void); void greatestDistanceToChanged (void); - void additionalTimeDelayChanged (double additionalTimeDelay); + void additionalTimeDelayChanged (void); }; #endif diff --git a/src/MissionManager/CorridorScan.SettingsGroup.json b/src/MissionManager/CorridorScan.SettingsGroup.json new file mode 100644 index 0000000000000000000000000000000000000000..29a5eb62a73db448985169695ced9205c0dd129d --- /dev/null +++ b/src/MissionManager/CorridorScan.SettingsGroup.json @@ -0,0 +1,46 @@ +[ +{ + "name": "Altitude", + "shortDescription": "Altitude for the bottom layer of the structure scan.", + "type": "double", + "units": "m", + "decimalPlaces": 1, + "defaultValue": 50 +}, +{ + "name": "CorridorWidth", + "shortDescription": "Corridor width. Specify 0 width for a single pass scan.", + "type": "double", + "units": "m", + "min": 0, + "decimalPlaces": 1, + "defaultValue": 50 +}, +{ + "name": "Trigger distance", + "shortDescription": "Distance between each triggering of the camera. 0 specifies not camera trigger.", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m", + "defaultValue": 25 +}, +{ + "name": "GridSpacing", + "shortDescription": "Amount of spacing in between parallel grid lines.", + "type": "double", + "decimalPlaces": 2, + "min": 0.1, + "units": "m", + "defaultValue": 30 +}, +{ + "name": "TurnaroundDist", + "shortDescription": "Amount of additional distance to add outside the grid area for vehicle turnaround.", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m", + "defaultValue": 30 +} +] diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc new file mode 100644 index 0000000000000000000000000000000000000000..7faada78f14661b25bee20d30a8ad5abdc84cb69 --- /dev/null +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -0,0 +1,417 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "CorridorScanComplexItem.h" +#include "JsonHelper.h" +#include "MissionController.h" +#include "QGCGeo.h" +#include "QGroundControlQmlGlobal.h" +#include "QGCQGeoCoordinate.h" +#include "SettingsManager.h" +#include "AppSettings.h" +#include "QGCQGeoCoordinate.h" + +#include + +QGC_LOGGING_CATEGORY(CorridorScanComplexItemLog, "CorridorScanComplexItemLog") + +const char* CorridorScanComplexItem::_corridorWidthFactName = "CorridorWidth"; + +const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan"; +const char* CorridorScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; + +QMap CorridorScanComplexItem::_metaDataMap; + +CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, QObject* parent) + : ComplexMissionItem (vehicle, parent) + , _sequenceNumber (0) + , _dirty (false) + , _corridorWidthFact (0, _corridorWidthFactName, FactMetaData::valueTypeDouble) + , _ignoreRecalc (false) + , _scanDistance (0.0) + , _cameraShots (0) + , _cameraMinTriggerInterval (0) + , _cameraCalc (vehicle) +{ + _editorQml = "qrc:/qml/CorridorScanEditor.qml"; + + if (_metaDataMap.isEmpty()) { + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CorridorScan.SettingsGroup.json"), NULL /* QObject parent */); + } + + _corridorWidthFact.setMetaData(_metaDataMap[_corridorWidthFactName]); + + _corridorWidthFact.setRawValue(_corridorWidthFact.rawDefaultValue()); + + connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_setDirty); + connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_setDirty); + connect(this, &CorridorScanComplexItem::altitudeRelativeChanged, this, &CorridorScanComplexItem::_setDirty); + + connect(this, &CorridorScanComplexItem::altitudeRelativeChanged, this, &CorridorScanComplexItem::coordinateHasRelativeAltitudeChanged); + connect(this, &CorridorScanComplexItem::altitudeRelativeChanged, this, &CorridorScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); + + connect(&_corridorPolyline, &QGCMapPolyline::dirtyChanged, this, &CorridorScanComplexItem::_polylineDirtyChanged); + connect(&_corridorPolyline, &QGCMapPolyline::countChanged, this, &CorridorScanComplexItem::_polylineCountChanged); + + connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildTransects); + + connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_rebuildCorridor); + connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildCorridor); + + connect(&_corridorPolyline, &QGCMapPolyline::countChanged, this, &CorridorScanComplexItem::_signalLastSequenceNumberChanged); + connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_signalLastSequenceNumberChanged); + connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &CorridorScanComplexItem::_signalLastSequenceNumberChanged); + + connect(&_corridorPolygon, &QGCMapPolygon::pathChanged, this, &CorridorScanComplexItem::coveredAreaChanged); + + connect(this, &CorridorScanComplexItem::transectPointsChanged, this, &CorridorScanComplexItem::complexDistanceChanged); + connect(this, &CorridorScanComplexItem::transectPointsChanged, this, &CorridorScanComplexItem::greatestDistanceToChanged); + + _rebuildCorridor(); +} + +void CorridorScanComplexItem::_setScanDistance(double scanDistance) +{ + if (!qFuzzyCompare(_scanDistance, scanDistance)) { + _scanDistance = scanDistance; + emit complexDistanceChanged(); + } +} + +void CorridorScanComplexItem::_setCameraShots(int cameraShots) +{ + if (_cameraShots != cameraShots) { + _cameraShots = cameraShots; + emit cameraShotsChanged(); + } +} + +void CorridorScanComplexItem::_clearInternal(void) +{ + setDirty(true); + + emit specifiesCoordinateChanged(); + emit lastSequenceNumberChanged(lastSequenceNumber()); +} + +void CorridorScanComplexItem::_polylineCountChanged(int count) +{ + Q_UNUSED(count); + emit lastSequenceNumberChanged(lastSequenceNumber()); +} + +int CorridorScanComplexItem::lastSequenceNumber(void) const +{ + return _sequenceNumber + ((_corridorPolyline.count() + 2 /* trigger start/stop */) * _transectCount()); +} + +void CorridorScanComplexItem::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + emit dirtyChanged(_dirty); + } +} + +void CorridorScanComplexItem::save(QJsonArray& missionItems) +{ + QJsonObject saveObject; + + saveObject[JsonHelper::jsonVersionKey] = 1; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + saveObject[_corridorWidthFactName] = _corridorWidthFact.rawValue().toDouble(); + + QJsonObject cameraCalcObject; + _cameraCalc.save(cameraCalcObject); + saveObject[_jsonCameraCalcKey] = cameraCalcObject; + + _corridorPolyline.saveToJson(saveObject); + + missionItems.append(saveObject); +} + +void CorridorScanComplexItem::setSequenceNumber(int sequenceNumber) +{ + if (_sequenceNumber != sequenceNumber) { + _sequenceNumber = sequenceNumber; + emit sequenceNumberChanged(sequenceNumber); + emit lastSequenceNumberChanged(lastSequenceNumber()); + } +} + +bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) +{ + QList keyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { _corridorWidthFactName, QJsonValue::Double, true }, + { QGCMapPolyline::jsonPolylineKey, QJsonValue::Array, true }, + { _jsonCameraCalcKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + _corridorPolyline.clear(); + + QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { + errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + return false; + } + + int version = complexObject[JsonHelper::jsonVersionKey].toInt(); + if (version != 1) { + errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + return false; + } + + setSequenceNumber(sequenceNumber); + + if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) { + return false; + } + + if (!_corridorPolyline.loadFromJson(complexObject, true /* required */, errorString)) { + _corridorPolyline.clear(); + _rebuildCorridor(); + return false; + } + + _rebuildCorridor(); + + return true; +} + +double CorridorScanComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const +{ + double greatestDistance = 0.0; + for (int i=0; i<_transectPoints.count(); i++) { + QGeoCoordinate vertex = _transectPoints[i].value(); + double distance = vertex.distanceTo(other); + if (distance > greatestDistance) { + greatestDistance = distance; + } + } + + return greatestDistance; +} + +bool CorridorScanComplexItem::specifiesCoordinate(void) const +{ + return _corridorPolyline.count() > 1; +} + +int CorridorScanComplexItem::_transectCount(void) const +{ + double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); + double fullWidth = _corridorWidthFact.rawValue().toDouble(); + return fullWidth > 0.0 ? qCeil(fullWidth / transectSpacing) : 1; +} + +void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + int pointIndex = 0; + + while (pointIndex < _transectPoints.count()) { + bool addTrigger = true; + + for (int i=0; i<_corridorPolyline.count(); i++) { + QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); + + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + MAV_FRAME_GLOBAL_RELATIVE_ALT, // FIXME: Manual camera should support AMSL alt + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + vertexCoord.latitude(), + vertexCoord.longitude(), + _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + + if (addTrigger) { + addTrigger = false; + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance + 0, // shutter integration (ignore) + 1, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + } + + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // stop triggering + 0, // shutter integration (ignore) + 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + +} + +void CorridorScanComplexItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) +{ + ComplexMissionItem::setMissionFlightStatus(missionFlightStatus); + if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) { + _cruiseSpeed = missionFlightStatus.vehicleSpeed; + emit timeBetweenShotsChanged(); + } +} + +void CorridorScanComplexItem::_setDirty(void) +{ + setDirty(true); +} + +void CorridorScanComplexItem::applyNewAltitude(double newAltitude) +{ + // FIXME: NYI + //_altitudeFact.setRawValue(newAltitude); +} + +void CorridorScanComplexItem::_polylineDirtyChanged(bool dirty) +{ + if (dirty) { + setDirty(true); + } +} + +double CorridorScanComplexItem::timeBetweenShots(void) +{ + return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintSide()->rawValue().toDouble() / _cruiseSpeed; +} + +void CorridorScanComplexItem::_updateCoordinateAltitudes(void) +{ + emit coordinateChanged(coordinate()); + emit exitCoordinateChanged(exitCoordinate()); +} + +void CorridorScanComplexItem::rotateEntryPoint(void) +{ +#if 0 + _entryVertex++; + if (_entryVertex >= _flightPolygon.count()) { + _entryVertex = 0; + } + emit coordinateChanged(coordinate()); + emit exitCoordinateChanged(exitCoordinate()); +#endif +} + +void CorridorScanComplexItem::_rebuildCorridorPolygon(void) +{ + if (_corridorPolyline.count() < 2) { + _corridorPolygon.clear(); + return; + } + + double halfWidth = _corridorWidthFact.rawValue().toDouble() / 2.0; + + QList firstSideVertices = _corridorPolyline.offsetPolyline(halfWidth); + QList secondSideVertices = _corridorPolyline.offsetPolyline(-halfWidth); + + _corridorPolygon.clear(); + foreach (const QGeoCoordinate& vertex, firstSideVertices) { + _corridorPolygon.appendVertex(vertex); + } + for (int i=secondSideVertices.count() - 1; i >= 0; i--) { + _corridorPolygon.appendVertex(secondSideVertices[i]); + } +} + +void CorridorScanComplexItem::_rebuildTransects(void) +{ + + _transectPoints.clear(); + + double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); + double fullWidth = _corridorWidthFact.rawValue().toDouble(); + double halfWidth = fullWidth / 2.0; + int transectCount = _transectCount(); + double normalizedTransectPosition = transectSpacing / 2.0; + + _cameraShots = 0; + int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + + bool reverseVertices = false; + for (int i=0; i transectVertices = _corridorPolyline.offsetPolyline(offsetDistance); + if (reverseVertices) { + reverseVertices = false; + QList reversedVertices; + for (int j=transectVertices.count()-1; j>=0; j--) { + reversedVertices.append(transectVertices[j]); + } + transectVertices = reversedVertices; + } else { + reverseVertices = true; + } + for (int i=0; i() : QGeoCoordinate(); + _exitCoordinate = _transectPoints.count() ? _transectPoints.last().value() : QGeoCoordinate(); + + emit transectPointsChanged(); + emit cameraShotsChanged(); + emit coordinateChanged(_coordinate); + emit exitCoordinateChanged(_exitCoordinate); +} + +void CorridorScanComplexItem::_rebuildCorridor(void) +{ + _rebuildCorridorPolygon(); + _rebuildTransects(); +} + +void CorridorScanComplexItem::_signalLastSequenceNumberChanged(void) +{ + emit lastSequenceNumberChanged(lastSequenceNumber()); +} + +double CorridorScanComplexItem::coveredArea(void) const +{ + return _corridorPolygon.area(); +} diff --git a/src/MissionManager/CorridorScanComplexItem.h b/src/MissionManager/CorridorScanComplexItem.h new file mode 100644 index 0000000000000000000000000000000000000000..751ad2c4bc4349486c1a077d2845da35eebd0fe4 --- /dev/null +++ b/src/MissionManager/CorridorScanComplexItem.h @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "ComplexMissionItem.h" +#include "MissionItem.h" +#include "SettingsFact.h" +#include "QGCLoggingCategory.h" +#include "QGCMapPolyline.h" +#include "QGCMapPolygon.h" +#include "CameraCalc.h" + +Q_DECLARE_LOGGING_CATEGORY(CorridorScanComplexItemLog) + +class CorridorScanComplexItem : public ComplexMissionItem +{ + Q_OBJECT + +public: + CorridorScanComplexItem(Vehicle* vehicle, QObject* parent = NULL); + + Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) + Q_PROPERTY(QGCMapPolyline* corridorPolyline READ corridorPolyline CONSTANT) + Q_PROPERTY(QGCMapPolygon* corridorPolygon READ corridorPolygon CONSTANT) + Q_PROPERTY(Fact* corridorWidth READ corridorWidth CONSTANT) + Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) + Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) + Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) + Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) + Q_PROPERTY(QVariantList transectPoints READ transectPoints NOTIFY transectPointsChanged) + + CameraCalc* cameraCalc (void) { return &_cameraCalc; } + QGCMapPolyline* corridorPolyline(void) { return &_corridorPolyline; } + QGCMapPolygon* corridorPolygon (void) { return &_corridorPolygon; } + Fact* corridorWidth (void) { return &_corridorWidthFact; } + QVariantList transectPoints (void) { return _transectPoints; } + + int cameraShots (void) const { return _cameraShots; } + double timeBetweenShots (void); + double coveredArea (void) const; + + Q_INVOKABLE void rotateEntryPoint(void); + + // Overrides from ComplexMissionItem + + double complexDistance (void) const final { return _scanDistance; } + int lastSequenceNumber (void) const final; + bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; + double greatestDistanceTo (const QGeoCoordinate &other) const final; + QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); } + + // Overrides from VisualMissionItem + + bool dirty (void) const final { return _dirty; } + bool isSimpleItem (void) const final { return false; } + bool isStandaloneCoordinate (void) const final { return false; } + bool specifiesCoordinate (void) const final; + bool specifiesAltitudeOnly (void) const final { return false; } + QString commandDescription (void) const final { return tr("Corridor Scan"); } + QString commandName (void) const final { return tr("Corridor Scan"); } + QString abbreviation (void) const final { return "S"; } + QGeoCoordinate coordinate (void) const final { return _coordinate; } + QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; } + int sequenceNumber (void) const final { return _sequenceNumber; } + double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + void applyNewAltitude (double newAltitude) final; + + bool coordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } + bool exitCoordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } + bool exitCoordinateSameAsEntry (void) const final { return false; } + + void setDirty (bool dirty) final; + void setCoordinate (const QGeoCoordinate& coordinate) final { Q_UNUSED(coordinate); } + void setSequenceNumber (int sequenceNumber) final; + void save (QJsonArray& missionItems) final; + + static const char* jsonComplexItemTypeValue; + +signals: + void cameraShotsChanged (void); + void timeBetweenShotsChanged (void); + void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval); + void altitudeRelativeChanged (bool altitudeRelative); + void transectPointsChanged (void); + void coveredAreaChanged (void); + +private slots: + void _setDirty (void); + void _polylineDirtyChanged (bool dirty); + void _polylineCountChanged (int count); + void _clearInternal (void); + void _updateCoordinateAltitudes (void); + void _signalLastSequenceNumberChanged (void); + void _rebuildCorridor (void); + void _rebuildTransects (void); + +private: + void _setExitCoordinate (const QGeoCoordinate& coordinate); + void _setScanDistance (double scanDistance); + void _setCameraShots (int cameraShots); + double _triggerDistance (void) const; + int _transectCount (void) const; + void _rebuildCorridorPolygon(void); + + int _sequenceNumber; + bool _dirty; + QGeoCoordinate _coordinate; + QGeoCoordinate _exitCoordinate; + QGCMapPolyline _corridorPolyline; + QGCMapPolygon _corridorPolygon; + Fact _corridorWidthFact; + QVariantList _transectPoints; + + bool _ignoreRecalc; + double _scanDistance; + int _cameraShots; + double _timeBetweenShots; + double _cameraMinTriggerInterval; + double _cruiseSpeed; + CameraCalc _cameraCalc; + + static QMap _metaDataMap; + + static const char* _corridorWidthFactName; + + static const char* _jsonCameraCalcKey; +}; diff --git a/src/MissionManager/CorridorScanComplexItemTest.cc b/src/MissionManager/CorridorScanComplexItemTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..32ce4e6dd1c343c9ccdecb059cad26b7ab9edc82 --- /dev/null +++ b/src/MissionManager/CorridorScanComplexItemTest.cc @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "CorridorScanComplexItemTest.h" +#include "QGCApplication.h" + +CorridorScanComplexItemTest::CorridorScanComplexItemTest(void) + : _offlineVehicle(NULL) +{ + _linePoints << QGeoCoordinate(47.633550640000003, -122.08982199) + << QGeoCoordinate(47.634129020000003, -122.08887249) + << QGeoCoordinate(47.633619320000001, -122.08811074); +} + +void CorridorScanComplexItemTest::init(void) +{ + UnitTest::init(); + + _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); + _corridorItem = new CorridorScanComplexItem(_offlineVehicle, this); +// _corridorItem->setTurnaroundDist(0); // Unit test written for no turnaround distance + _corridorItem->setDirty(false); + _mapPolyline = _corridorItem->corridorPolyline(); + + _rgSignals[complexDistanceChangedIndex] = SIGNAL(complexDistanceChanged()); + _rgSignals[greatestDistanceToChangedIndex] = SIGNAL(greatestDistanceToChanged()); + _rgSignals[additionalTimeDelayChangedIndex] = SIGNAL(additionalTimeDelayChanged()); + _rgSignals[transectPointsChangedIndex] = SIGNAL(transectPointsChanged()); + _rgSignals[cameraShotsChangedIndex] = SIGNAL(cameraShotsChanged()); + _rgSignals[coveredAreaChangedIndex] = SIGNAL(coveredAreaChanged()); + _rgSignals[timeBetweenShotsChangedIndex] = SIGNAL(timeBetweenShotsChanged()); + _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + + _multiSpy = new MultiSignalSpy(); + QCOMPARE(_multiSpy->init(_corridorItem, _rgSignals, _cSignals), true); + + _rgCorridorPolygonSignals[corridorPolygonPathChangedIndex] = SIGNAL(pathChanged()); + + _multiSpyCorridorPolygon = new MultiSignalSpy(); + QCOMPARE(_multiSpyCorridorPolygon->init(_corridorItem->corridorPolygon(), _rgCorridorPolygonSignals, _cCorridorPolygonSignals), true); +} + +void CorridorScanComplexItemTest::cleanup(void) +{ + delete _corridorItem; + delete _offlineVehicle; + delete _multiSpy; +} + +void CorridorScanComplexItemTest::_testDirty(void) +{ + QVERIFY(!_corridorItem->dirty()); + _corridorItem->setDirty(false); + QVERIFY(!_corridorItem->dirty()); + QVERIFY(_multiSpy->checkNoSignals()); + + _corridorItem->setDirty(true); + QVERIFY(_corridorItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _multiSpy->clearAllSignals(); + + _corridorItem->setDirty(false); + QVERIFY(!_corridorItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(!_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _multiSpy->clearAllSignals(); + + // These facts should set dirty when changed + QList rgFacts; +#if 0 + rgFacts << _corridorItem->gridAltitude() << _corridorItem->gridAngle() << _corridorItem->gridSpacing() << _corridorItem->turnaroundDist() << _corridorItem->cameraTriggerDistance() << + _corridorItem->gridAltitudeRelative() << _corridorItem->cameraTriggerInTurnaround() << _corridorItem->hoverAndCapture(); +#endif + rgFacts << _corridorItem->corridorWidth(); + foreach(Fact* fact, rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_corridorItem->dirty()); + if (fact->typeIsBool()) { + fact->setRawValue(!fact->rawValue().toBool()); + } else { + fact->setRawValue(fact->rawValue().toDouble() + 1); + } + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _corridorItem->setDirty(false); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); + + // These facts should not change dirty bit +#if 0 + rgFacts << _corridorItem->groundResolution() << _corridorItem->frontalOverlap() << _corridorItem->sideOverlap() << _corridorItem->cameraSensorWidth() << _corridorItem->cameraSensorHeight() << + _corridorItem->cameraResolutionWidth() << _corridorItem->cameraResolutionHeight() << _corridorItem->cameraFocalLength() << _corridorItem->cameraOrientationLandscape() << + _corridorItem->fixedValueIsAltitude() << _corridorItem->camera() << _corridorItem->manualGrid(); +#endif + foreach(Fact* fact, rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_corridorItem->dirty()); + if (fact->typeIsBool()) { + fact->setRawValue(!fact->rawValue().toBool()); + } else { + fact->setRawValue(fact->rawValue().toDouble() + 1); + } + QVERIFY(_multiSpy->checkNoSignalByMask(dirtyChangedMask)); + QVERIFY(!_corridorItem->dirty()); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); +} + +void CorridorScanComplexItemTest::_testCameraTrigger(void) +{ +#if 0 + QCOMPARE(_corridorItem->property("cameraTrigger").toBool(), true); + + // Set up a grid + + for (int i=0; i<3; i++) { + _mapPolyline->appendVertex(_linePoints[i]); + } + + _corridorItem->setDirty(false); + _multiSpy->clearAllSignals(); + + int lastSeq = _corridorItem->lastSequenceNumber(); + QVERIFY(lastSeq > 0); + + // Turning off camera triggering should remove two camera trigger mission items, this should trigger: + // lastSequenceNumberChanged + // dirtyChanged + + _corridorItem->setProperty("cameraTrigger", false); + QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask)); + QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq - 2); + + _corridorItem->setDirty(false); + _multiSpy->clearAllSignals(); + + // Turn on camera triggering and make sure things go back to previous count + + _corridorItem->setProperty("cameraTrigger", true); + QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask)); + QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq); +#endif +} + +void CorridorScanComplexItemTest::_setPolyline(void) +{ + for (int i=0; i<_linePoints.count(); i++) { + QGeoCoordinate& vertex = _linePoints[i]; + _mapPolyline->appendVertex(vertex); + } +} + +#if 0 +void CorridorScanComplexItemTest::_testEntryLocation(void) +{ + _setPolygon(); + + for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { + _corridorItem->gridAngle()->setRawValue(gridAngle); + + QList rgSeenEntryCoords; + QList rgEntryLocation; + rgEntryLocation << SurveyMissionItem::EntryLocationTopLeft + << SurveyMissionItem::EntryLocationTopRight + << SurveyMissionItem::EntryLocationBottomLeft + << SurveyMissionItem::EntryLocationBottomRight; + + // Validate that each entry location is unique + for (int i=0; igridEntryLocation()->setRawValue(entryLocation); + QVERIFY(!rgSeenEntryCoords.contains(_corridorItem->coordinate())); + rgSeenEntryCoords << _corridorItem->coordinate(); + } + rgSeenEntryCoords.clear(); + } +} +#endif + +void CorridorScanComplexItemTest::_testItemCount(void) +{ + QList items; + + _setPolyline(); + +// _corridorItem->cameraTriggerInTurnaround()->setRawValue(false); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count(), _corridorItem->lastSequenceNumber()); + items.clear(); +} + +void CorridorScanComplexItemTest::_testPathChanges(void) +{ + _setPolyline(); + _corridorItem->setDirty(false); + _multiSpy->clearAllSignals(); + _multiSpyCorridorPolygon->clearAllSignals(); + + QGeoCoordinate vertex = _mapPolyline->vertexCoordinate(1); + vertex.setLatitude(vertex.latitude() + 0.01); + _mapPolyline->adjustVertex(1, vertex); + + QVERIFY(_corridorItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalsByMask(dirtyChangedMask | transectPointsChangedMask | cameraShotsChangedMask | coveredAreaChangedMask | complexDistanceChangedMask | greatestDistanceToChangedMask)); + QVERIFY(_multiSpyCorridorPolygon->checkSignalsByMask(corridorPolygonPathChangedMask)); + _multiSpy->clearAllSignals(); +} diff --git a/src/MissionManager/CorridorScanComplexItemTest.h b/src/MissionManager/CorridorScanComplexItemTest.h new file mode 100644 index 0000000000000000000000000000000000000000..c0dddd5d0e1aac207fc88cb5e47c3543cfcf2f72 --- /dev/null +++ b/src/MissionManager/CorridorScanComplexItemTest.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" +#include "TCPLink.h" +#include "MultiSignalSpy.h" +#include "CorridorScanComplexItem.h" + +#include + +class CorridorScanComplexItemTest : public UnitTest +{ + Q_OBJECT + +public: + CorridorScanComplexItemTest(void); + +protected: + void init(void) final; + void cleanup(void) final; + +private slots: + void _testDirty(void); + void _testCameraTrigger(void); +// void _testEntryLocation(void); + void _testItemCount(void); + void _testPathChanges(void); + +private: + void _setPolyline(void); + + enum { + complexDistanceChangedIndex = 0, + greatestDistanceToChangedIndex, + additionalTimeDelayChangedIndex, + transectPointsChangedIndex, + cameraShotsChangedIndex, + coveredAreaChangedIndex, + timeBetweenShotsChangedIndex, + dirtyChangedIndex, + maxSignalIndex + }; + + enum { + complexDistanceChangedMask = 1 << complexDistanceChangedIndex, + greatestDistanceToChangedMask = 1 << greatestDistanceToChangedIndex, + additionalTimeDelayChangedMask = 1 << additionalTimeDelayChangedIndex, + transectPointsChangedMask = 1 << transectPointsChangedIndex, + cameraShotsChangedMask = 1 << cameraShotsChangedIndex, + coveredAreaChangedMask = 1 << coveredAreaChangedIndex, + timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex, + dirtyChangedMask = 1 << dirtyChangedIndex + }; + + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; + + enum { + corridorPolygonPathChangedIndex = 0, + maxCorridorPolygonSignalIndex + }; + + enum { + corridorPolygonPathChangedMask = 1 << corridorPolygonPathChangedIndex, + }; + + static const size_t _cCorridorPolygonSignals = maxCorridorPolygonSignalIndex; + const char* _rgCorridorPolygonSignals[_cCorridorPolygonSignals]; + + Vehicle* _offlineVehicle; + MultiSignalSpy* _multiSpy; + MultiSignalSpy* _multiSpyCorridorPolygon; + CorridorScanComplexItem* _corridorItem; + QGCMapPolyline* _mapPolyline; + QList _linePoints; +}; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 0a4d899544d22a5d03ddb2ab2ed82864b7bf556c..991c460348eb7c337be226cba9a806d54158b7f1 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -19,7 +19,7 @@ #include "SurveyMissionItem.h" #include "FixedWingLandingComplexItem.h" #include "StructureScanComplexItem.h" -#include "StructureScanComplexItem.h" +#include "CorridorScanComplexItem.h" #include "JsonHelper.h" #include "ParameterManager.h" #include "QGroundControlQmlGlobal.h" @@ -63,6 +63,7 @@ MissionController::MissionController(PlanMasterController* masterController, QOb , _surveyMissionItemName(tr("Survey")) , _fwLandingMissionItemName(tr("Fixed Wing Landing")) , _structureScanMissionItemName(tr("Structure Scan")) + , _corridorScanMissionItemName(tr("Corridor Scan")) , _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings()) , _progressPct(0) , _currentPlanViewIndex(-1) @@ -416,6 +417,8 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems); } else if (itemName == _structureScanMissionItemName) { newItem = new StructureScanComplexItem(_controllerVehicle, _visualItems); + } else if (itemName == _corridorScanMissionItemName) { + newItem = new CorridorScanComplexItem(_controllerVehicle, _visualItems); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return sequenceNumber; @@ -697,6 +700,15 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec nextSequenceNumber = structureItem->lastSequenceNumber() + 1; qCDebug(MissionControllerLog) << "Structure Scan load complete: nextSequenceNumber" << nextSequenceNumber; visualItems->append(structureItem); + } else if (complexItemType == CorridorScanComplexItem::jsonComplexItemTypeValue) { + qCDebug(MissionControllerLog) << "Loading Corridor Scan: nextSequenceNumber" << nextSequenceNumber; + CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_controllerVehicle, visualItems); + if (!corridorItem->load(itemObject, nextSequenceNumber++, errorString)) { + return false; + } + nextSequenceNumber = corridorItem->lastSequenceNumber() + 1; + qCDebug(MissionControllerLog) << "Corridor Scan load complete: nextSequenceNumber" << nextSequenceNumber; + visualItems->append(corridorItem); } else if (complexItemType == MissionSettingsItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber; MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); @@ -1841,6 +1853,7 @@ QStringList MissionController::complexMissionItemNames(void) const QStringList complexItems; complexItems.append(_surveyMissionItemName); + complexItems.append(_corridorScanMissionItemName); if (_controllerVehicle->fixedWing()) { complexItems.append(_fwLandingMissionItemName); } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 1f2818823862dcb9bd834af176958c60a2d82f2d..390a4e8a62be8a3304959eea4d2168e90151068f 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -253,6 +253,7 @@ private: QString _surveyMissionItemName; QString _fwLandingMissionItemName; QString _structureScanMissionItemName; + QString _corridorScanMissionItemName; AppSettings* _appSettings; double _progressPct; int _currentPlanViewIndex; diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index 18ae00fdfbc9bd9f90d48ed1b5f09a007f170d8b..3803e203ed08495ae8d2d3cb62dd9728999a143b 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -362,7 +362,7 @@ QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const } } -QList QGCMapPolygon::nedPolygon(void) +QList QGCMapPolygon::nedPolygon(void) const { QList nedPolygon; @@ -515,3 +515,19 @@ bool QGCMapPolygon::loadKMLFile(const QString& kmlFile) return true; } + +double QGCMapPolygon::area(void) const +{ + // https://www.mathopenref.com/coordpolygonarea2.html + + double coveredArea = 0.0; + QList nedVertices = nedPolygon(); + for (int i=0; i nedPolygon(void); + QList nedPolygon(void) const; + + /// Returns the area of the polygon in meters squared + double area(void) const; // Property methods diff --git a/src/MissionManager/QGCMapPolygonTest.cc b/src/MissionManager/QGCMapPolygonTest.cc index 679292ad3a4c202125450fa402c2a2c4bbe6e0ae..920acab442543a22a07c9837e192541cac64336f 100644 --- a/src/MissionManager/QGCMapPolygonTest.cc +++ b/src/MissionManager/QGCMapPolygonTest.cc @@ -205,17 +205,23 @@ void QGCMapPolygonTest::_testVertexManipulation(void) void QGCMapPolygonTest::_testKMLLoad(void) { - QVERIFY(_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/GoodPolygon.kml"))); + QVERIFY(_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/PolygonGood.kml"))); setExpectedMessageBox(QMessageBox::Ok); - QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/BadXml.kml"))); + QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/PolygonBadXml.kml"))); checkExpectedMessageBox(); setExpectedMessageBox(QMessageBox::Ok); - QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/MissingPolygonNode.kml"))); + QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/PolygonMissingNode.kml"))); checkExpectedMessageBox(); setExpectedMessageBox(QMessageBox::Ok); - QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/BadCoordinatesNode.kml"))); + QVERIFY(!_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/PolygonBadCoordinatesNode.kml"))); checkExpectedMessageBox(); } + +void QGCMapPolygonTest::_testArea(void) +{ + QVERIFY(_mapPolygon->loadKMLFile(QStringLiteral(":/unittest/PolygonAreaTest.kml"))); + QCOMPARE(_mapPolygon->area(), 4620.05313465); +} diff --git a/src/MissionManager/QGCMapPolygonTest.h b/src/MissionManager/QGCMapPolygonTest.h index 7f36e926ff372188dbcf5d7bca555fbefb19f880..3ea2bb21190dba040ef38b045cb227736d34042b 100644 --- a/src/MissionManager/QGCMapPolygonTest.h +++ b/src/MissionManager/QGCMapPolygonTest.h @@ -30,6 +30,7 @@ private slots: void _testDirty(void); void _testVertexManipulation(void); void _testKMLLoad(void); + void _testArea(void); private: enum { diff --git a/src/MissionManager/QGCMapPolyline.cc b/src/MissionManager/QGCMapPolyline.cc new file mode 100644 index 0000000000000000000000000000000000000000..cb3a39ef8107a35e42d373b7a4d38d6a308ccf29 --- /dev/null +++ b/src/MissionManager/QGCMapPolyline.cc @@ -0,0 +1,440 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCMapPolyline.h" +#include "QGCGeo.h" +#include "JsonHelper.h" +#include "QGCQGeoCoordinate.h" +#include "QGCApplication.h" + +#include +#include +#include +#include +#include +#include + +const char* QGCMapPolyline::jsonPolylineKey = "polyline"; + +QGCMapPolyline::QGCMapPolyline(QObject* parent) + : QObject (parent) + , _dirty (false) + , _interactive (false) +{ + _init(); +} + +QGCMapPolyline::QGCMapPolyline(const QGCMapPolyline& other, QObject* parent) + : QObject (parent) + , _dirty (false) + , _interactive (false) +{ + *this = other; + + _init(); +} + +const QGCMapPolyline& QGCMapPolyline::operator=(const QGCMapPolyline& other) +{ + clear(); + + QVariantList vertices = other.path(); + for (int i=0; i()); + } + + setDirty(true); + + return *this; +} + +void QGCMapPolyline::_init(void) +{ + connect(&_polylineModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolyline::_polylineModelDirtyChanged); + connect(&_polylineModel, &QmlObjectListModel::countChanged, this, &QGCMapPolyline::_polylineModelCountChanged); +} + +void QGCMapPolyline::clear(void) +{ + _polylinePath.clear(); + emit pathChanged(); + + _polylineModel.clearAndDeleteContents(); + + emit cleared(); + + setDirty(true); +} + +void QGCMapPolyline::adjustVertex(int vertexIndex, const QGeoCoordinate coordinate) +{ + _polylinePath[vertexIndex] = QVariant::fromValue(coordinate); + emit pathChanged(); + _polylineModel.value(vertexIndex)->setCoordinate(coordinate); + setDirty(true); +} + +void QGCMapPolyline::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + if (!dirty) { + _polylineModel.setDirty(false); + } + emit dirtyChanged(dirty); + } +} +QGeoCoordinate QGCMapPolyline::_coordFromPointF(const QPointF& point) const +{ + QGeoCoordinate coord; + + if (_polylinePath.count() > 0) { + QGeoCoordinate tangentOrigin = _polylinePath[0].value(); + convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord); + } + + return coord; +} + +QPointF QGCMapPolyline::_pointFFromCoord(const QGeoCoordinate& coordinate) const +{ + if (_polylinePath.count() > 0) { + double y, x, down; + QGeoCoordinate tangentOrigin = _polylinePath[0].value(); + + convertGeoToNed(coordinate, tangentOrigin, &y, &x, &down); + return QPointF(x, -y); + } + + return QPointF(); +} + +void QGCMapPolyline::setPath(const QList& path) +{ + _polylinePath.clear(); + _polylineModel.clearAndDeleteContents(); + foreach (const QGeoCoordinate& coord, path) { + _polylinePath.append(QVariant::fromValue(coord)); + _polylineModel.append(new QGCQGeoCoordinate(coord, this)); + } + + setDirty(true); + emit pathChanged(); +} + +void QGCMapPolyline::setPath(const QVariantList& path) +{ + _polylinePath = path; + + _polylineModel.clearAndDeleteContents(); + for (int i=0; i<_polylinePath.count(); i++) { + _polylineModel.append(new QGCQGeoCoordinate(_polylinePath[i].value(), this)); + } + + setDirty(true); + emit pathChanged(); +} + + +void QGCMapPolyline::saveToJson(QJsonObject& json) +{ + QJsonValue jsonValue; + + JsonHelper::saveGeoCoordinateArray(_polylinePath, false /* writeAltitude*/, jsonValue); + json.insert(jsonPolylineKey, jsonValue); + setDirty(false); +} + +bool QGCMapPolyline::loadFromJson(const QJsonObject& json, bool required, QString& errorString) +{ + errorString.clear(); + clear(); + + if (required) { + if (!JsonHelper::validateRequiredKeys(json, QStringList(jsonPolylineKey), errorString)) { + return false; + } + } else if (!json.contains(jsonPolylineKey)) { + return true; + } + + if (!JsonHelper::loadGeoCoordinateArray(json[jsonPolylineKey], false /* altitudeRequired */, _polylinePath, errorString)) { + return false; + } + + for (int i=0; i<_polylinePath.count(); i++) { + _polylineModel.append(new QGCQGeoCoordinate(_polylinePath[i].value(), this)); + } + + setDirty(false); + emit pathChanged(); + + return true; +} + +QList QGCMapPolyline::coordinateList(void) const +{ + QList coords; + + for (int i=0; i<_polylinePath.count(); i++) { + coords.append(_polylinePath[i].value()); + } + + return coords; +} + +void QGCMapPolyline::splitSegment(int vertexIndex) +{ + int nextIndex = vertexIndex + 1; + if (nextIndex > _polylinePath.length() - 1) { + return; + } + + QGeoCoordinate firstVertex = _polylinePath[vertexIndex].value(); + QGeoCoordinate nextVertex = _polylinePath[nextIndex].value(); + + double distance = firstVertex.distanceTo(nextVertex); + double azimuth = firstVertex.azimuthTo(nextVertex); + QGeoCoordinate newVertex = firstVertex.atDistanceAndAzimuth(distance / 2, azimuth); + + if (nextIndex == 0) { + appendVertex(newVertex); + } else { + _polylineModel.insert(nextIndex, new QGCQGeoCoordinate(newVertex, this)); + _polylinePath.insert(nextIndex, QVariant::fromValue(newVertex)); + emit pathChanged(); + } +} + +void QGCMapPolyline::appendVertex(const QGeoCoordinate& coordinate) +{ + _polylinePath.append(QVariant::fromValue(coordinate)); + _polylineModel.append(new QGCQGeoCoordinate(coordinate, this)); + emit pathChanged(); +} + +void QGCMapPolyline::removeVertex(int vertexIndex) +{ + if (vertexIndex < 0 || vertexIndex > _polylinePath.length() - 1) { + qWarning() << "Call to removeVertex with bad vertexIndex:count" << vertexIndex << _polylinePath.length(); + return; + } + + if (_polylinePath.length() <= 2) { + // Don't allow the user to trash the polyline + return; + } + + QObject* coordObj = _polylineModel.removeAt(vertexIndex); + coordObj->deleteLater(); + + _polylinePath.removeAt(vertexIndex); + emit pathChanged(); +} + +void QGCMapPolyline::setInteractive(bool interactive) +{ + if (_interactive != interactive) { + _interactive = interactive; + emit interactiveChanged(interactive); + } +} + +QGeoCoordinate QGCMapPolyline::vertexCoordinate(int vertex) const +{ + if (vertex >= 0 && vertex < _polylinePath.count()) { + return _polylinePath[vertex].value(); + } else { + qWarning() << "QGCMapPolyline::vertexCoordinate bad vertex requested"; + return QGeoCoordinate(); + } +} + +QList QGCMapPolyline::nedPolyline(void) +{ + QList nedPolyline; + + if (count() > 0) { + QGeoCoordinate tangentOrigin = vertexCoordinate(0); + + for (int i=0; i<_polylinePath.count(); i++) { + double y, x, down; + QGeoCoordinate vertex = vertexCoordinate(i); + if (i == 0) { + // This avoids a nan calculation that comes out of convertGeoToNed + x = y = 0; + } else { + convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + } + nedPolyline += QPointF(x, y); + } + } + + return nedPolyline; +} + + +QList QGCMapPolyline::offsetPolyline(double distance) +{ + QList rgNewPolyline; + + // I'm sure there is some beautiful famous algorithm to do this, but here is a brute force method + + if (count() > 1) { + // Convert the polygon to NED + QList rgNedVertices = nedPolyline(); + + // Walk the edges, offsetting by the specified distance + QList rgOffsetEdges; + for (int i=0; ishowMessage(tr("File not found: %1").arg(kmlFile)); + return false; + } + + if (!file.open(QIODevice::ReadOnly)) { + qgcApp()->showMessage(tr("Unable to open file: %1 error: $%2").arg(kmlFile).arg(file.errorString())); + return false; + } + + QDomDocument doc; + QString errorMessage; + int errorLine; + if (!doc.setContent(&file, &errorMessage, &errorLine)) { + qgcApp()->showMessage(tr("Unable to parse KML file: %1 error: %2 line: %3").arg(kmlFile).arg(errorMessage).arg(errorLine)); + return false; + } + + QDomNodeList rgNodes = doc.elementsByTagName("Polygon"); + if (rgNodes.count() == 0) { + qgcApp()->showMessage(tr("Unable to find Polygon node in KML")); + return false; + } + + QDomNode coordinatesNode = rgNodes.item(0).namedItem("outerBoundaryIs").namedItem("LinearRing").namedItem("coordinates"); + if (coordinatesNode.isNull()) { + qgcApp()->showMessage(tr("Internal error: Unable to find coordinates node in KML")); + return false; + } + + QString coordinatesString = coordinatesNode.toElement().text().simplified(); + QStringList rgCoordinateStrings = coordinatesString.split(" "); + + QList rgCoords; + for (int i=0; i rgReversed; + + for (int i=0; i(); + QGeoCoordinate to = _polylinePath[i+1].value(); + length += from.distanceTo(to); + } + + return length; +} diff --git a/src/MissionManager/QGCMapPolyline.h b/src/MissionManager/QGCMapPolyline.h new file mode 100644 index 0000000000000000000000000000000000000000..f4ec936b2e2bebaab59f84c0d08d4f7b970d4ad3 --- /dev/null +++ b/src/MissionManager/QGCMapPolyline.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "QmlObjectListModel.h" + +class QGCMapPolyline : public QObject +{ + Q_OBJECT + +public: + QGCMapPolyline(QObject* parent = NULL); + QGCMapPolyline(const QGCMapPolyline& other, QObject* parent = NULL); + + const QGCMapPolyline& operator=(const QGCMapPolyline& other); + + Q_PROPERTY(int count READ count NOTIFY countChanged) + Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged) + Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged) + + Q_INVOKABLE void clear(void); + Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate); + Q_INVOKABLE void removeVertex(int vertexIndex); + + /// Adjust the value for the specified coordinate + /// @param vertexIndex Polygon point index to modify (0-based) + /// @param coordinate New coordinate for point + Q_INVOKABLE void adjustVertex(int vertexIndex, const QGeoCoordinate coordinate); + + /// Splits the line segment comprised of vertexIndex -> vertexIndex + 1 + Q_INVOKABLE void splitSegment(int vertexIndex); + + /// Offsets the current polyline edges by the specified distance in meters + /// @return Offset set of vertices + QList offsetPolyline(double distance); + + /// Loads a polyline from a KML file + /// @return true: success + Q_INVOKABLE bool loadKMLFile(const QString& kmlFile); + + /// Returns the path in a list of QGeoCoordinate's format + QList coordinateList(void) const; + + /// Returns the QGeoCoordinate for the vertex specified + QGeoCoordinate vertexCoordinate(int vertex) const; + + /// Saves the polyline to the json object. + /// @param json Json object to save to + void saveToJson(QJsonObject& json); + + /// Load a polyline from json + /// @param json Json object to load from + /// @param required true: no polygon in object will generate error + /// @param errorString Error string if return is false + /// @return true: success, false: failure (errorString set) + bool loadFromJson(const QJsonObject& json, bool required, QString& errorString); + + /// Convert polyline to NED and return (D is ignored) + QList nedPolyline(void); + + /// Returns the length of the polyline in meters + double length(void) const; + + // Property methods + + int count (void) const { return _polylinePath.count(); } + bool dirty (void) const { return _dirty; } + void setDirty (bool dirty); + bool interactive (void) const { return _interactive; } + QVariantList path (void) const { return _polylinePath; } + + QmlObjectListModel* qmlPathModel(void) { return &_polylineModel; } + QmlObjectListModel& pathModel (void) { return _polylineModel; } + + void setPath (const QList& path); + void setPath (const QVariantList& path); + void setInteractive (bool interactive); + + static const char* jsonPolylineKey; + +signals: + void countChanged (int count); + void pathChanged (void); + void dirtyChanged (bool dirty); + void cleared (void); + void interactiveChanged (bool interactive); + +private slots: + void _polylineModelCountChanged(int count); + void _polylineModelDirtyChanged(bool dirty); + +private: + void _init(void); + QGeoCoordinate _coordFromPointF(const QPointF& point) const; + QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const; + + QVariantList _polylinePath; + QmlObjectListModel _polylineModel; + bool _dirty; + bool _interactive; +}; diff --git a/src/MissionManager/QGCMapPolylineTest.cc b/src/MissionManager/QGCMapPolylineTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..e53ee73ead9bffe3be49de09d208452b89a88d4b --- /dev/null +++ b/src/MissionManager/QGCMapPolylineTest.cc @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCMapPolylineTest.h" +#include "QGCApplication.h" +#include "QGCQGeoCoordinate.h" + +QGCMapPolylineTest::QGCMapPolylineTest(void) +{ + _linePoints << QGeoCoordinate(47.635638361473475, -122.09269407980834 ) << + QGeoCoordinate(47.635638361473475, -122.08545246602667) << + QGeoCoordinate(47.63057923872075, -122.08545246602667) << + QGeoCoordinate(47.63057923872075, -122.09269407980834); +} + +void QGCMapPolylineTest::init(void) +{ + UnitTest::init(); + + _rgSignals[countChangedIndex] = SIGNAL(countChanged(int)); + _rgSignals[pathChangedIndex] = SIGNAL(pathChanged()); + _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + _rgSignals[clearedIndex] = SIGNAL(cleared()); + + _rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int)); + _rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + + _mapPolyline = new QGCMapPolyline(this); + _pathModel = _mapPolyline->qmlPathModel(); + QVERIFY(_pathModel); + + _multiSpyPolyline = new MultiSignalSpy(); + QCOMPARE(_multiSpyPolyline->init(_mapPolyline, _rgSignals, _cSignals), true); + + _multiSpyModel = new MultiSignalSpy(); + QCOMPARE(_multiSpyModel->init(_pathModel, _rgModelSignals, _cModelSignals), true); +} + +void QGCMapPolylineTest::cleanup(void) +{ + delete _mapPolyline; + delete _multiSpyPolyline; + delete _multiSpyModel; +} + +void QGCMapPolylineTest::_testDirty(void) +{ + // Check basic dirty bit set/get + + QVERIFY(!_mapPolyline->dirty()); + QVERIFY(!_pathModel->dirty()); + + _mapPolyline->setDirty(false); + QVERIFY(!_mapPolyline->dirty()); + QVERIFY(!_pathModel->dirty()); + QVERIFY(_multiSpyPolyline->checkNoSignals()); + QVERIFY(_multiSpyModel->checkNoSignals()); + + _mapPolyline->setDirty(true); + QVERIFY(_mapPolyline->dirty()); + QVERIFY(!_pathModel->dirty()); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpyPolyline->pullBoolFromSignalIndex(dirtyChangedIndex)); + QVERIFY(_multiSpyModel->checkNoSignals()); + _multiSpyPolyline->clearAllSignals(); + + _mapPolyline->setDirty(false); + QVERIFY(!_mapPolyline->dirty()); + QVERIFY(!_pathModel->dirty()); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(!_multiSpyPolyline->pullBoolFromSignalIndex(dirtyChangedIndex)); + QVERIFY(_multiSpyModel->checkNoSignals()); + _multiSpyPolyline->clearAllSignals(); + + _pathModel->setDirty(true); + QVERIFY(_pathModel->dirty()); + QVERIFY(_mapPolyline->dirty()); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpyPolyline->pullBoolFromSignalIndex(dirtyChangedIndex)); + QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask)); + QVERIFY(_multiSpyModel->pullBoolFromSignalIndex(modelDirtyChangedIndex)); + _multiSpyPolyline->clearAllSignals(); + _multiSpyModel->clearAllSignals(); + + _mapPolyline->setDirty(false); + QVERIFY(!_mapPolyline->dirty()); + QVERIFY(!_pathModel->dirty()); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(!_multiSpyPolyline->pullBoolFromSignalIndex(dirtyChangedIndex)); + QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask)); + QVERIFY(!_multiSpyModel->pullBoolFromSignalIndex(modelDirtyChangedIndex)); + _multiSpyPolyline->clearAllSignals(); + _multiSpyModel->clearAllSignals(); +} + +void QGCMapPolylineTest::_testVertexManipulation(void) +{ + // Vertex addition testing + + for (int i=0; i<_linePoints.count(); i++) { + QCOMPARE(_mapPolyline->count(), i); + + _mapPolyline->appendVertex(_linePoints[i]); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(pathChangedMask | dirtyChangedMask | countChangedMask)); + QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask | modelCountChangedMask)); + QCOMPARE(_multiSpyPolyline->pullIntFromSignalIndex(countChangedIndex), i+1); + QCOMPARE(_multiSpyModel->pullIntFromSignalIndex(modelCountChangedIndex), i+1); + + QVERIFY(_mapPolyline->dirty()); + QVERIFY(_pathModel->dirty()); + + QCOMPARE(_mapPolyline->count(), i+1); + + QVariantList vertexList = _mapPolyline->path(); + QCOMPARE(vertexList.count(), i+1); + QCOMPARE(vertexList[i].value(), _linePoints[i]); + + QCOMPARE(_pathModel->count(), i+1); + QCOMPARE(_pathModel->value(i)->coordinate(), _linePoints[i]); + + _mapPolyline->setDirty(false); + _multiSpyPolyline->clearAllSignals(); + _multiSpyModel->clearAllSignals(); + } + + // Vertex adjustment testing + + QGCQGeoCoordinate* geoCoord = _pathModel->value(1); + QSignalSpy coordSpy(geoCoord, SIGNAL(coordinateChanged(QGeoCoordinate))); + QSignalSpy coordDirtySpy(geoCoord, SIGNAL(dirtyChanged(bool))); + QGeoCoordinate adjustCoord(_linePoints[1].latitude() + 1, _linePoints[1].longitude() + 1); + _mapPolyline->adjustVertex(1, adjustCoord); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(pathChangedMask | dirtyChangedMask)); + QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask)); + QCOMPARE(coordSpy.count(), 1); + QCOMPARE(coordDirtySpy.count(), 1); + QCOMPARE(geoCoord->coordinate(), adjustCoord); + QVariantList vertexList = _mapPolyline->path(); + QCOMPARE(vertexList[0].value(), _linePoints[0]); + QCOMPARE(_pathModel->value(0)->coordinate(), _linePoints[0]); + QCOMPARE(vertexList[2].value(), _linePoints[2]); + QCOMPARE(_pathModel->value(2)->coordinate(), _linePoints[2]); + QCOMPARE(vertexList[3].value(), _linePoints[3]); + QCOMPARE(_pathModel->value(3)->coordinate(), _linePoints[3]); + + _mapPolyline->setDirty(false); + _multiSpyPolyline->clearAllSignals(); + _multiSpyModel->clearAllSignals(); + + // Vertex removal testing + + _mapPolyline->removeVertex(1); + QVERIFY(_multiSpyPolyline->checkOnlySignalByMask(pathChangedMask | dirtyChangedMask | countChangedMask)); + QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask | modelCountChangedMask)); + QCOMPARE(_mapPolyline->count(), 3); + vertexList = _mapPolyline->path(); + QCOMPARE(vertexList.count(), 3); + QCOMPARE(_pathModel->count(), 3); + QCOMPARE(vertexList[0].value(), _linePoints[0]); + QCOMPARE(_pathModel->value(0)->coordinate(), _linePoints[0]); + QCOMPARE(vertexList[1].value(), _linePoints[2]); + QCOMPARE(_pathModel->value(1)->coordinate(), _linePoints[2]); + QCOMPARE(vertexList[2].value(), _linePoints[3]); + QCOMPARE(_pathModel->value(2)->coordinate(), _linePoints[3]); + + // Clear testing + + _mapPolyline->clear(); + QVERIFY(_multiSpyPolyline->checkOnlySignalsByMask(pathChangedMask | dirtyChangedMask | countChangedMask | clearedMask)); + QVERIFY(_multiSpyModel->checkOnlySignalsByMask(modelDirtyChangedMask | modelCountChangedMask)); + QVERIFY(_mapPolyline->dirty()); + QVERIFY(_pathModel->dirty()); + QCOMPARE(_mapPolyline->count(), 0); + vertexList = _mapPolyline->path(); + QCOMPARE(vertexList.count(), 0); + QCOMPARE(_pathModel->count(), 0); +} + +#if 0 +void QGCMapPolylineTest::_testKMLLoad(void) +{ + QVERIFY(_mapPolyline->loadKMLFile(QStringLiteral(":/unittest/PolygonGood.kml"))); + + setExpectedMessageBox(QMessageBox::Ok); + QVERIFY(!_mapPolyline->loadKMLFile(QStringLiteral(":/unittest/BadXml.kml"))); + checkExpectedMessageBox(); + + setExpectedMessageBox(QMessageBox::Ok); + QVERIFY(!_mapPolyline->loadKMLFile(QStringLiteral(":/unittest/MissingPolygonNode.kml"))); + checkExpectedMessageBox(); + + setExpectedMessageBox(QMessageBox::Ok); + QVERIFY(!_mapPolyline->loadKMLFile(QStringLiteral(":/unittest/BadCoordinatesNode.kml"))); + checkExpectedMessageBox(); +} +#endif diff --git a/src/MissionManager/QGCMapPolylineTest.h b/src/MissionManager/QGCMapPolylineTest.h new file mode 100644 index 0000000000000000000000000000000000000000..9b02ac54e3d22f1f08ecb594d416e8a9920e4010 --- /dev/null +++ b/src/MissionManager/QGCMapPolylineTest.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" +#include "MultiSignalSpy.h" +#include "QGCMapPolyline.h" +#include "QmlObjectListModel.h" + +class QGCMapPolylineTest : public UnitTest +{ + Q_OBJECT + +public: + QGCMapPolylineTest(void); + +protected: + void init(void) final; + void cleanup(void) final; + +private slots: + void _testDirty(void); + void _testVertexManipulation(void); +// void _testKMLLoad(void); + +private: + enum { + countChangedIndex = 0, + pathChangedIndex, + dirtyChangedIndex, + clearedIndex, + maxSignalIndex + }; + + enum { + countChangedMask = 1 << countChangedIndex, + pathChangedMask = 1 << pathChangedIndex, + dirtyChangedMask = 1 << dirtyChangedIndex, + clearedMask = 1 << clearedIndex, + }; + + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; + + enum { + modelCountChangedIndex = 0, + modelDirtyChangedIndex, + maxModelSignalIndex + }; + + enum { + modelCountChangedMask = 1 << modelCountChangedIndex, + modelDirtyChangedMask = 1 << modelDirtyChangedIndex, + }; + + static const size_t _cModelSignals = maxModelSignalIndex; + const char* _rgModelSignals[_cModelSignals]; + + MultiSignalSpy* _multiSpyPolyline; + MultiSignalSpy* _multiSpyModel; + QGCMapPolyline* _mapPolyline; + QmlObjectListModel* _pathModel; + QList _linePoints; +}; diff --git a/src/MissionManager/QGCMapPolylineVisuals.qml b/src/MissionManager/QGCMapPolylineVisuals.qml new file mode 100644 index 0000000000000000000000000000000000000000..27f9ed8a787b0fb76effea19e02e4ed687fc69b4 --- /dev/null +++ b/src/MissionManager/QGCMapPolylineVisuals.qml @@ -0,0 +1,287 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 +import QtQuick.Dialogs 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +/// QGCmapPolyline map visuals +Item { + id: _root + + property var qgcView ///< QGCView for popping dialogs + property var mapControl ///< Map control to place item in + property var mapPolyline ///< QGCMapPolyline object + property bool interactive: mapPolyline.interactive + property int lineWidth: 3 + property color lineColor: "#be781c" + + + property var _polylineComponent + property var _dragHandlesComponent + property var _splitHandlesComponent + + property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap + property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2 + + function addVisuals() { + _polylineComponent = polylineComponent.createObject(mapControl) + mapControl.addMapItem(_polylineComponent) + } + + function removeVisuals() { + _polylineComponent.destroy() + } + + function addHandles() { + if (!_dragHandlesComponent) { + _dragHandlesComponent = dragHandlesComponent.createObject(mapControl) + _splitHandlesComponent = splitHandlesComponent.createObject(mapControl) + } + } + + function removeHandles() { + if (_dragHandlesComponent) { + _dragHandlesComponent.destroy() + _dragHandlesComponent = undefined + } + if (_splitHandlesComponent) { + _splitHandlesComponent.destroy() + _splitHandlesComponent = undefined + } + } + + /// Calculate the default/initial polyline + function defaultPolylineVertices() { + var x = map.centerViewport.x + (map.centerViewport.width / 2) + var yInset = map.centerViewport.height / 4 + var topPointCoord = map.toCoordinate(Qt.point(x, map.centerViewport.y + yInset), false /* clipToViewPort */) + var bottomPointCoord = map.toCoordinate(Qt.point(x, map.centerViewport.y + map.centerViewport.height - yInset), false /* clipToViewPort */) + return [ topPointCoord, bottomPointCoord ] + } + + /// Add an initial 2 point polyline + function addInitialPolyline() { + if (mapPolyline.count < 2) { + mapPolyline.clear() + var initialVertices = defaultPolylineVertices() + mapPolyline.appendVertex(initialVertices[0]) + mapPolyline.appendVertex(initialVertices[1]) + } + } + + /// Reset polyline back to initial default + function resetPolyline() { + mapPolyline.clear() + addInitialPolyline() + } + + onInteractiveChanged: { + if (interactive) { + addHandles() + } else { + removeHandles() + } + } + + Component.onCompleted: { + addVisuals() + if (interactive) { + addHandles() + } + } + + Component.onDestruction: { + removeVisuals() + removeHandles() + } + + QGCPalette { id: qgcPal } + + QGCFileDialog { + id: kmlLoadDialog + qgcView: _root.qgcView + folder: QGroundControl.settingsManager.appSettings.missionSavePath + title: qsTr("Select KML File") + selectExisting: true + nameFilters: [ qsTr("KML files (*.kml)") ] + + + onAcceptedForLoad: { + mapPolyline.loadKMLFile(file) + close() + } + } + + Component { + id: polylineComponent + + MapPolyline { + line.width: lineWidth + line.color: lineColor + path: mapPolyline.path + } + } + + Component { + id: splitHandleComponent + + MapQuickItem { + id: mapQuickItem + anchorPoint.x: splitHandle.width / 2 + anchorPoint.y: splitHandle.height / 2 + + property int vertexIndex + + sourceItem: Rectangle { + id: splitHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width / 2 + border.color: "white" + color: "transparent" + opacity: .50 + z: _zorderSplitHandle + + QGCLabel { + anchors.horizontalCenter: parent.horizontalCenter + anchors.verticalCenter: parent.verticalCenter + text: "+" + } + + QGCMouseArea { + fillItem: parent + onClicked: mapPolyline.splitSegment(mapQuickItem.vertexIndex) + } + } + } + } + + Component { + id: splitHandlesComponent + + Repeater { + model: mapPolyline.path + + delegate: Item { + property var _splitHandle + property var _vertices: mapPolyline.path + + function _setHandlePosition() { + var nextIndex = index + 1 + var distance = _vertices[index].distanceTo(_vertices[nextIndex]) + var azimuth = _vertices[index].azimuthTo(_vertices[nextIndex]) + _splitHandle.coordinate = _vertices[index].atDistanceAndAzimuth(distance / 2, azimuth) + } + + Component.onCompleted: { + if (index + 1 <= _vertices.length - 1) { + _splitHandle = splitHandleComponent.createObject(mapControl) + _splitHandle.vertexIndex = index + _setHandlePosition() + mapControl.addMapItem(_splitHandle) + } + } + + Component.onDestruction: { + if (_splitHandle) { + _splitHandle.destroy() + } + } + } + } + } + + // Control which is used to drag polygon vertices + Component { + id: dragAreaComponent + + MissionItemIndicatorDrag { + id: dragArea + z: _zorderDragHandle + + property int polylineVertex + + property bool _creationComplete: false + + Component.onCompleted: _creationComplete = true + + onItemCoordinateChanged: { + if (_creationComplete) { + // During component creation some bad coordinate values got through which screws up draw + mapPolyline.adjustVertex(polylineVertex, itemCoordinate) + } + } + + onClicked: mapPolyline.removeVertex(polylineVertex) + } + } + + Component { + id: dragHandleComponent + + MapQuickItem { + id: mapQuickItem + anchorPoint.x: dragHandle.width / 2 + anchorPoint.y: dragHandle.height / 2 + z: _zorderDragHandle + + property int polylineVertex + + sourceItem: Rectangle { + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width / 2 + color: "white" + opacity: .90 + } + } + } + + // Add all polygon vertex drag handles to the map + Component { + id: dragHandlesComponent + + Repeater { + model: mapPolyline.pathModel + + delegate: Item { + property var _visuals: [ ] + + Component.onCompleted: { + var dragHandle = dragHandleComponent.createObject(mapControl) + dragHandle.coordinate = Qt.binding(function() { return object.coordinate }) + dragHandle.polylineVertex = Qt.binding(function() { return index }) + mapControl.addMapItem(dragHandle) + var dragArea = dragAreaComponent.createObject(mapControl, { "itemIndicator": dragHandle, "itemCoordinate": object.coordinate }) + dragArea.polylineVertex = Qt.binding(function() { return index }) + _visuals.push(dragHandle) + _visuals.push(dragArea) + } + + Component.onDestruction: { + for (var i=0; i<_visuals.length; i++) { + _visuals[i].destroy() + } + _visuals = [ ] + } + } + } + } +} + diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index a32b22848e47e6dfe7c711536447c84615da1a02..8c7aeeaf8b3d3aea23e6d6c0e7253c5b652d81c4 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -102,7 +102,7 @@ void StructureScanComplexItem::_setScanDistance(double scanDistance) { if (!qFuzzyCompare(_scanDistance, scanDistance)) { _scanDistance = scanDistance; - emit complexDistanceChanged(_scanDistance); + emit complexDistanceChanged(); } } diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index 91d2989fea2f34a92daaf23574c4ac1d848f90c5..1e8b5e9f4590bde163effb055a5212090f76e4b0 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -159,7 +159,7 @@ void SurveyMissionItem::_setSurveyDistance(double surveyDistance) { if (!qFuzzyCompare(_surveyDistance, surveyDistance)) { _surveyDistance = surveyDistance; - emit complexDistanceChanged(_surveyDistance); + emit complexDistanceChanged(); } } @@ -742,7 +742,7 @@ void SurveyMissionItem::_generateGrid(void) if (_hoverAndCaptureEnabled()) { _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds; } - emit additionalTimeDelayChanged(_additionalFlightDelaySeconds); + emit additionalTimeDelayChanged(); emit gridPointsChanged(); diff --git a/src/MissionManager/UnitTest/PolygonAreaTest.kml b/src/MissionManager/UnitTest/PolygonAreaTest.kml new file mode 100755 index 0000000000000000000000000000000000000000..455643f37757e65fa10cd34aa3088ea31aafbbd6 --- /dev/null +++ b/src/MissionManager/UnitTest/PolygonAreaTest.kml @@ -0,0 +1,48 @@ + + + + AreaTestPolygon.kmz + + + normal + #s_ylw-pushpin + + + highlight + #s_ylw-pushpin_hl + + + + + + Untitled Polygon + #m_ylw-pushpin + + 1 + + + + -122.1059149362712,47.65965281788451,0 -122.1044593196253,47.66002598220988,0 -122.1047336695092,47.66034166158975,0 -122.1061470943783,47.6599810708829,0 -122.1059149362712,47.65965281788451,0 + + + + + + + diff --git a/src/MissionManager/UnitTest/BadCoordinatesNode.kml b/src/MissionManager/UnitTest/PolygonBadCoordinatesNode.kml similarity index 100% rename from src/MissionManager/UnitTest/BadCoordinatesNode.kml rename to src/MissionManager/UnitTest/PolygonBadCoordinatesNode.kml diff --git a/src/MissionManager/UnitTest/BadXml.kml b/src/MissionManager/UnitTest/PolygonBadXml.kml similarity index 100% rename from src/MissionManager/UnitTest/BadXml.kml rename to src/MissionManager/UnitTest/PolygonBadXml.kml diff --git a/src/MissionManager/UnitTest/GoodPolygon.kml b/src/MissionManager/UnitTest/PolygonGood.kml similarity index 100% rename from src/MissionManager/UnitTest/GoodPolygon.kml rename to src/MissionManager/UnitTest/PolygonGood.kml diff --git a/src/MissionManager/UnitTest/MissingPolygonNode.kml b/src/MissionManager/UnitTest/PolygonMissingNode.kml similarity index 100% rename from src/MissionManager/UnitTest/MissingPolygonNode.kml rename to src/MissionManager/UnitTest/PolygonMissingNode.kml diff --git a/src/PlanView/CorridorScanEditor.qml b/src/PlanView/CorridorScanEditor.qml new file mode 100644 index 0000000000000000000000000000000000000000..fcf37938841d0f1398c1613307d4e25a488dcd6e --- /dev/null +++ b/src/PlanView/CorridorScanEditor.qml @@ -0,0 +1,122 @@ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Dialogs 1.2 +import QtQuick.Extras 1.4 +import QtQuick.Layouts 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.FlightMap 1.0 + +// Editor for Survery mission items +Rectangle { + id: _root + height: visible ? (editorColumn.height + (_margin * 2)) : 0 + width: availableWidth + color: qgcPal.windowShadeDark + radius: _radius + + // The following properties must be available up the hierarchy chain + //property real availableWidth ///< Width for control + //property var missionItem ///< Mission Item for editor + + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + + function polygonCaptureStarted() { + missionItem.clearPolygon() + } + + function polygonCaptureFinished(coordinates) { + for (var i=0; i 0 && missionItem.cameraMinTriggerInterval !== 0 && missionItem.cameraMinTriggerInterval > missionItem.timeBetweenShots + } + + CameraCalc { + cameraCalc: missionItem.cameraCalc + vehicleFlightIsFrontal: true + distanceToSurfaceLabel: qsTr("Altitude") + frontalDistanceLabel: qsTr("Trigger Distance") + sideDistanceLabel: qsTr("Spacing") + } + + SectionHeader { + id: corridorHeader + text: qsTr("Corridor") + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + visible: corridorHeader.checked + + QGCLabel { text: qsTr("Width") } + FactTextField { + fact: missionItem.corridorWidth + Layout.fillWidth: true + } + } + + SectionHeader { + id: statsHeader + text: qsTr("Statistics") + } + + Grid { + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + visible: statsHeader.checked + + QGCLabel { text: qsTr("Photo count") } + QGCLabel { text: missionItem.cameraShots } + + QGCLabel { text: qsTr("Photo interval") } + QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } + } + } // Column +} // Rectangle diff --git a/src/PlanView/CorridorScanMapVisual.qml b/src/PlanView/CorridorScanMapVisual.qml new file mode 100644 index 0000000000000000000000000000000000000000..014bbe9a77f3470e169abac0cc6b04ccb9da5106 --- /dev/null +++ b/src/PlanView/CorridorScanMapVisual.qml @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 + +/// Corridor Scan Complex Mission Item visuals +Item { + id: _root + + property var map ///< Map control to place item in + property var qgcView ///< QGCView to use for popping dialogs + + property var _missionItem: object + property var _entryCoordinate + property var _exitCoordinate + property var _transectLines + + signal clicked(int sequenceNumber) + + function _addVisualElements() { + _entryCoordinate = entryPointComponent.createObject(map) + _exitCoordinate = exitPointComponent.createObject(map) + _transectLines = transectsComponent.createObject(map) + map.addMapItem(_entryCoordinate) + map.addMapItem(_exitCoordinate) + map.addMapItem(_transectLines) + } + + function _destroyVisualElements() { + _entryCoordinate.destroy() + _exitCoordinate.destroy() + _transectLines.destroy() + } + + Component.onCompleted: { + mapPolylineVisuals.addInitialPolyline() + _addVisualElements() + } + + Component.onDestruction: { + _destroyVisualElements() + } + + QGCMapPolygonVisuals { + qgcView: _root.qgcView + mapControl: map + mapPolygon: object.corridorPolygon + interactive: false + interiorColor: "green" + interiorOpacity: 0.25 + } + + QGCMapPolylineVisuals { + id: mapPolylineVisuals + qgcView: _root.qgcView + mapControl: map + mapPolyline: object.corridorPolyline + interactive: _missionItem.isCurrentItem + lineWidth: 3 + lineColor: "#be781c" + } + + // Entry point + Component { + id: entryPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.coordinate + visible: _missionItem.coordinate.isValid + + sourceItem: MissionItemIndexLabel { + index: _missionItem.sequenceNumber + label: "Entry" + checked: _missionItem.isCurrentItem + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } + + // Exit point + Component { + id: exitPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.exitCoordinate + visible: _missionItem.exitCoordinate.isValid + + sourceItem: MissionItemIndexLabel { + index: _missionItem.lastSequenceNumber + label: "Exit" + checked: _missionItem.isCurrentItem + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } + + // Transect lines + Component { + id: transectsComponent + + MapPolyline { + line.color: "white" + line.width: 2 + path: _missionItem.transectPoints + } + } +} diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 82ffdc70fad541820d0fdb1e6095de125cb289b0..02680ff52d8632d517d830ff2f053a9ef7bfcdbb 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -48,6 +48,7 @@ QGCListView 1.0 QGCListView.qml QGCMapLabel 1.0 QGCMapLabel.qml QGCMapCircleVisuals 1.0 QGCMapCircleVisuals.qml QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml +QGCMapPolylineVisuals 1.0 QGCMapPolylineVisuals.qml QGCMouseArea 1.0 QGCMouseArea.qml QGCMovableItem 1.0 QGCMovableItem.qml QGCPipable 1.0 QGCPipable.qml diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index fe67513cc739dcc3e4222173b87b48043dda3be0..973636411581482b084ac99635169618a9db2906 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -40,6 +40,8 @@ #include "QGCMapPolygonTest.h" #include "AudioOutputTest.h" #include "StructureScanComplexItemTest.h" +#include "QGCMapPolylineTest.h" +#include "CorridorScanComplexItemTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -67,6 +69,8 @@ UT_REGISTER_TEST(MissionSettingsTest) UT_REGISTER_TEST(QGCMapPolygonTest) UT_REGISTER_TEST(AudioOutputTest) UT_REGISTER_TEST(StructureScanComplexItemTest) +UT_REGISTER_TEST(CorridorScanComplexItemTest) +UT_REGISTER_TEST(QGCMapPolylineTest) // List of unit test which are currently disabled. // If disabling a new test, include reason in comment.