From ed0f21c9767602ab88be126048a0c6abb9d03739 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Mon, 23 Dec 2019 09:52:57 -0800 Subject: [PATCH] Add support for incomplete segment visuals --- src/MissionManager/ComplexMissionItem.h | 12 ++- .../FixedWingLandingComplexItem.cc | 1 + src/MissionManager/MissionController.cc | 100 ++++++++++++------ src/MissionManager/MissionController.h | 32 +++--- .../StructureScanComplexItem.cc | 10 +- src/MissionManager/StructureScanComplexItem.h | 3 +- .../TransectStyleComplexItem.cc | 6 ++ src/PlanView/PlanView.qml | 13 +++ 8 files changed, 119 insertions(+), 58 deletions(-) diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 49de903ad..4fad5041c 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -24,9 +24,10 @@ public: const ComplexMissionItem& operator=(const ComplexMissionItem& other); - Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) - Q_PROPERTY(bool presetsSupported READ presetsSupported CONSTANT) - Q_PROPERTY(QStringList presetNames READ presetNames NOTIFY presetNamesChanged) + Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) + Q_PROPERTY(bool presetsSupported READ presetsSupported CONSTANT) + Q_PROPERTY(QStringList presetNames READ presetNames NOTIFY presetNamesChanged) + Q_PROPERTY(bool isIncomplete READ isIncomplete NOTIFY isIncompleteChanged) /// @return The distance covered the complex mission item in meters. /// Signals complexDistanceChanged @@ -64,7 +65,8 @@ public: /// Empty string signals no support for presets. virtual QString presetsSettingsGroup(void) { return QString(); } - bool presetsSupported(void) { return !presetsSettingsGroup().isEmpty(); } + bool presetsSupported (void) { return !presetsSettingsGroup().isEmpty(); } + bool isIncomplete (void) const { return _isIncomplete; } /// This mission item attribute specifies the type of the complex item. static const char* jsonComplexItemTypeKey; @@ -74,11 +76,13 @@ signals: void boundingCubeChanged (void); void greatestDistanceToChanged (void); void presetNamesChanged (void); + void isIncompleteChanged (void); protected: void _savePresetJson (const QString& name, QJsonObject& presetObject); QJsonObject _loadPresetJson (const QString& name); + bool _isIncomplete = true; QMap _metaDataMap; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index e9f9f1704..b206d2699 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -66,6 +66,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool , _altitudesAreRelative (true) { _editorQml = "qrc:/qml/FWLandingPatternEditor.qml"; + _isIncomplete = false; connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 48568e781..10e4822ad 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1139,6 +1139,26 @@ double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, Vi return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0; } +CoordinateVector* MissionController::_createCoordinateVectorWorker(VisualItemPair& pair) +{ + CoordinateVector* coordVector = nullptr; + + // Create a new segment and wire update notifiers + coordVector = new CoordinateVector(pair.first->isSimpleItem() ? pair.first->coordinate() : pair.first->exitCoordinate(), pair.second->coordinate(), this); + auto originNotifier = pair.first->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged; + auto endNotifier = &VisualMissionItem::coordinateChanged; + + // Use signals/slots to update the coordinate endpoints + connect(pair.first, originNotifier, coordVector, &CoordinateVector::setCoordinate1); + connect(pair.second, endNotifier, coordVector, &CoordinateVector::setCoordinate2); + + // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change + // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes + connect(pair.second, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus); + + return coordVector; +} + CoordinateVector* MissionController::_addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair) { CoordinateVector* coordVector = nullptr; @@ -1147,18 +1167,7 @@ CoordinateVector* MissionController::_addWaypointLineSegment(CoordVectHashTable& // Pair already exists and connected, just re-use _linesTable[pair] = coordVector = prevItemPairHashTable.take(pair); } else { - // Create a new segment and wire update notifiers - coordVector = new CoordinateVector(pair.first->isSimpleItem() ? pair.first->coordinate() : pair.first->exitCoordinate(), pair.second->coordinate(), this); - auto originNotifier = pair.first->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged; - auto endNotifier = &VisualMissionItem::coordinateChanged; - - // Use signals/slots to update the coordinate endpoints - connect(pair.first, originNotifier, coordVector, &CoordinateVector::setCoordinate1); - connect(pair.second, endNotifier, coordVector, &CoordinateVector::setCoordinate2); - - // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change - // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes - connect(pair.second, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus); + coordVector = _createCoordinateVectorWorker(pair); _linesTable[pair] = coordVector; } @@ -1209,6 +1218,8 @@ void MissionController::_recalcWaypointLines(void) bool foundRTL = false; bool homePositionValid = _settingsItem->coordinate().isValid(); bool roiActive = false; + bool setupIncompleteItem = false; + VisualMissionItem* startVIForIncompleteItem = nullptr; qCDebug(MissionControllerLog) << "_recalcWaypointLines homePositionValid" << homePositionValid; @@ -1219,15 +1230,18 @@ void MissionController::_recalcWaypointLines(void) _waypointLines.beginReset(); _directionArrows.beginReset(); + _incompleteComplexItemLines.beginReset(); _waypointLines.clear(); _directionArrows.clear(); + _incompleteComplexItemLines.clearAndDeleteContents(); // Grovel through the list of items keeping track of things needed to correctly draw waypoints lines for (int i=1; i<_visualItems->count(); i++) { - VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); - SimpleMissionItem* simpleItem = qobject_cast(visualItem); + VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); + SimpleMissionItem* simpleItem = qobject_cast(visualItem); + ComplexMissionItem* complexItem = qobject_cast(visualItem); if (simpleItem) { if (roiActive) { @@ -1267,29 +1281,47 @@ void MissionController::_recalcWaypointLines(void) } if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) { - if (lastCoordinateItemBeforeRTL != _settingsItem || (homePositionValid && linkStartToHome)) { - // Direction arrows are added to the first segment and every 5 segments in the middle. - bool addDirectionArrow = false; - if (firstCoordinateNotFound || !lastCoordinateItemBeforeRTL->isSimpleItem() || !visualItem->isSimpleItem()) { - addDirectionArrow = true; - } else if (segmentCount > 5) { - segmentCount = 0; - addDirectionArrow = true; + // Incomplete items are complex items which are waiting for the user to complete setup before there visuals can become valid. + // For example a Survey which has no polygon set for it yet. For these cases we draw incomplete segment lines so that there + // isn't a hole in the flight path lines. + if (complexItem && complexItem->isIncomplete()) { + setupIncompleteItem = true; + } else { + if (setupIncompleteItem) { + VisualItemPair viPair(startVIForIncompleteItem, visualItem); + CoordinateVector* coordVector = _createCoordinateVectorWorker(viPair); + + _incompleteComplexItemLines.append(coordVector); + startVIForIncompleteItem = nullptr; + setupIncompleteItem = false; + } else { + startVIForIncompleteItem = visualItem; } - segmentCount++; - - lastSegmentVisualItemPair = VisualItemPair(lastCoordinateItemBeforeRTL, visualItem); - if (!_flyView || addDirectionArrow) { - CoordinateVector* coordVector = _addWaypointLineSegment(old_table, lastSegmentVisualItemPair); - coordVector->setSpecialVisual(roiActive); - if (addDirectionArrow) { - _directionArrows.append(coordVector); + + if (lastCoordinateItemBeforeRTL != _settingsItem || (homePositionValid && linkStartToHome)) { + // Direction arrows are added to the first segment and every 5 segments in the middle. + bool addDirectionArrow = false; + if (firstCoordinateNotFound || !lastCoordinateItemBeforeRTL->isSimpleItem() || !visualItem->isSimpleItem()) { + addDirectionArrow = true; + } else if (segmentCount > 5) { + segmentCount = 0; + addDirectionArrow = true; + } + segmentCount++; + + lastSegmentVisualItemPair = VisualItemPair(lastCoordinateItemBeforeRTL, visualItem); + if (!_flyView || addDirectionArrow) { + CoordinateVector* coordVector = _addWaypointLineSegment(old_table, lastSegmentVisualItemPair); + coordVector->setSpecialVisual(roiActive); + if (addDirectionArrow) { + _directionArrows.append(coordVector); + } } } + firstCoordinateNotFound = false; + _waypointPath.append(QVariant::fromValue(visualItem->coordinate())); + lastCoordinateItemBeforeRTL = visualItem; } - firstCoordinateNotFound = false; - _waypointPath.append(QVariant::fromValue(visualItem->coordinate())); - lastCoordinateItemBeforeRTL = visualItem; } } @@ -1342,6 +1374,7 @@ void MissionController::_recalcWaypointLines(void) _waypointLines.endReset(); _directionArrows.endReset(); + _incompleteComplexItemLines.endReset(); // Anything left in the old table is an obsolete line object that can go qDeleteAll(old_table); @@ -1852,6 +1885,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) if (complexItem) { connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus); connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(complexItem, &ComplexMissionItem::isIncompleteChanged, this, &MissionController::_recalcWaypointLines); } else { qWarning() << "ComplexMissionItem not found"; } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 0d5b28f9a..67664d461 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -70,6 +70,7 @@ public: Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines CONSTANT) ///< Used by Plan view only for interactive editing Q_PROPERTY(QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged) ///< Used by Fly view only for static display Q_PROPERTY(QmlObjectListModel* directionArrows READ directionArrows CONSTANT) + Q_PROPERTY(QmlObjectListModel* incompleteComplexItemLines READ incompleteComplexItemLines CONSTANT) ///< Segments which are not yet completed. Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged) Q_PROPERTY(QGeoCoordinate plannedHomePosition READ plannedHomePosition NOTIFY plannedHomePositionChanged) Q_PROPERTY(QGeoCoordinate previousCoordinate MEMBER _previousCoordinate NOTIFY previousCoordinateChanged) @@ -200,6 +201,7 @@ public: QmlObjectListModel* visualItems (void) { return _visualItems; } QmlObjectListModel* waypointLines (void) { return &_waypointLines; } QmlObjectListModel* directionArrows (void) { return &_directionArrows; } + QmlObjectListModel* incompleteComplexItemLines (void) { return &_incompleteComplexItemLines; } QVariantList waypointPath (void) { return _waypointPath; } QStringList complexMissionItemNames (void) const; QGeoCoordinate plannedHomePosition (void) const; @@ -270,20 +272,20 @@ signals: void previousCoordinateChanged (void); private slots: - void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); - void _itemCommandChanged(void); - void _inProgressChanged(bool inProgress); - void _currentMissionIndexChanged(int sequenceNumber); - void _recalcWaypointLines(void); - void _recalcMissionFlightStatus(void); - void _updateContainsItems(void); - void _progressPctChanged(double progressPct); - void _visualItemsDirtyChanged(bool dirty); - void _managerSendComplete(bool error); - void _managerRemoveAllComplete(bool error); - void _updateTimeout(); - void _complexBoundingBoxChanged(); - void _recalcAll(void); + void _newMissionItemsAvailableFromVehicle (bool removeAllRequested); + void _itemCommandChanged (void); + void _inProgressChanged (bool inProgress); + void _currentMissionIndexChanged (int sequenceNumber); + void _recalcWaypointLines (void); + void _recalcMissionFlightStatus (void); + void _updateContainsItems (void); + void _progressPctChanged (double progressPct); + void _visualItemsDirtyChanged (bool dirty); + void _managerSendComplete (bool error); + void _managerRemoveAllComplete (bool error); + void _updateTimeout (void); + void _complexBoundingBoxChanged (void); + void _recalcAll (void); private: void _init(void); @@ -324,6 +326,7 @@ private: void _warnIfTerrainFrameUsed(void); bool _isROIBeginItem(SimpleMissionItem* simpleItem); bool _isROICancelItem(SimpleMissionItem* simpleItem); + CoordinateVector* _createCoordinateVectorWorker(VisualItemPair& pair); private: MissionManager* _missionManager; @@ -333,6 +336,7 @@ private: QmlObjectListModel _waypointLines; QVariantList _waypointPath; QmlObjectListModel _directionArrows; + QmlObjectListModel _incompleteComplexItemLines; CoordVectHashTable _linesTable; bool _firstItemsFromVehicle; bool _itemsRequested; diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 51d6a261a..fe92950a1 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -264,6 +264,11 @@ void StructureScanComplexItem::_flightPathChanged(void) emit coordinateChanged(coordinate()); emit exitCoordinateChanged(exitCoordinate()); emit greatestDistanceToChanged(); + + if (_isIncomplete) { + _isIncomplete = false; + emit isIncompleteChanged(); + } } double StructureScanComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const @@ -282,11 +287,6 @@ double StructureScanComplexItem::greatestDistanceTo(const QGeoCoordinate &other) return greatestDistance; } -bool StructureScanComplexItem::specifiesCoordinate(void) const -{ - return _flightPolygon.count() > 2; -} - void StructureScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) { int seqNum = _sequenceNumber; diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index 4d0359bc8..80c4d0c4d 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -73,7 +73,7 @@ public: 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 specifiesCoordinate (void) const final { return true; } bool specifiesAltitudeOnly (void) const final { return false; } QString commandDescription (void) const final { return tr("Structure Scan"); } QString commandName (void) const final { return tr("Structure Scan"); } @@ -129,7 +129,6 @@ private slots: void _recalcScanDistance (void); private: - void _setExitCoordinate(const QGeoCoordinate& coordinate); void _setCameraShots(int cameraShots); double _triggerDistance(void) const; diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 308d5c835..5c3742ba9 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -230,6 +230,7 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, bool forP } _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); + _isIncomplete = false; // Load generated mission items _loadedMissionItemsParent = new QObject(this); @@ -408,6 +409,11 @@ void TransectStyleComplexItem::_rebuildTransects(void) emit coordinateChanged(_coordinate); emit exitCoordinateChanged(_exitCoordinate); + if (_isIncomplete) { + _isIncomplete = false; + emit isIncompleteChanged(); + } + _recalcComplexDistance(); _recalcCameraShots(); diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index d6ffba86a..489bfd4f7 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -434,6 +434,7 @@ Item { model: _editingLayer == _layerMission ? _missionController.waypointLines : undefined } + // Direction arrows in waypoint lines MapItemView { model: _editingLayer == _layerMission ? _missionController.directionArrows : undefined @@ -445,6 +446,18 @@ Item { } } + // Incomplete segment lines + MapItemView { + model: _editingLayer == _layerMission ? _missionController.incompleteComplexItemLines : undefined + + delegate: MapPolyline { + path: [ object.coordinate1, object.coordinate2 ] + line.width: 1 + line.color: "red" + z: QGroundControl.zOrderWaypointLines + } + } + // UI for splitting the current segment MapQuickItem { id: splitSegmentItem -- 2.22.0