diff --git a/Paths/KlingenbachTest.wima b/Paths/KlingenbachTest.wima index 1cb19f361b42ca2f5f7de8d5390726c697b17df4..7be92fc6087fddb023aa5524b3e2abd63ea5884e 100644 --- a/Paths/KlingenbachTest.wima +++ b/Paths/KlingenbachTest.wima @@ -100,10 +100,12 @@ { "Alpha": 23, "MinLength": 5, + "NumRuns": 1, "ReferencePointAlt": 0, "ReferencePointLat": 47.76807182520187, "ReferencePointLong": 16.530610531894183, - "TransectDistance": 10, + "Run": 1, + "TransectDistance": 2, "TransectStyleComplexItem": { "CameraCalc": { "AdjustedFootprintFrontal": 25, @@ -128,8 +130,8 @@ 0, 0, null, - 47.7677958621741, - 16.53051039683979, + 47.76782117293054, + 16.53045952207802, 15 ], "type": "SimpleItem" @@ -144,8 +146,8 @@ 0, 0, null, - 47.76795620865596, - 16.530658248522045, + 47.76801762181985, + 16.531145995440625, 15 ], "type": "SimpleItem" @@ -160,8 +162,8 @@ 0, 0, null, - 47.768085758380195, - 16.53111095084737, + 47.76812651838593, + 16.53090512987649, 15 ], "type": "SimpleItem" @@ -176,8 +178,8 @@ 0, 0, null, - 47.768170929071175, - 16.531067144987073, + 47.768125250928335, + 16.530839239105784, 15 ], "type": "SimpleItem" @@ -192,8 +194,8 @@ 0, 0, null, - 47.76812500283788, - 16.530906658047133, + 47.768187963203616, + 16.531058383792516, 15 ], "type": "SimpleItem" @@ -208,8 +210,8 @@ 0, 0, null, - 47.768170929071175, - 16.531067144987073, + 47.768204997344334, + 16.531049622605572, 15 ], "type": "SimpleItem" @@ -224,8 +226,8 @@ 0, 0, null, - 47.76811004588309, - 16.530512964621895, + 47.76812144975848, + 16.530757670470322, 15 ], "type": "SimpleItem" @@ -235,6 +237,70 @@ "command": 16, "doJumpId": 9, "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811764852164, + 16.530676101820056, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768113847226864, + 16.530594533208365, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, "params": [ 0, 0, @@ -249,7 +315,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 10, + "doJumpId": 14, "frame": 3, "params": [ 0, @@ -265,7 +331,71 @@ { "autoContinue": true, "command": 16, - "doJumpId": 11, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810624447235, + 16.53043139603398, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568510607, + 16.530399305509537, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, "frame": 3, "params": [ 0, @@ -281,7 +411,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 12, + "doJumpId": 20, "frame": 3, "params": [ 0, @@ -297,15 +427,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 13, + "doJumpId": 21, "frame": 3, "params": [ 0, 0, 0, null, - 47.76834127040819, - 16.53097953282404, + 47.76831125125834, + 16.530806346203697, 15 ], "type": "SimpleItem" @@ -313,7 +443,247 @@ { "autoContinue": true, "command": 16, - "doJumpId": 14, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809864148597, + 16.530268258893802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768102443012644, + 16.530349827431266, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809124406109, + 16.53085697602152, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805748523563, + 16.53080729404687, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802372639744, + 16.53075761214986, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798996754654, + 16.530707930317146, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, "frame": 3, "params": [ 0, @@ -329,15 +699,111 @@ { "autoContinue": true, "command": 16, - "doJumpId": 15, + "doJumpId": 38, "frame": 3, "params": [ 0, 0, 0, null, - 47.7677958621741, - 16.53051039683979, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792244976166, + 16.53060856681793, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788869084566, + 16.53055888517811, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767854931889964, + 16.530509203589244, 15 ], "type": "SimpleItem" @@ -347,32 +813,48 @@ "TurnAroundDistance": 10, "VisualTransectPoints": [ [ - 47.7677958621741, - 16.53051039683979 + 47.76782117293054, + 16.53045952207802 ], [ - 47.76795620865596, - 16.530658248522045 + 47.76801762181985, + 16.531145995440625 ], [ - 47.768085758380195, - 16.53111095084737 + 47.76812651838593, + 16.53090512987649 ], [ - 47.768170929071175, - 16.531067144987073 + 47.768125250928335, + 16.530839239105784 ], [ - 47.76812500283788, - 16.530906658047133 + 47.768187963203616, + 16.531058383792516 ], [ - 47.768170929071175, - 16.531067144987073 + 47.768204997344334, + 16.531049622605572 ], [ - 47.76811004588309, - 16.530512964621895 + 47.76812144975848, + 16.530757670470322 + ], + [ + 47.76811764852164, + 16.530676101820056 + ], + [ + 47.768222031484335, + 16.53104086139957 + ], + [ + 47.768239065614615, + 16.53103210018785 + ], + [ + 47.768113847226864, + 16.530594533208365 ], [ 47.76811004588309, @@ -382,6 +864,22 @@ 47.76825609974418, 16.531023338983744 ], + [ + 47.76827313388202, + 16.53101457776058 + ], + [ + 47.76810624447235, + 16.53043139603398 + ], + [ + 47.76815568510607, + 16.530399305509537 + ], + [ + 47.76832423627323, + 16.530988294070095 + ], [ 47.76834127040819, 16.53097953282404 @@ -391,21 +889,106 @@ 16.530564330637993 ], [ - 47.76834127040819, - 16.53097953282404 + 47.76831125125834, + 16.530806346203697 + ], + [ + 47.768358304533436, + 16.530970771572267 + ], + [ + 47.768307202146545, + 16.53099705529708 + ], + [ + 47.76809864148597, + 16.530268258893802 + ], + [ + 47.768102443012644, + 16.530349827431266 + ], + [ + 47.76829016801015, + 16.531005816531692 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76812500283788, + 16.530906658047133 + ], + [ + 47.76809124406109, + 16.53085697602152 + ], + [ + 47.76815389493801, + 16.53107590617591 + ], + [ + 47.768136860795146, + 16.531084667345684 + ], + [ + 47.76805748523563, + 16.53080729404687 + ], + [ + 47.76802372639744, + 16.53075761214986 + ], + [ + 47.76811982666054, + 16.531093428523086 + ], + [ + 47.76810279252523, + 16.53110218969476 + ], + [ + 47.76798996754654, + 16.530707930317146 ], [ 47.76795620865596, 16.530658248522045 ], [ - 47.7677958621741, - 16.53051039683979 + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.76806872423443, + 16.5311197120076 + ], + [ + 47.76792244976166, + 16.53060856681793 + ], + [ + 47.76788869084566, + 16.53055888517811 + ], + [ + 47.768051690105956, + 16.531128473148772 + ], + [ + 47.76803465596775, + 16.53113723429756 + ], + [ + 47.767854931889964, + 16.530509203589244 ] ], "version": 1 }, "Type": 1, + "Variant": 0, "complexItemType": "CircularSurvey", "polygon": [ [ @@ -442,7 +1025,7 @@ "AltitudeMode": 1, "autoContinue": true, "command": 21, - "doJumpId": 19, + "doJumpId": 48, "frame": 3, "params": [ 0, diff --git a/src/PlanView/CircularSurveyItemEditor.qml b/src/PlanView/CircularSurveyItemEditor.qml index 2e8f55aac97b5971848ecc70948f5bc683aa5c64..ad57b3c6b14d2c2fd5351fd5414f656c892b75db 100644 --- a/src/PlanView/CircularSurveyItemEditor.qml +++ b/src/PlanView/CircularSurveyItemEditor.qml @@ -130,7 +130,7 @@ Rectangle { columnSpacing: _margin rowSpacing: _margin - columns: 4 + columns: 6 Repeater{ id: variantRepeater @@ -141,9 +141,9 @@ Rectangle { property int len: missionItem.variantNames.length model: len - QGCRadioButton { + delegate: QGCRadioButton { checked: index === variantRepeater.variant - text: variantRepeater.names[index] + text: variantRepeater.names[index] onCheckedChanged: { if (checked){ @@ -154,6 +154,44 @@ Rectangle { } } } + + + QGCLabel { + text: qsTr("Runs") + } + FactTextField { + fact: missionItem.numRuns + } + + GridLayout{ + Layout.columnSpan: 2 + + columnSpacing: _margin + rowSpacing: _margin + columns: 6 + + Repeater{ + id: runRepeater + + property var fact: missionItem.run + property int run: fact.value + property var names: missionItem.runNames + property int len: missionItem.runNames.length + + model: len > 1 ? len : 0 + QGCRadioButton { + checked: index === runRepeater.run + text: runRepeater.names[index] + + onCheckedChanged: { + if (checked){ + missionItem.run.value = index + } + checked = Qt.binding(function(){ return index === runRepeater.run}) + } + } + } + } } SectionHeader { @@ -171,8 +209,8 @@ Rectangle { QGCLabel { text: qsTr("Distance") } FactTextField { - fact: missionItem.transectDistance - Layout.fillWidth: true + fact: missionItem.transectDistance + Layout.fillWidth: true } /*QGCSlider { diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index cf45fe6e8a42973be449ae230b97c9f9d38e03f4..40387c2e2e8605e76fdbf5dec49a2f9ef662a5d3 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -50,6 +50,8 @@ const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat"; const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong"; const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt"; const char *CircularSurvey::variantName = "Variant"; +const char *CircularSurvey::numRunsName = "NumRuns"; +const char *CircularSurvey::runName = "Run"; CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent) @@ -62,6 +64,8 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, _minLength(settingsGroup, _metaDataMap[minLengthName]), _type(settingsGroup, _metaDataMap[typeName]), _variant(settingsGroup, _metaDataMap[variantName]), + _numRuns(settingsGroup, _metaDataMap[numRunsName]), + _run(settingsGroup, _metaDataMap[runName]), _pWorker(std::make_unique()), _state(STATE::DEFAULT), _hidePolygon(false) { Q_UNUSED(kmlOrShpFile) @@ -84,6 +88,11 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, &CircularSurvey::_rebuildTransects); connect(&this->_variant, &Fact::rawValueChanged, this, &CircularSurvey::_changeVariant); + connect(&this->_run, &Fact::rawValueChanged, this, + &CircularSurvey::_changeRun); + connect(&this->_numRuns, &Fact::rawValueChanged, this, + &CircularSurvey::_rebuildTransects); + // Connect worker. connect(this->_pWorker.get(), &RoutingThread::result, this, &CircularSurvey::_setTransects); @@ -122,6 +131,8 @@ bool CircularSurvey::hidePolygon() const { return _hidePolygon; } QList CircularSurvey::variantNames() const { return _variantNames; } +QList CircularSurvey::runNames() const { return _runNames; } + QGeoCoordinate CircularSurvey::depot() const { return this->_depot; } QList CircularSurvey::safeArea() const { @@ -180,6 +191,8 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, {minLengthName, QJsonValue::Double, true}, {typeName, QJsonValue::Double, true}, {variantName, QJsonValue::Double, false}, + {numRunsName, QJsonValue::Double, false}, + {runName, QJsonValue::Double, false}, {refPointLatitudeName, QJsonValue::Double, true}, {refPointLongitudeName, QJsonValue::Double, true}, {refPointAltitudeName, QJsonValue::Double, true}, @@ -222,6 +235,8 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, _minLength.setRawValue(complexObject[minLengthName].toDouble()); _type.setRawValue(complexObject[typeName].toInt()); _variant.setRawValue(complexObject[variantName].toInt()); + _numRuns.setRawValue(complexObject[numRunsName].toInt()); + _run.setRawValue(complexObject[runName].toInt()); _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble()); _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble()); _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble()); @@ -256,6 +271,8 @@ void CircularSurvey::save(QJsonArray &planItems) { saveObject[minLengthName] = _minLength.rawValue().toDouble(); saveObject[typeName] = double(_type.rawValue().toUInt()); saveObject[variantName] = double(_variant.rawValue().toUInt()); + saveObject[numRunsName] = double(_numRuns.rawValue().toUInt()); + saveObject[runName] = double(_numRuns.rawValue().toUInt()); saveObject[refPointLongitudeName] = _referencePoint.longitude(); saveObject[refPointLatitudeName] = _referencePoint.latitude(); saveObject[refPointAltitudeName] = _referencePoint.altitude(); @@ -335,13 +352,20 @@ void CircularSurvey::_changeVariant() { this->_rebuildTransects(); } +void CircularSurvey::_changeRun() { + this->_state = STATE::RUN_CHANGE; + this->_rebuildTransects(); +} + void CircularSurvey::_updateWorker() { // Reset data. this->_transects.clear(); this->_rawTransects.clear(); - this->_routes.clear(); + this->_variantVector.clear(); this->_variantNames.clear(); + this->_runNames.clear(); emit variantNamesChanged(); + emit runNamesChanged(); // Prepare data. auto ref = this->_referencePoint; @@ -354,8 +378,22 @@ void CircularSurvey::_updateWorker() { v.setAltitude(0); } auto depot = this->_depot; + + // Routing par. RoutingParameter par; par.numSolutionsPerRun = 5; + if (this->_numRuns.rawValue().toUInt() < 1) { + disconnect(&this->_numRuns, &Fact::rawValueChanged, this, + &CircularSurvey::_rebuildTransects); + + this->_numRuns.setCookedValue(QVariant(1)); + + connect(&this->_numRuns, &Fact::rawValueChanged, this, + &CircularSurvey::_rebuildTransects); + } + par.numRuns = this->_numRuns.rawValue().toUInt(); + + // Convert safe area. auto &safeAreaENU = par.safeArea; bool useDepot = false; if (this->_depot.isValid() && this->_safeArea.size() >= 3) { @@ -364,6 +402,8 @@ void CircularSurvey::_updateWorker() { } else { snake::areaToEnu(ref, polygon, safeAreaENU); } + + // Fetch transect parameter. auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() * bu::si::meter); auto minLength = @@ -407,24 +447,82 @@ void CircularSurvey::_updateWorker() { this->_transectsDirty = true; } -void CircularSurvey::_changeVariantWorker() { +void CircularSurvey::_changeVariantRunWorker() { auto variant = this->_variant.rawValue().toUInt(); - // Find old route variant. Old variant corresponts with empty list. - std::size_t old_variant = 0; - for (std::size_t i = 0; i < std::size_t(this->_routes.size()); ++i) { - const auto &r = this->_routes[i]; - if (r.isEmpty()) { - old_variant = i; - break; + auto run = this->_run.rawValue().toUInt(); + + // Find old variant and run. Old run corresponts with empty list. + std::size_t old_variant = std::numeric_limits::max(); + std::size_t old_run = std::numeric_limits::max(); + for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) { + const auto &solution = this->_variantVector.at(i); + for (std::size_t j = 0; j < std::size_t(solution.size()); ++j) { + const auto &r = solution[j]; + if (r.isEmpty()) { + old_variant = i; + old_run = j; + // break + i = std::numeric_limits::max() - 1; + j = std::numeric_limits::max() - 1; + } } } - // Swap in new variant, if condition. - if (variant < std::size_t(this->_routes.size()) && - old_variant < std::size_t(this->_routes.size()) && - variant != old_variant) { - this->_routes[old_variant].swap(this->_transects); - this->_transects.swap(this->_routes[variant]); + // Swap route. + if (variant != old_variant || run != old_run) { + // Swap in new variant, if condition. + if (variant < std::size_t(this->_variantVector.size()) && + run < std::size_t(this->_variantVector.at(variant).size())) { + if (old_variant != std::numeric_limits::max()) { + // this->_transects containes a route, swap it back to + // this->_solutionVector + auto &old_solution = this->_variantVector[old_variant]; + auto &old_route = old_solution[old_run]; + old_route.swap(this->_transects); + } + auto &solution = this->_variantVector[variant]; + auto &route = solution[run]; + this->_transects.swap(route); + + if (variant != old_variant) { + // Add run names. + this->_runNames.clear(); + for (std::size_t i = 1; i <= std::size_t(solution.size()); ++i) { + this->_runNames.append(QString::number(i)); + } + emit runNamesChanged(); + } + + } else { // error + qWarning() << "Variant or run out of bounds (variant = " << variant + << ", run = " << run << ")."; + qWarning() << "Resetting variant and run."; + + disconnect(&this->_variant, &Fact::rawValueChanged, this, + &CircularSurvey::_changeVariant); + disconnect(&this->_run, &Fact::rawValueChanged, this, + &CircularSurvey::_changeRun); + if (old_variant < std::size_t(this->_variantVector.size())) { + this->_variant.setCookedValue(QVariant::fromValue(old_variant)); + auto &solution = this->_variantVector[old_variant]; + if (old_run < std::size_t(solution.size())) { + this->_run.setCookedValue(QVariant::fromValue(old_run)); + } else { + this->_run.setCookedValue(QVariant(0)); + } + } else { + this->_variant.setCookedValue(QVariant(0)); + this->_run.setCookedValue(QVariant(0)); + } + connect(&this->_variant, &Fact::rawValueChanged, this, + &CircularSurvey::_changeVariant); + connect(&this->_run, &Fact::rawValueChanged, this, + &CircularSurvey::_changeRun); + if (this->_variantVector.size() > 0 && + this->_variantVector.front().size() > 0) { + this->_changeVariantRunWorker(); + } + } } } @@ -451,7 +549,7 @@ void CircularSurvey::_storeWorker() { } // Store raw transects. - const auto &pRoutingData = this->_workerOutput; + const auto &pRoutingData = this->_pRoutingData; const auto &ori = this->_referencePoint; const auto &transectsENU = pRoutingData->transects; std::size_t startIdx = 0; @@ -472,91 +570,121 @@ void CircularSurvey::_storeWorker() { } } - // Store routes. - auto nRoutes = pRoutingData->routeVector.size(); - QVector routes(nRoutes, Transects{QList()}); - for (std::size_t k = 0; k < nRoutes; ++k) { - const auto &transectsInfo = pRoutingData->routeInfoVector.at(k); - if (transectsInfo.size() > 1) { - const auto &route = pRoutingData->routeVector.at(k); - - // Find index of first waypoint. - std::size_t idxFirst = 0; - const auto &infoFirst = - depotValid ? transectsInfo.at(1) : transectsInfo.at(0); - const auto &firstTransect = transectsENU[infoFirst.index]; - if (firstTransect.size() > 0) { - const auto &firstWaypoint = - infoFirst.reversed ? firstTransect.back() : firstTransect.front(); - double th = 0.01; - for (std::size_t i = 0; i < route.size(); ++i) { - auto dist = bg::distance(route[i], firstWaypoint); - if (dist < th) { - idxFirst = i; - break; - } - } - - // Find index of last waypoint. - std::size_t idxLast = route.size() - 1; - const auto &infoLast = transectsInfo.at(transectsInfo.size() - 2); - const auto &lastTransect = transectsENU[infoLast.index]; - if (lastTransect.size() > 0) { - const auto &lastWaypoint = - infoLast.reversed ? lastTransect.front() : lastTransect.back(); - for (long i = route.size() - 1; i >= 0; --i) { - auto dist = bg::distance(route[i], lastWaypoint); + // Store solutions. + QVector solutionVector; + const auto nSolutions = pRoutingData->solutionVector.size(); + for (std::size_t j = 0; j < nSolutions; ++j) { + const auto &solution = pRoutingData->solutionVector.at(j); + const auto nRuns = solution.size(); + // Store runs. + Runs runs(nRuns, Transects{QList()}); + for (std::size_t k = 0; k < nRuns; ++k) { + const auto &route = solution.at(k); + const auto &path = route.path; + const auto &info = route.info; + if (info.size() > 1) { + // Find index of first waypoint. + std::size_t idxFirst = 0; + const auto &infoFirst = depotValid ? info.at(1) : info.at(0); + const auto &firstTransect = transectsENU[infoFirst.index]; + if (firstTransect.size() > 0) { + const auto &firstWaypoint = + infoFirst.reversed ? firstTransect.back() : firstTransect.front(); + double th = 0.01; + for (std::size_t i = 0; i < path.size(); ++i) { + auto dist = bg::distance(path[i], firstWaypoint); if (dist < th) { - idxLast = i; + idxFirst = i; break; } } - // Convert to geo coordinates. - auto &list = routes[k].front(); - for (std::size_t i = idxFirst; i <= idxLast; ++i) { - auto &vertex = route[i]; - QGeoCoordinate c; - snake::fromENU(ori, vertex, c); - list.append(CoordInfo_t{c, CoordTypeInterior}); + // Find index of last waypoint. + std::size_t idxLast = path.size() - 1; + const auto &infoLast = info.at(info.size() - 2); + const auto &lastTransect = transectsENU[infoLast.index]; + if (lastTransect.size() > 0) { + const auto &lastWaypoint = + infoLast.reversed ? lastTransect.front() : lastTransect.back(); + for (long i = path.size() - 1; i >= 0; --i) { + auto dist = bg::distance(path[i], lastWaypoint); + if (dist < th) { + idxLast = i; + break; + } + } + + // Convert to geo coordinates. + auto &list = runs[k].front(); + for (std::size_t i = idxFirst; i <= idxLast; ++i) { + auto &vertex = path[i]; + QGeoCoordinate c; + snake::fromENU(ori, vertex, c); + list.append(CoordInfo_t{c, CoordTypeInterior}); + } + } else { + qWarning() << "CS::_storeWorker(): lastTransect.size() == 0"; } } else { - qWarning() << "CS::_storeWorker(): lastTransect.size() == 0"; + qWarning() << "CS::_storeWorker(): firstTransect.size() == 0"; } } else { - qWarning() << "CS::_storeWorker(): firstTransect.size() == 0"; + qWarning() << "CS::_storeWorker(): transectsInfo.size() <= 1"; + } + } + // Remove empty runs. + bool error = true; + for (auto it = runs.begin(); it < runs.end();) { + if (it->size() > 0 && it->front().size() > 0) { + error = false; + ++it; + } else { + it = runs.erase(it); } - } else { - qWarning() << "CS::_storeWorker(): transectsInfo.size() <= 1"; + } + if (!error) { + solutionVector.push_back(std::move(runs)); } } - // Remove empty routes. - bool error = true; - std::size_t n = 0; - for (auto it = routes.begin(); it < routes.end();) { + // Remove empty solutions. + std::size_t nSol = 0; + for (auto it = solutionVector.begin(); it < solutionVector.end();) { if (it->size() > 0 && it->front().size() > 0) { - error = false; ++it; - ++n; + ++nSol; } else { - it = routes.erase(it); + it = solutionVector.erase(it); } } // Assign routes if no error occured. - if (!error) { + if (nSol > 0) { // Swap first route to _transects. - this->_transects.swap(routes.front()); - this->_routes.swap(routes); + this->_variantVector.swap(solutionVector); + // Add route variant names. - for (std::size_t i = 1; i <= n; ++i) { + this->_variantNames.clear(); + for (std::size_t i = 1; i <= std::size_t(this->_variantVector.size()); + ++i) { this->_variantNames.append(QString::number(i)); } emit variantNamesChanged(); - this->_variant.setCookedValue(QVariant(0)); + // Swap in rawTransects. this->_rawTransects.swap(rawTransects); + + disconnect(&this->_variant, &Fact::rawValueChanged, this, + &CircularSurvey::_changeVariant); + disconnect(&this->_run, &Fact::rawValueChanged, this, + &CircularSurvey::_changeRun); + this->_variant.setCookedValue(QVariant(0)); + this->_run.setCookedValue(QVariant(0)); + connect(&this->_variant, &Fact::rawValueChanged, this, + &CircularSurvey::_changeVariant); + connect(&this->_run, &Fact::rawValueChanged, this, + &CircularSurvey::_changeRun); + this->_changeVariantRunWorker(); // Mark transect as stored and ready. this->_transectsDirty = false; } @@ -601,7 +729,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { #ifdef SHOW_CIRCULAR_SURVEY_TIME qWarning() << "CS::rebuildTransectsPhase1: variant change."; #endif - this->_changeVariantWorker(); + this->_changeVariantRunWorker(); + break; + case STATE::RUN_CHANGE: +#ifdef SHOW_CIRCULAR_SURVEY_TIME + qWarning() << "CS::rebuildTransectsPhase1: run change."; +#endif + this->_changeVariantRunWorker(); break; case STATE::REVERSE: #ifdef SHOW_CIRCULAR_SURVEY_TIME @@ -643,7 +777,7 @@ void CircularSurvey::_recalcComplexDistance() { void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; } void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) { - this->_workerOutput = pRoute; + this->_pRoutingData = pRoute; this->_state = STATE::STORE; this->_rebuildTransects(); } @@ -654,6 +788,10 @@ Fact *CircularSurvey::type() { return &_type; } Fact *CircularSurvey::variant() { return &_variant; } +Fact *CircularSurvey::numRuns() { return &_numRuns; } + +Fact *CircularSurvey::run() { return &_run; } + int CircularSurvey::typeCount() const { return int(integral(Type::Count)); } bool CircularSurvey::calculating() const { @@ -740,7 +878,8 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, //#ifdef DEBUG_CIRCULAR_SURVEY // qWarning() << "CS::circularTransects(): sector parameres:"; // qWarning() << "alpha1: " << - // to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: " + // to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: + // " // << to_string(snake::Degree(alpha2)).c_str(); qWarning() << "n: " // << to_string((alpha2 - alpha1) / deltaAlpha).c_str(); qWarning() // << "nSectors: " << nSectors; qWarning() << "rMin: " << diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index d391632fb89c3cc929a59b9667852fc5e40ed09a..85168d7c2ac349a86eb0382dcf172a1a26a2e41b 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -35,11 +35,14 @@ public: Q_PROPERTY(Fact *minLength READ minLength CONSTANT) Q_PROPERTY(Fact *type READ type CONSTANT) Q_PROPERTY(Fact *variant READ variant CONSTANT) + Q_PROPERTY(Fact *numRuns READ numRuns CONSTANT) + Q_PROPERTY(Fact *run READ run CONSTANT) Q_PROPERTY(int typeCount READ typeCount CONSTANT) Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged) Q_PROPERTY(bool hidePolygon READ hidePolygon NOTIFY hidePolygonChanged) Q_PROPERTY( QList variantNames READ variantNames NOTIFY variantNamesChanged) + Q_PROPERTY(QList runNames READ runNames NOTIFY runNamesChanged) Q_INVOKABLE void resetReference(void); Q_INVOKABLE void reverse(void); @@ -57,10 +60,13 @@ public: Fact *minLength(); Fact *type(); Fact *variant(); + Fact *numRuns(); + Fact *run(); int typeCount() const; bool calculating() const; bool hidePolygon() const; QList variantNames() const; + QList runNames() const; QGeoCoordinate depot() const; QList safeArea() const; const QList> &rawTransects() const; @@ -87,6 +93,8 @@ public: static const char *minLengthName; static const char *typeName; static const char *variantName; + static const char *numRunsName; + static const char *runName; static const char *CircularSurveyName; static const char *refPointLongitudeName; static const char *refPointLatitudeName; @@ -99,6 +107,7 @@ signals: void depotChanged(); void safeAreaChanged(); void variantNamesChanged(); + void runNamesChanged(); private slots: // Overrides from TransectStyleComplexItem @@ -113,10 +122,13 @@ private: void _buildAndAppendMissionItems(QList &items, QObject *missionItemParent); void _changeVariant(); + void _changeRun(); + void _updateWorker(); - void _changeVariantWorker(); + void _changeVariantRunWorker(); void _reverseWorker(); void _storeWorker(); + void _changeRunWorker(); // center of the circular lanes, e.g. base station QGeoCoordinate _referencePoint; @@ -131,22 +143,29 @@ private: SettingsFact _type; SettingsFact _variant; QList _variantNames; + SettingsFact _numRuns; + SettingsFact _run; + QList _runNames; // Worker using PtrWorker = std::shared_ptr; PtrWorker _pWorker; - PtrRoutingData _workerOutput; + PtrRoutingData _pRoutingData; - // Data and State. + // Routing data. QGeoCoordinate _depot; QList _safeArea; QList> _rawTransects; - QVector _routes; + using Runs = QVector; + QVector _variantVector; + + // State. enum class STATE { DEFAULT, STORE, REVERSE, VARIANT_CHANGE, + RUN_CHANGE, }; STATE _state; diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp index 37cf195e25d7d994e7f5bec6b0c41e8dc49eafb5..77e8f2127eb63a94129d9e8ba37a3b5f75e913bd 100644 --- a/src/Wima/RoutingThread.cpp +++ b/src/Wima/RoutingThread.cpp @@ -73,10 +73,13 @@ void RoutingThread::run() { #endif } else { // Prepare data for routing. - auto &routeInfoVector = pRouteData->routeInfoVector; - auto &routeVector = pRouteData->routeVector; + auto &solutionVector = pRouteData->solutionVector; + snake::RouteParameter snakePar; snakePar.numSolutionsPerRun = numSolutionsPerRun; + snakePar.numRuns = numRuns; + + // Set time limit to 10 min. const auto maxRoutingTime = std::chrono::minutes(10); const auto routingEnd = std::chrono::high_resolution_clock::now() + maxRoutingTime; @@ -87,11 +90,11 @@ void RoutingThread::run() { }; // Route transects. - bool success = snake::route(safeAreaENU, transectsENU, routeInfoVector, - routeVector, snakePar); + bool success = + snake::route(safeAreaENU, transectsENU, solutionVector, snakePar); // Check if routing was successful. - if ((!success || routeVector.size() < 1) && !this->_restart) { + if ((!success || solutionVector.size() < 1) && !this->_restart) { #ifdef DEBUG_CIRCULAR_SURVEY qWarning() << "RoutingWorker::run(): " "routing failed."; diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h index fed6b88e44e2509a107019448967c4045a600d3f..46ba2eb757aff864e10ad89c1bf4452e5cdfb41b 100644 --- a/src/Wima/RoutingThread.h +++ b/src/Wima/RoutingThread.h @@ -12,8 +12,7 @@ struct RoutingData { snake::Transects transects; - std::vector routeVector; - std::vector routeInfoVector; + std::vector solutionVector; std::string errorString; }; diff --git a/src/Wima/Snake/snake.cpp b/src/Wima/Snake/snake.cpp index 43891743f7d22c835a0c896eeb9e848feda006d7..94f3a7283e018e73adcac6cb17b6148d11d571a1 100644 --- a/src/Wima/Snake/snake.cpp +++ b/src/Wima/Snake/snake.cpp @@ -627,8 +627,7 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle, } bool route(const FPolygon &area, const Transects &transects, - std::vector &routeInfoVector, - std::vector &routeVector, const RouteParameter &par) { + std::vector &solutionVector, const RouteParameter &par) { #ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); @@ -797,9 +796,18 @@ bool route(const FPolygon &area, const Transects &transects, #endif // Create Routing Index Manager. - long numVehicles = 1; + auto minNumTransectsPerRun = + std::max(1, par.minNumTransectsPerRun); + auto maxRuns = std::max( + 1, std::floor(double(transects.size()) / minNumTransectsPerRun)); + auto numRuns = std::max(1, par.numRuns); + numRuns = std::min(numRuns, maxRuns); + RoutingIndexManager::NodeIndex depot(0); - RoutingIndexManager manager(nNodes, numVehicles, depot); + // std::vector depots(numRuns, depot); + // RoutingIndexManager manager(nNodes, numRuns, depots, depots); + RoutingIndexManager manager(nNodes, numRuns, depot); + // Create Routing Model. RoutingModel routing(manager); // Create and register a transit callback. @@ -812,6 +820,14 @@ bool route(const FPolygon &area, const Transects &transects, }); // Define cost of each arc. routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); + // Add distance dimension. + if (numRuns > 1) { + routing.AddDimension(transitCallbackIndex, 0, 300000000, + true, // start cumul to zero + "Distance"); + routing.GetMutableDimension("Distance") + ->SetGlobalSpanCostCoefficient(100000000); + } // Define disjunctions. #ifdef SNAKE_DEBUG @@ -834,7 +850,8 @@ bool route(const FPolygon &area, const Transects &transects, searchParameters.set_first_solution_strategy( FirstSolutionStrategy::PATH_CHEAPEST_ARC); // Number of solutions. - searchParameters.set_number_of_solutions_to_collect(par.numSolutionsPerRun); + auto numSolutionsPerRun = std::max(1, par.numSolutionsPerRun); + searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun); // Set costume limit. auto *solver = routing.solver(); auto *limit = solver->MakeCustomLimit(par.stop); @@ -862,7 +879,16 @@ bool route(const FPolygon &area, const Transects &transects, par.errorString = "User terminated."; return false; } + if (pSolutions->size() == 0) { + std::stringstream ss; + ss << "No solution found." << std::endl; + par.errorString = ss.str(); + return false; + } + //================================================================ + // Construc route. + //================================================================ #ifdef SNAKE_SHOW_TIME start = std::chrono::high_resolution_clock::now(); #endif @@ -879,88 +905,101 @@ bool route(const FPolygon &area, const Transects &transects, par.errorString = ss.str(); continue; } - //================================================================ - // Construc route. - //================================================================ - // Create index list. - auto index = routing.Start(0); - std::vector route_idx; - route_idx.push_back(manager.IndexToNode(index).value()); - while (!routing.IsEnd(index)) { - index = (*solution)->Value(routing.NextVar(index)); + // Iterate over all routes. + Solution routeVector; + for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) { + if (!routing.IsVehicleUsed(**solution, vehicle)) + continue; + + // Create index list. + auto index = routing.Start(vehicle); + std::vector route_idx; route_idx.push_back(manager.IndexToNode(index).value()); - } + while (!routing.IsEnd(index)) { + index = (*solution)->Value(routing.NextVar(index)); + route_idx.push_back(manager.IndexToNode(index).value()); + } #ifdef SNAKE_DEBUG - // Print route. - std::cout << "route " << counter - << " route_idx.size() = " << route_idx.size() << std::endl; - std::cout << "route: "; - for (const auto &idx : route_idx) { - std::cout << idx << ", "; - } - std::cout << std::endl; + // Print route. + std::cout << "route " << counter + << " route_idx.size() = " << route_idx.size() << std::endl; + std::cout << "route: "; + for (const auto &idx : route_idx) { + std::cout << idx << ", "; + } + std::cout << std::endl; #endif + if (route_idx.size() < 2) { + std::stringstream ss; + ss << par.errorString + << "Error while assembling route (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; + par.errorString = ss.str(); + continue; + } - if (route_idx.size() < 2) { - std::stringstream ss; - ss << par.errorString << "Error while assembling route " << counter << "." - << std::endl; - par.errorString = ss.str(); - continue; - } - - // Construct route. - Route r; - RouteInfo routeInfo; - for (size_t i = 0; i < route_idx.size() - 1; ++i) { - size_t nodeIndex0 = route_idx[i]; - size_t nodeIndex1 = route_idx[i + 1]; - const auto &n2t0 = nodeToTransectList[nodeIndex0]; - routeInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed); - // Copy transect to route. - const auto &t = transects[n2t0.transectsIndex]; - if (n2t0.reversed) { // transect reversal needed? - for (auto it = t.end() - 1; it > t.begin(); --it) { - r.push_back(*it); + // Assemble route. + Route r; + auto &path = r.path; + auto &info = r.info; + for (size_t i = 0; i < route_idx.size() - 1; ++i) { + size_t nodeIndex0 = route_idx[i]; + size_t nodeIndex1 = route_idx[i + 1]; + const auto &n2t0 = nodeToTransectList[nodeIndex0]; + info.emplace_back(n2t0.transectsIndex, n2t0.reversed); + // Copy transect to route. + const auto &t = transects[n2t0.transectsIndex]; + if (n2t0.reversed) { // transect reversal needed? + for (auto it = t.end() - 1; it > t.begin(); --it) { + path.push_back(*it); + } + } else { + for (auto it = t.begin(); it < t.end() - 1; ++it) { + path.push_back(*it); + } } - } else { - for (auto it = t.begin(); it < t.end() - 1; ++it) { - r.push_back(*it); + // Connect transects. + std::vector idxList; + if (!shortestPathFromGraph(connectionGraph, + nodeList[nodeIndex0].fromIndex, + nodeList[nodeIndex1].toIndex, idxList)) { + std::stringstream ss; + ss << par.errorString + << "Error while assembling route (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; + par.errorString = ss.str(); + continue; + } + if (i != route_idx.size() - 2) { + idxList.pop_back(); + } + for (auto idx : idxList) { + auto p = int2Float(vertices[idx]); + path.push_back(p); } } - // Connect transects. - std::vector idxList; - if (!shortestPathFromGraph(connectionGraph, - nodeList[nodeIndex0].fromIndex, - nodeList[nodeIndex1].toIndex, idxList)) { + // Append last transect info. + const auto &n2t0 = nodeToTransectList.back(); + info.emplace_back(n2t0.transectsIndex, n2t0.reversed); + + if (path.size() < 2 || info.size() < 2) { std::stringstream ss; - ss << par.errorString << "Error while assembling route " << counter - << "." << std::endl; + ss << par.errorString << "Route empty (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; par.errorString = ss.str(); continue; } - if (i != route_idx.size() - 2) { - idxList.pop_back(); - } - for (auto idx : idxList) { - auto p = int2Float(vertices[idx]); - r.push_back(p); - } - } - // Append last transect info. - const auto &n2t0 = nodeToTransectList.back(); - routeInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed); - if (r.size() < 2 || routeInfo.size() < 2) { + routeVector.push_back(std::move(r)); + } + if (routeVector.size() > 0) { + solutionVector.push_back(std::move(routeVector)); + } else { std::stringstream ss; - ss << par.errorString << "Route " << counter << " empty." << std::endl; + ss << par.errorString << "Solution " << counter << " empty." << std::endl; par.errorString = ss.str(); - continue; } - - routeVector.push_back(std::move(r)); - routeInfoVector.push_back(std::move(routeInfo)); } #ifdef SNAKE_SHOW_TIME delta = std::chrono::duration_cast( @@ -968,227 +1007,13 @@ bool route(const FPolygon &area, const Transects &transects, cout << "reconstruct route: " << delta.count() << " ms" << endl; #endif - if (routeVector.size() > 0 && routeVector.size() == routeInfoVector.size()) { + if (solutionVector.size() > 0) { return true; } else { return false; } } -bool route_old(const FPolygon &area, const Transects &transects, - std::vector &transectInfo, Route &r, - std::function stop, string &errorString) { - //======================================= - // Route Transects using Google or-tools. - //======================================= - - // Create vertex list; - FLineString vertices; - size_t n0 = 0; - for (const auto &t : transects) { - n0 += std::min(t.size(), 2); - } - vertices.reserve(n0); - - struct LocalInfo { - LocalInfo(size_t n, bool f) : index(n), front(f) {} - size_t index; - bool front; - }; - std::vector localTransectInfo; - for (size_t i = 0; i < transects.size(); ++i) { - const auto &t = transects[i]; - vertices.push_back(t.front()); - localTransectInfo.push_back(LocalInfo{i, true}); - if (t.size() >= 2) { - vertices.push_back(t.back()); - localTransectInfo.push_back(LocalInfo{i, false}); - } - } - - for (long i = 0; i < long(area.outer().size()) - 1; ++i) { - vertices.push_back(area.outer()[i]); - } - for (auto &ring : area.inners()) { - for (auto &vertex : ring) - vertices.push_back(vertex); - } - size_t n1 = vertices.size(); - // Generate routing model. - Matrix connectionGraph(n1, n1); - // Offset joined area. - FPolygon areaOffset; - offsetPolygon(area, areaOffset, detail::offsetConstant); - // Generate routing model. -#ifdef SNAKE_SHOW_TIME - auto start = std::chrono::high_resolution_clock::now(); -#endif - graphFromPolygon(areaOffset, vertices, connectionGraph); -#ifdef SNAKE_SHOW_TIME - auto delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" - << endl; -#endif - Matrix distanceMatrix(connectionGraph); -#ifdef SNAKE_SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - if (!toDistanceMatrix(distanceMatrix)) { - errorString = "Error while generating distance matrix."; - return false; - } -#ifdef SNAKE_SHOW_TIME - delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" - << endl; -#endif - Matrix dm(n0, n0); - for (size_t i = 0; i < n0; ++i) { - dm(i, i) = 0; - for (size_t j = i + 1; j < n0; ++j) { - dm(i, j) = int64_t(distanceMatrix(i, j) * CLIPPER_SCALE); - dm(j, i) = int64_t(distanceMatrix(i, j) * CLIPPER_SCALE); - } - } - - // Create Routing Index Manager. - RoutingIndexManager manager( - dm.n() /*num indices*/, 1 /*vehicles*/, - RoutingIndexManager::NodeIndex(0) /*depot index*/); - // Create Routing Model. - RoutingModel routing(manager); - - // Create and register a transit callback. - const int transitCallbackIndex = routing.RegisterTransitCallback( - [&dm, &manager](int64 from_index, int64 to_index) -> int64 { - // Convert from routing variable Index to distance matrix NodeIndex. - auto from_node = manager.IndexToNode(from_index).value(); - auto to_node = manager.IndexToNode(to_index).value(); - return dm(from_node, to_node); - }); - - // Define cost of each arc. - routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); - - // Define Constraints (this constraints have a huge impact on the - // solving time, improvments could be done, e.g. SearchFilter). -#ifdef SNAKE_DEBUG - std::cout << "snake::route(): Constraints:" << std::endl; -#endif - Solver *solver = routing.solver(); - size_t index = 0; - for (size_t i = 0; i < transects.size(); ++i) { - const auto &t = transects[i]; -#ifdef SNAKE_DEBUG - std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl; -#endif - if (t.size() >= 2) { - auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index)); - auto idx1 = - manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1)); - auto cond0 = routing.NextVar(idx0)->IsEqual(idx1); - auto cond1 = routing.NextVar(idx1)->IsEqual(idx0); - auto c = solver->MakeNonEquality(cond0, cond1); - solver->AddConstraint(c); -#ifdef SNAKE_DEBUG - std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next(" - << index + 1 << ") == " << index << " )" << std::endl; -#endif - index += 2; - } else { - index += 1; - } - } - // Set first solution heuristic. - auto searchParameters = DefaultRoutingSearchParameters(); - searchParameters.set_first_solution_strategy( - FirstSolutionStrategy::PATH_CHEAPEST_ARC); - // Set costume limit. - auto *limit = solver->MakeCustomLimit(stop); - routing.AddSearchMonitor(limit); - // Solve the problem. -#ifdef SNAKE_SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - const Assignment *solution = routing.SolveWithParameters(searchParameters); -#ifdef SNAKE_SHOW_TIME - delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "Execution time routing.SolveWithParameters(): " << delta.count() - << " ms" << endl; -#endif - - if (!solution || solution->Size() <= 1) { - errorString = "Not able to solve the routing problem."; - return false; - } else if (stop()) { - errorString = "User terminated."; - return false; - } - - // Extract index list from solution. - index = routing.Start(0); - std::vector route_idx; - route_idx.push_back(manager.IndexToNode(index).value()); - while (!routing.IsEnd(index)) { - index = solution->Value(routing.NextVar(index)); - route_idx.push_back(manager.IndexToNode(index).value()); - } - - // Helper Lambda. - auto idx2Vertex = [&vertices](const std::vector &idxArray, - std::vector &path) { - for (auto idx : idxArray) - path.push_back(vertices[idx]); - }; - - // Construct route. - for (size_t i = 0; i < route_idx.size() - 1; ++i) { - size_t idx0 = route_idx[i]; - size_t idx1 = route_idx[i + 1]; - const auto &info1 = localTransectInfo[idx0]; - const auto &info2 = localTransectInfo[idx1]; - if (info1.index == info2.index) { // same transect? - TransectInfo trInfo(info1.index, - !info1.front ? true : false /*transect reversed?*/); - transectInfo.push_back(trInfo); - if (!info1.front) { // transect reversal needed? - FLineString reversedTransect; - const auto &t = transects[info1.index]; - for (auto it = t.end() - 1; it >= t.begin(); --it) { - reversedTransect.push_back(*it); - } - for (auto it = reversedTransect.begin(); - it < reversedTransect.end() - 1; ++it) { - r.push_back(*it); - } - } else { - const auto &t = transects[info1.index]; - for (auto it = t.begin(); it < t.end() - 1; ++it) { - r.push_back(*it); - } - } - } else { - std::vector idxList; - shortestPathFromGraph(connectionGraph, idx0, idx1, idxList); - if (i != route_idx.size() - 2) { - idxList.pop_back(); - } - idx2Vertex(idxList, r); - } - } - return true; -} - -bool route_old(const FPolygon &area, const Transects &transects, - std::vector &transectInfo, Route &r, - string &errorString) { - auto stop = [] { return false; }; - return route_old(area, transects, transectInfo, r, stop, errorString); -} - FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); } FPoint int2Float(const IPoint &ip, IntType scale) { diff --git a/src/Wima/Snake/snake.h b/src/Wima/Snake/snake.h index 5f73e8f9872845cbdb769f4f8db851cb60edae05..1627a6744ac1b5513fdf24c139030c44b05c9aaa 100644 --- a/src/Wima/Snake/snake.h +++ b/src/Wima/Snake/snake.h @@ -215,7 +215,6 @@ bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, using Transects = vector; using Progress = vector; -using Route = FLineString; bool transectsFromScenario(Length distance, Length minLength, Angle angle, const FPolygon &mArea, @@ -228,27 +227,26 @@ struct TransectInfo { size_t index; bool reversed; }; -using RouteInfo = std::vector; +struct Route { + FLineString path; + std::vector info; +}; +using Solution = + std::vector; // Every route corresponds to one run/vehicle struct RouteParameter { RouteParameter() - : numSolutionsPerRun(1), numRuns(1), stop([] { return false; }) {} + : numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5), + stop([] { return false; }) {} std::size_t numSolutionsPerRun; std::size_t numRuns; + std::size_t minNumTransectsPerRun; std::function stop; mutable std::string errorString; }; bool route(const FPolygon &area, const Transects &transects, - std::vector &routeInfoVector, - std::vector &routeVector, + std::vector &solutionVector, const RouteParameter &par = RouteParameter()); -bool route_old(const FPolygon &area, const Transects &transects, - std::vector &transectInfo, Route &r, - string &errorString); -bool route_old(const FPolygon &area, const Transects &transects, - std::vector &transectInfo, Route &r, - std::function stop, string &errorString); - namespace detail { const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 41230679e9dd29745a7bf8c87cf0810980a94dde..4f9a7f13ce2d9b472c5b053d0ff7b1ee00076821 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -798,34 +798,36 @@ void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) { // Copy waypoints to waypoint manager. _snakeWM.clear(); - if (data->routeVector.size() > 0 && data->routeVector.front().size() > 0 && - data->routeInfoVector.size() > 0) { + if (data->solutionVector.size() > 0 && + data->solutionVector.front().size() > 0) { // Store route. const auto &transectsENU = data->transects; - const auto &routeInfo = data->routeInfoVector.front(); - const auto &route = data->routeVector.front(); + const auto &solution = data->solutionVector.front(); + const auto &route = solution.front(); + const auto &path = route.path; + const auto &info = route.info; // Find index of first waypoint. std::size_t idxFirst = 0; - const auto &infoFirst = routeInfo.front(); + const auto &infoFirst = info.front(); const auto &firstTransect = transectsENU[infoFirst.index]; const auto &firstWaypoint = infoFirst.reversed ? firstTransect.back() : firstTransect.front(); double th = 0.001; - for (std::size_t i = 0; i < route.size(); ++i) { - auto dist = bg::distance(route[i], firstWaypoint); + for (std::size_t i = 0; i < path.size(); ++i) { + auto dist = bg::distance(path[i], firstWaypoint); if (dist < th) { idxFirst = i; break; } } // Find index of last waypoint. - std::size_t idxLast = route.size() - 1; - const auto &infoLast = routeInfo.back(); + std::size_t idxLast = path.size() - 1; + const auto &infoLast = info.back(); const auto &lastTransect = transectsENU[infoLast.index]; const auto &lastWaypoint = infoLast.reversed ? lastTransect.front() : lastTransect.back(); - for (long i = route.size() - 1; i >= 0; --i) { - auto dist = bg::distance(route[i], lastWaypoint); + for (long i = path.size() - 1; i >= 0; --i) { + auto dist = bg::distance(path[i], lastWaypoint); if (dist < th) { idxLast = i; break; @@ -834,7 +836,7 @@ void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) { // Convert to geo coordinates and append to waypoint manager. const auto &ori = this->_origin; for (std::size_t i = idxFirst; i <= idxLast; ++i) { - auto &vertex = route[i]; + auto &vertex = path[i]; QGeoCoordinate c; snake::fromENU(ori, vertex, c); _snakeWM.push_back(c); diff --git a/src/Wima/json/CircularSurvey.SettingsGroup.json b/src/Wima/json/CircularSurvey.SettingsGroup.json index 3efb86015f6efc6c0146a7f607b58ffb1242ebda..8f712af16209d8f5d6fd1dee5e3c87a69d858926 100644 --- a/src/Wima/json/CircularSurvey.SettingsGroup.json +++ b/src/Wima/json/CircularSurvey.SettingsGroup.json @@ -38,5 +38,18 @@ "shortDescription": "Route variant.", "type": "uint64", "defaultValue": 0 +}, +{ + "name": "NumRuns", + "shortDescription": "The number of runs.", + "type": "uint64", + "min": 1, + "defaultValue": 1 +}, +{ + "name": "Run", + "shortDescription": "The current run.", + "type": "uint64", + "defaultValue": 0 } ] diff --git a/src/WimaView/WimaMeasurementAreaEditor.qml b/src/WimaView/WimaMeasurementAreaEditor.qml index 939cf2cc9e16e6edf9d06f2b29e32a1f9b2019ef..a2cc4035460b2b2ad02fdc865bc3edc4792b2503 100644 --- a/src/WimaView/WimaMeasurementAreaEditor.qml +++ b/src/WimaView/WimaMeasurementAreaEditor.qml @@ -128,40 +128,6 @@ Rectangle { } // Tile Column - SectionHeader { - id: transectsHeader - text: qsTr("Transects") - } - - Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: transectsHeader.checked - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: _margin - rowSpacing: _margin - columns: 2 - - QGCLabel { text: qsTr("Distance") } - FactTextField { - fact: areaItem.transectDistance - Layout.fillWidth: true - } - - QGCLabel { text: qsTr("Min. Length") } - - FactTextField { - fact: areaItem.minTransectLength - Layout.fillWidth: true - } - } // Transects GridLayout - } // Transects Column - - SectionHeader { id: statsHeader text: qsTr("Statistics")