Skip to content

Q
qgroundcontrol
Commit 9282a10c authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

route variants added to circular survey

parent 053f9768
Hide whitespace changes
Inline Side-by-side
Showing with 881 additions and 303 deletions
Paths/temp0.plan 0 → 100644
View file @ 9282a10c
{
"fileType": "Plan",
"geoFence": {
"circles": [
],
"polygons": [
],
"version": 2
},
"groundStation": "QGroundControl",
"mission": {
"cruiseSpeed": 15,
"firmwareType": 3,
"hoverSpeed": 1,
"items": [
{
"autoContinue": true,
"command": 22,
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
null,
47.76779586216649,
16.530510396830728,
5
],
"type": "SimpleItem"
},
{
"Alpha": 23,
"MinLength": 5,
"ReferencePointAlt": 0,
"ReferencePointLat": 47.76807182520187,
"ReferencePointLong": 16.530610531894183,
"TransectDistance": 10,
"TransectStyleComplexItem": {
"CameraCalc": {
"AdjustedFootprintFrontal": 25,
"AdjustedFootprintSide": 25,
"CameraName": "Manual (no camera specs)",
"DistanceToSurface": 15,
"DistanceToSurfaceRelative": true,
"version": 1
},
"CameraShots": 0,
"CameraTriggerInTurnAround": true,
"FollowTerrain": false,
"HoverAndCapture": false,
"Items": [
{
"autoContinue": true,
"command": 16,
"doJumpId": 2,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7677958621741,
16.53051039683979,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76795620865596,
16.530658248522045,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768085758380195,
16.53111095084737,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 5,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768170929071175,
16.531067144987073,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 6,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76812500283788,
16.530906658047133,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 7,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768170929071175,
16.531067144987073,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 8,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76811004588309,
16.530512964621895,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 9,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76811004588309,
16.530512964621895,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 10,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76825609974418,
16.531023338983744,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 11,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76834127040819,
16.53097953282404,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 12,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76822245220941,
16.530564330637993,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 13,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76834127040819,
16.53097953282404,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 14,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76795620865596,
16.530658248522045,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 15,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7677958621741,
16.53051039683979,
15
],
"type": "SimpleItem"
}
],
"Refly90Degrees": false,
"TurnAroundDistance": 10,
"VisualTransectPoints": [
[
47.7677958621741,
16.53051039683979
],
[
47.76795620865596,
16.530658248522045
],
[
47.768085758380195,
16.53111095084737
],
[
47.768170929071175,
16.531067144987073
],
[
47.76812500283788,
16.530906658047133
],
[
47.768170929071175,
16.531067144987073
],
[
47.76811004588309,
16.530512964621895
],
[
47.76811004588309,
16.530512964621895
],
[
47.76825609974418,
16.531023338983744
],
[
47.76834127040819,
16.53097953282404
],
[
47.76822245220941,
16.530564330637993
],
[
47.76834127040819,
16.53097953282404
],
[
47.76795620865596,
16.530658248522045
],
[
47.7677958621741,
16.53051039683979
]
],
"version": 1
},
"Type": 1,
"complexItemType": "CircularSurvey",
"polygon": [
[
47.76809580679245,
16.530246122817612
],
[
47.76823933601322,
16.53060087654427
],
[
47.7683711160486,
16.530967006195464
],
[
47.7680076482754,
16.531153949077463
],
[
47.7677855557718,
16.530403347547246
],
[
47.768126518382985,
16.5309051298743
]
],
"type": "ComplexItem",
"version": 1
},
{
"AMSLAltAboveTerrain": 0,
"Altitude": 0,
"AltitudeMode": 1,
"autoContinue": true,
"command": 21,
"doJumpId": 19,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76779586216649,
16.530510396830728,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.76779586216649,
16.530510396830728,
178
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}
src/PlanView/CircularSurveyItemEditor.qml
View file @ 9282a10c
......@@ -99,23 +99,58 @@ Rectangle {
Layout.columnSpan: 2
}
property var typeFact: missionItem.type
property int type: typeFact.value
property var names: ["Circular", "Linear"]
ExclusiveGroup{id: typeGroup}
Repeater{
id: typeRepeater
property var typeFact: missionItem.type
property int type: typeFact.value
property var names: ["Circular", "Linear"]
model: missionItem.typeCount
QGCRadioButton {
checked: index === generalGrid.type
text: qsTr(generalGrid.names[index])
checked: index === typeRepeater.type
text: qsTr(typeRepeater.names[index])
onCheckedChanged: {
if (checked){
missionItem.type.value = index
}
checked = Qt.binding(function(){ return index === generalGrid.type})
checked = Qt.binding(function(){ return index === typeRepeater.type})
}
}
}
QGCLabel {
text: qsTr("Variant")
Layout.columnSpan: 2
}
GridLayout{
Layout.columnSpan: 2
columnSpacing: _margin
rowSpacing: _margin
columns: 4
Repeater{
id: variantRepeater
property var fact: missionItem.variant
property int variant: fact.value
property var names: missionItem.variantNames
property int len: missionItem.variantNames.length
model: len
QGCRadioButton {
checked: index === variantRepeater.variant
text: variantRepeater.names[index]
onCheckedChanged: {
if (checked){
missionItem.variant.value = index
}
checked = Qt.binding(function(){ return index === variantRepeater.variant})
}
}
}
}
......
src/Wima/CircularSurvey.cc
View file @ 9282a10c
......@@ -49,6 +49,7 @@ const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat";
const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong";
const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt";
const char *CircularSurvey::variantName = "Variant";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
......@@ -60,8 +61,9 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
_alpha(settingsGroup, _metaDataMap[alphaName]),
_minLength(settingsGroup, _metaDataMap[minLengthName]),
_type(settingsGroup, _metaDataMap[typeName]),
_pWorker(std::make_unique<RoutingThread>()), _needsStoring(false),
_needsReversal(false), _hidePolygon(false) {
_variant(settingsGroup, _metaDataMap[variantName]),
_pWorker(std::make_unique<RoutingThread>()), _state(STATE::DEFAULT),
_hidePolygon(false) {
Q_UNUSED(kmlOrShpFile)
_editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
......@@ -80,6 +82,8 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_rebuildTransects);
connect(&this->_type, &Fact::rawValueChanged, this,
&CircularSurvey::_rebuildTransects);
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
// Connect worker.
connect(this->_pWorker.get(), &RoutingThread::result, this,
&CircularSurvey::_setTransects);
......@@ -95,7 +99,7 @@ void CircularSurvey::resetReference() {
}
void CircularSurvey::reverse() {
this->_needsReversal = true;
this->_state = STATE::REVERSE;
this->_rebuildTransects();
}
......@@ -116,6 +120,8 @@ Fact *CircularSurvey::alpha() { return &_alpha; }
bool CircularSurvey::hidePolygon() const { return _hidePolygon; }
QList<QString> CircularSurvey::variantNames() const { return _variantNames; }
QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }
QList<QGeoCoordinate> CircularSurvey::safeArea() const {
......@@ -173,6 +179,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
{alphaName, QJsonValue::Double, true},
{minLengthName, QJsonValue::Double, true},
{typeName, QJsonValue::Double, true},
{variantName, QJsonValue::Double, false},
{refPointLatitudeName, QJsonValue::Double, true},
{refPointLongitudeName, QJsonValue::Double, true},
{refPointAltitudeName, QJsonValue::Double, true},
......@@ -214,6 +221,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_alpha.setRawValue(complexObject[alphaName].toDouble());
_minLength.setRawValue(complexObject[minLengthName].toDouble());
_type.setRawValue(complexObject[typeName].toInt());
_variant.setRawValue(complexObject[variantName].toInt());
_referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
_referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
_referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());
......@@ -247,6 +255,7 @@ void CircularSurvey::save(QJsonArray &planItems) {
saveObject[alphaName] = _alpha.rawValue().toDouble();
saveObject[minLengthName] = _minLength.rawValue().toDouble();
saveObject[typeName] = double(_type.rawValue().toUInt());
saveObject[variantName] = double(_variant.rawValue().toUInt());
saveObject[refPointLongitudeName] = _referencePoint.longitude();
saveObject[refPointLatitudeName] = _referencePoint.latitude();
saveObject[refPointAltitudeName] = _referencePoint.altitude();
......@@ -321,68 +330,160 @@ void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
}
}
void CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
void CircularSurvey::_changeVariant() {
this->_state = STATE::VARIANT_CHANGE;
this->_rebuildTransects();
}
double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const {
return tr("Circular Survey");
void CircularSurvey::_updateWorker() {
// Reset data.
this->_transects.clear();
this->_rawTransects.clear();
this->_routes.clear();
this->_variantNames.clear();
emit variantNamesChanged();
// Prepare data.
auto ref = this->_referencePoint;
auto polygon = this->_surveyAreaPolygon.coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
auto safeArea = this->_safeArea;
for (auto &v : safeArea) {
v.setAltitude(0);
}
auto depot = this->_depot;
RoutingParameter par;
par.numSolutionsPerRun = 5;
auto &safeAreaENU = par.safeArea;
bool useDepot = false;
if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
useDepot = true;
snake::areaToEnu(ref, safeArea, safeAreaENU);
} else {
snake::areaToEnu(ref, polygon, safeAreaENU);
}
auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() *
bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha =
snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
// Select survey type.
if (this->_type.rawValue().toUInt() == integral(Type::Circular)) {
// Clip angle.
if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
alpha <= snake::Angle(45 * bu::degree::degree)) {
auto generator = [ref, depot, useDepot, polygon, distance, alpha,
minLength](snake::Transects &transects) -> bool {
return circularTransects(ref, depot, useDepot, polygon, distance, alpha,
minLength, transects);
};
// Start routing worker.
this->_pWorker->route(par, generator);
} else {
if (alpha < snake::Angle(0.3 * bu::degree::degree)) {
this->_alpha.setCookedValue(QVariant(0.3));
} else {
this->_alpha.setCookedValue(QVariant(45));
}
}
} else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
auto generator = [ref, depot, useDepot, polygon, distance, alpha,
minLength](snake::Transects &transects) -> bool {
return linearTransects(ref, depot, useDepot, polygon, distance, alpha,
minLength, transects);
};
// Start routing worker.
this->_pWorker->route(par, generator);
} else {
qWarning()
<< "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
<< this->_type.rawValue().toUInt();
}
// Mark transects as dirty.
this->_transectsDirty = true;
}
QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
void CircularSurvey::_changeVariantWorker() {
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;
}
}
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
// 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]);
}
}
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
void CircularSurvey::_reverseWorker() {
if (this->_transects.size() > 0) {
auto &t = this->_transects.front();
QList<QList<CoordInfo_t>> tr{QList<CoordInfo_t>()};
auto &list = tr.front();
list.reserve(t.size());
for (auto it = t.end() - 1; it >= t.begin(); --it) {
list.append(*it);
}
this->_transects.swap(tr);
}
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
void CircularSurvey::_storeWorker() {
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
_loadedMissionItems.clear();
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
void CircularSurvey::_rebuildTransectsPhase1(void) {
qWarning() << "_rebuildTransectsPhase1: TODO: remove depot valid stuff";
// Store result of former calculation.
if (this->_needsStoring) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
_loadedMissionItems.clear();
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
bool error = false;
// Store raw transects.
const auto &transectsENU = this->_workerOutput->transects;
const auto &ori = this->_referencePoint;
std::size_t startIdx = 0;
if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
startIdx = 1;
}
for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
const auto &t = transectsENU[i];
QList<QGeoCoordinate> trGeo;
for (auto &v : t) {
QGeoCoordinate c;
snake::fromENU(ori, v, c);
trGeo.append(c);
}
this->_rawTransects.append(trGeo);
// Store raw transects.
const auto &pRoutingData = this->_workerOutput;
const auto &ori = this->_referencePoint;
const auto &transectsENU = pRoutingData->transects;
std::size_t startIdx = 0;
bool depotValid = false;
if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
depotValid = true;
startIdx = 1;
}
QList<QList<QGeoCoordinate>> rawTransects;
for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
const auto &t = transectsENU[i];
rawTransects.append(QList<QGeoCoordinate>());
auto trGeo = rawTransects.back();
for (auto &v : t) {
QGeoCoordinate c;
snake::fromENU(ori, v, c);
trGeo.append(c);
}
// Store route.
const auto &transectsInfo = this->_workerOutput->transectsInfo;
}
// Store routes.
auto nRoutes = pRoutingData->routeVector.size();
QVector<Transects> routes(nRoutes, Transects{QList<CoordInfo_t>()});
for (std::size_t k = 0; k < nRoutes; ++k) {
const auto &transectsInfo = pRoutingData->routeInfoVector.at(k);
if (transectsInfo.size() > 1) {
const auto &route = this->_workerOutput->route;
const auto &route = pRoutingData->routeVector.at(k);
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst =
this->depot().isValid() ? transectsInfo.at(1) : transectsInfo.at(0);
depotValid ? transectsInfo.at(1) : transectsInfo.at(0);
const auto &firstTransect = transectsENU[infoFirst.index];
if (firstTransect.size() > 0) {
const auto &firstWaypoint =
......@@ -395,6 +496,7 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
break;
}
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.at(transectsInfo.size() - 2);
......@@ -409,134 +511,120 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
break;
}
}
// Convert to geo coordinates.
QList<CoordInfo_t> list;
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});
}
this->_transects.append(std::move(list));
} else {
qWarning()
<< "CS::rebuildTransectsPhase1(): lastTransect.size() == 0";
error = true;
qWarning() << "CS::_storeWorker(): lastTransect.size() == 0";
}
} else {
qWarning() << "CS::rebuildTransectsPhase1(): firstTransect.size() == 0";
error = true;
qWarning() << "CS::_storeWorker(): firstTransect.size() == 0";
}
} else {
qWarning() << "CS::rebuildTransectsPhase1(): transectsInfo.size() <= 1";
error = true;
qWarning() << "CS::_storeWorker(): transectsInfo.size() <= 1";
}
}
this->_needsStoring = false;
if (!error) {
// Mark transect as stored and ready.
this->_transectsDirty = false;
} else { // clear up
this->_rawTransects.clear();
this->_transects.clear();
// Remove empty routes.
bool error = true;
std::size_t n = 0;
for (auto it = routes.begin(); it < routes.end();) {
if (it->size() > 0 && it->front().size() > 0) {
error = false;
++it;
++n;
} else {
it = routes.erase(it);
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1(): store: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
}
// Reverse transects only.
else if (this->_needsReversal) {
if (this->_transects.size() > 0) {
auto &t = this->_transects.front();
QList<CoordInfo_t> list;
list.reserve(t.size());
for (auto it = t.end() - 1; it >= t.begin(); --it) {
list.append(*it);
}
this->_transects.clear();
this->_transects.append(list);
// Assign routes if no error occured.
if (!error) {
// Swap first route to _transects.
this->_transects.swap(routes.front());
this->_routes.swap(routes);
// Add route variant names.
for (std::size_t i = 1; i <= n; ++i) {
this->_variantNames.append(QString::number(i));
}
this->_needsReversal = false;
emit variantNamesChanged();
this->_variant.setCookedValue(QVariant(0));
// Swap in rawTransects.
this->_rawTransects.swap(rawTransects);
// Mark transect as stored and ready.
this->_transectsDirty = false;
}
// Start calculation.
else {
}
void CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
}
double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const {
return tr("Circular Survey");
}
QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
void CircularSurvey::_rebuildTransectsPhase1(void) {
qWarning() << "_rebuildTransectsPhase1: TODO: remove depot valid stuff";
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now();
auto start = std::chrono::high_resolution_clock::now();
#endif
this->_transects.clear();
this->_rawTransects.clear();
// Prepare data.
auto ref = this->_referencePoint;
auto polygon = this->_surveyAreaPolygon.coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
auto safeArea = this->_safeArea;
for (auto &v : safeArea) {
v.setAltitude(0);
}
auto depot = this->_depot;
snake::FPolygon safeAreaENU;
bool useDepot = false;
if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
useDepot = true;
snake::areaToEnu(ref, safeArea, safeAreaENU);
} else {
snake::areaToEnu(ref, polygon, safeAreaENU);
}
auto distance = snake::Length(
this->_transectDistance.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha =
snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
// Select survey type.
this->_needsStoring = false;
if (this->_type.rawValue().toUInt() == integral(Type::Circular)) {
if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
alpha <= snake::Angle(45 * bu::degree::degree)) {
auto generator = [ref, depot, useDepot, polygon, distance, alpha,
minLength](snake::Transects &transects) -> bool {
return circularTransects(ref, depot, useDepot, polygon, distance,
alpha, minLength, transects);
};
// Start routing worker.
this->_pWorker->route(safeAreaENU, generator);
} else {
if (alpha < snake::Angle(0.3 * bu::degree::degree)) {
this->_alpha.setCookedValue(QVariant(0.3));
} else {
this->_alpha.setCookedValue(QVariant(45));
}
}
} else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
auto generator = [ref, depot, useDepot, polygon, distance, alpha,
minLength](snake::Transects &transects) -> bool {
return linearTransects(ref, depot, useDepot, polygon, distance, alpha,
minLength, transects);
};
// Start routing worker.
this->_pWorker->route(safeAreaENU, generator);
} else {
qWarning()
<< "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
<< this->_type.rawValue().toUInt();
}
// Mark transects as dirty.
this->_transectsDirty = true;
switch (this->_state) {
case STATE::STORE:
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1: store.";
#endif
this->_storeWorker();
break;
case STATE::VARIANT_CHANGE:
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1: variant change.";
#endif
this->_changeVariantWorker();
break;
case STATE::REVERSE:
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1(): start: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
qWarning() << "CS::rebuildTransectsPhase1: reverse.";
#endif
this->_reverseWorker();
break;
case STATE::DEFAULT:
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1: update.";
#endif
this->_updateWorker();
break;
}
this->_state = STATE::DEFAULT;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1(): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
}
void CircularSurvey::_recalcComplexDistance() {
......@@ -556,7 +644,7 @@ void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
this->_workerOutput = pRoute;
this->_needsStoring = true;
this->_state = STATE::STORE;
this->_rebuildTransects();
}
......@@ -564,6 +652,8 @@ Fact *CircularSurvey::minLength() { return &_minLength; }
Fact *CircularSurvey::type() { return &_type; }
Fact *CircularSurvey::variant() { return &_variant; }
int CircularSurvey::typeCount() const { return int(integral(Type::Count)); }
bool CircularSurvey::calculating() const {
......
src/Wima/CircularSurvey.h
View file @ 9282a10c
......@@ -34,9 +34,12 @@ public:
Q_PROPERTY(Fact *alpha READ alpha CONSTANT)
Q_PROPERTY(Fact *minLength READ minLength CONSTANT)
Q_PROPERTY(Fact *type READ type CONSTANT)
Q_PROPERTY(Fact *variant READ variant 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<QString> variantNames READ variantNames NOTIFY variantNamesChanged)
Q_INVOKABLE void resetReference(void);
Q_INVOKABLE void reverse(void);
......@@ -53,9 +56,11 @@ public:
Fact *alpha();
Fact *minLength();
Fact *type();
Fact *variant();
int typeCount() const;
bool calculating() const;
bool hidePolygon() const;
QList<QString> variantNames() const;
QGeoCoordinate depot() const;
QList<QGeoCoordinate> safeArea() const;
const QList<QList<QGeoCoordinate>> &rawTransects() const;
......@@ -81,6 +86,7 @@ public:
static const char *alphaName;
static const char *minLengthName;
static const char *typeName;
static const char *variantName;
static const char *CircularSurveyName;
static const char *refPointLongitudeName;
static const char *refPointLatitudeName;
......@@ -92,6 +98,7 @@ signals:
void hidePolygonChanged();
void depotChanged();
void safeAreaChanged();
void variantNamesChanged();
private slots:
// Overrides from TransectStyleComplexItem
......@@ -105,6 +112,11 @@ private:
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _changeVariant();
void _updateWorker();
void _changeVariantWorker();
void _reverseWorker();
void _storeWorker();
// center of the circular lanes, e.g. base station
QGeoCoordinate _referencePoint;
......@@ -117,16 +129,26 @@ private:
// this value
SettingsFact _minLength;
SettingsFact _type;
SettingsFact _variant;
QList<QString> _variantNames;
// Worker
using PtrWorker = std::shared_ptr<RoutingThread>;
PtrWorker _pWorker;
PtrRoutingData _workerOutput;
QList<QList<QGeoCoordinate>> _rawTransects;
bool _needsStoring;
bool _needsReversal;
bool _hidePolygon;
// Data and State.
QGeoCoordinate _depot;
QList<QGeoCoordinate> _safeArea;
QList<QList<QGeoCoordinate>> _rawTransects;
QVector<Transects> _routes;
enum class STATE {
DEFAULT,
STORE,
REVERSE,
VARIANT_CHANGE,
};
STATE _state;
bool _hidePolygon;
};
src/Wima/RoutingThread.cpp
View file @ 9282a10c
......@@ -23,11 +23,11 @@ RoutingThread::~RoutingThread() {
bool RoutingThread::calculating() const { return this->_calculating; }
void RoutingThread::route(const snake::FPolygon &safeArea,
const RoutingThread::Generator &generator) {
void RoutingThread::route(const RoutingParameter &par,
const Generator &generator) {
// Sample input.
Lock lk(this->_mutex);
this->_safeArea = safeArea;
this->_par = par;
this->_generator = generator;
lk.unlock();
......@@ -54,9 +54,12 @@ void RoutingThread::run() {
this->_calculating = true;
emit calculatingChanged();
Lock lk(this->_mutex);
auto safeAreaENU = this->_safeArea;
auto par = this->_par;
auto generator = this->_generator;
lk.unlock();
auto safeAreaENU = par.safeArea;
auto numRuns = par.numRuns;
auto numSolutionsPerRun = par.numSolutionsPerRun;
PtrRoutingData pRouteData(new RoutingData());
auto &transectsENU = pRouteData->transects;
......@@ -70,53 +73,29 @@ void RoutingThread::run() {
#endif
} else {
// Prepare data for routing.
auto &transectsInfo = pRouteData->transectsInfo;
auto &route = pRouteData->route;
const auto routingStart = std::chrono::high_resolution_clock::now();
auto &routeInfoVector = pRouteData->routeInfoVector;
auto &routeVector = pRouteData->routeVector;
snake::RouteParameter snakePar;
snakePar.numSolutionsPerRun = numSolutionsPerRun;
const auto maxRoutingTime = std::chrono::minutes(10);
const auto routingEnd = routingStart + maxRoutingTime;
const auto routingEnd =
std::chrono::high_resolution_clock::now() + maxRoutingTime;
const auto &restart = this->_restart;
auto stopLambda = [&restart, routingEnd] {
snakePar.stop = [&restart, routingEnd] {
bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
return restart || expired;
};
std::string errorString;
// Route transects
//#ifdef SHOW_CIRCULAR_SURVEY_TIME
// auto s = std::chrono::high_resolution_clock::now();
//#endif
// snake::route_old(safeAreaENU, transectsENU, transectsInfo,
// route,
// stopLambda, errorString);
//#ifdef SHOW_CIRCULAR_SURVEY_TIME
// qWarning() << "RoutingWorker::run(): route_old time: "
// <<
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() -
// s) .count()
// << " ms";
//#endif
//#ifdef SHOW_CIRCULAR_SURVEY_TIME
// s = std::chrono::high_resolution_clock::now();
//#endif
// transectsInfo.clear();
// route.clear();
// errorString.clear();
bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
route, stopLambda, errorString);
//#ifdef SHOW_CIRCULAR_SURVEY_TIME
// qWarning() << "RoutingWorker::run(): route time: "
// <<
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() -
// s) .count()
// << " ms";
//#endif
// Route transects.
bool success = snake::route(safeAreaENU, transectsENU, routeInfoVector,
routeVector, snakePar);
// Check if routing was successful.
if ((!success || route.size() < 3) && !this->_restart) {
if ((!success || routeVector.size() < 1) && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "RoutingWorker::run(): "
"routing failed.";
qWarning() << snakePar.errorString.c_str();
#endif
} else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
......@@ -145,6 +124,7 @@ void RoutingThread::run() {
.count()
<< " ms";
#endif
// Signal calulation end and set thread to sleep.
this->_calculating = false;
emit calculatingChanged();
Lock lk2(this->_mutex);
......
src/Wima/RoutingThread.h
View file @ 9282a10c
......@@ -11,11 +11,18 @@
#include <mutex>
struct RoutingData {
snake::FLineString route;
snake::Transects transects;
std::vector<snake::TransectInfo> transectsInfo;
std::vector<snake::Route> routeVector;
std::vector<snake::RouteInfo> routeInfoVector;
std::string errorString;
};
struct RoutingParameter {
RoutingParameter() : numSolutionsPerRun(1), numRuns(1) {}
snake::FPolygon safeArea;
std::size_t numSolutionsPerRun;
std::size_t numRuns;
};
//!
//! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
......@@ -35,7 +42,7 @@ public:
bool calculating() const;
public slots:
void route(const snake::FPolygon &safeArea, const Generator &generator);
void route(const RoutingParameter &par, const Generator &generator);
signals:
void result(PtrRoutingData pTransects);
......@@ -48,7 +55,7 @@ private:
mutable std::mutex _mutex;
mutable std::condition_variable _cv;
// Internal data
snake::FPolygon _safeArea;
RoutingParameter _par;
Generator _generator; // transect generator
// State
std::atomic_bool _calculating;
......
src/Wima/Snake/snake.cpp
View file @ 9282a10c
......@@ -627,8 +627,8 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
}
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool()> stop, string &errorString) {
std::vector<RouteInfo> &routeInfoVector,
std::vector<Route> &routeVector, const RouteParameter &par) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
......@@ -761,7 +761,7 @@ bool route(const FPolygon &area, const Transects &transects,
if (std::isinf(dist)) {
std::vector<std::size_t> route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
errorString = "Distance matrix calculation failed.";
par.errorString = "Distance matrix calculation failed.";
return false;
}
(void)route;
......@@ -833,9 +833,11 @@ bool route(const FPolygon &area, const Transects &transects,
auto searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
// Number of solutions.
searchParameters.set_number_of_solutions_to_collect(par.numSolutionsPerRun);
// Set costume limit.
auto *solver = routing.solver();
auto *limit = solver->MakeCustomLimit(stop);
auto *limit = solver->MakeCustomLimit(par.stop);
routing.AddSearchMonitor(limit);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
......@@ -849,97 +851,128 @@ bool route(const FPolygon &area, const Transects &transects,
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment *solution = routing.SolveWithParameters(searchParameters);
auto pSolutions = std::make_unique<std::vector<const Assignment *>>();
(void)routing.SolveWithParameters(searchParameters, pSolutions.get());
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "solve routing model: " << 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.";
if (par.stop()) {
par.errorString = "User terminated.";
return false;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
// Create index list.
auto index = routing.Start(0);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = solution->Value(routing.NextVar(index));
long long counter = -1;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin();
--solution) {
++counter;
if (!*solution || (*solution)->Size() <= 1) {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << "invalid."
<< std::endl;
par.errorString = ss.str();
continue;
}
//================================================================
// Construc route.
//================================================================
// Create index list.
auto index = routing.Start(0);
std::vector<size_t> 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_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) {
errorString = "Error while assembling route.";
return false;
}
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.
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];
transectInfo.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);
// 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);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
r.push_back(*it);
}
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
r.push_back(*it);
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString << "Error while assembling route " << counter
<< "." << 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);
}
}
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph, nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
errorString = "Error while assembling route.";
return false;
}
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) {
std::stringstream ss;
ss << par.errorString << "Route " << counter << " empty." << std::endl;
par.errorString = ss.str();
continue;
}
routeVector.push_back(std::move(r));
routeInfoVector.push_back(std::move(routeInfo));
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
transectInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed);
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (r.size() < 2) {
errorString = "Route empty.";
if (routeVector.size() > 0 && routeVector.size() == routeInfoVector.size()) {
return true;
} else {
return false;
}
return true;
}
bool route_old(const FPolygon &area, const Transects &transects,
......@@ -1156,13 +1189,6 @@ bool route_old(const FPolygon &area, const Transects &transects,
return route_old(area, transects, transectInfo, r, stop, errorString);
}
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
string &errorString) {
auto stop = [] { return false; };
return route(area, transects, transectInfo, r, stop, errorString);
}
FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); }
FPoint int2Float(const IPoint &ip, IntType scale) {
......
src/Wima/Snake/snake.h
View file @ 9282a10c
......@@ -228,12 +228,19 @@ struct TransectInfo {
size_t index;
bool reversed;
};
using RouteInfo = std::vector<TransectInfo>;
struct RouteParameter {
RouteParameter()
: numSolutionsPerRun(1), numRuns(1), stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
string &errorString);
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool(void)> stop, string &errorString);
std::vector<RouteInfo> &routeInfoVector,
std::vector<Route> &routeVector,
const RouteParameter &par = RouteParameter());
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
......
src/Wima/WimaController.cc
View file @ 9282a10c
......@@ -798,14 +798,15 @@ void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) {
// Copy waypoints to waypoint manager.
_snakeWM.clear();
if (data->route.size() > 0) {
if (data->routeVector.size() > 0 && data->routeVector.front().size() > 0 &&
data->routeInfoVector.size() > 0) {
// Store route.
const auto &transectsENU = data->transects;
const auto &transectsInfo = data->transectsInfo;
const auto &route = data->route;
const auto &routeInfo = data->routeInfoVector.front();
const auto &route = data->routeVector.front();
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst = transectsInfo.front();
const auto &infoFirst = routeInfo.front();
const auto &firstTransect = transectsENU[infoFirst.index];
const auto &firstWaypoint =
infoFirst.reversed ? firstTransect.back() : firstTransect.front();
......@@ -819,7 +820,7 @@ void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) {
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.back();
const auto &infoLast = routeInfo.back();
const auto &lastTransect = transectsENU[infoLast.index];
const auto &lastWaypoint =
infoLast.reversed ? lastTransect.front() : lastTransect.back();
......@@ -924,11 +925,12 @@ void WimaController::_updateRoute() {
}
if (numTiles > 0) {
// Fetch safe area and convert to ENU.
RoutingParameter par;
auto safeArea = this->_joinedArea.coordinateList();
for (auto &v : safeArea) {
v.setAltitude(0);
}
snake::FPolygon safeAreaENU;
auto &safeAreaENU = par.safeArea;
snake::areaToEnu(origin, safeArea, safeAreaENU);
const auto &depot = this->_serviceArea.depot();
snake::FPoint depotENU;
......@@ -988,7 +990,7 @@ void WimaController::_updateRoute() {
snake::FLineString transect;
for (const auto &v : child->Contour) {
snake::FPoint c{static_cast<double>(v.X) / CLIPPER_SCALE,
static_cast<double>(v.Y) / CLIPPER_SCALE};
static_cast<double>(v.Y) / CLIPPER_SCALE};
transect.push_back(c);
}
......@@ -1002,7 +1004,7 @@ void WimaController::_updateRoute() {
return false;
}
}; // generator
this->_routingThread.route(safeAreaENU, generator);
this->_routingThread.route(par, generator);
} else {
this->_storeRoute(RoutingThread::PtrRoutingData(new RoutingData()));
}
......
src/Wima/json/CircularSurvey.SettingsGroup.json
View file @ 9282a10c
......@@ -31,8 +31,12 @@
"name": "Type",
"shortDescription": "Survey Type.",
"type": "uint64",
"min": 0,
"max": 1,
"defaultValue": 0
},
{
"name": "Variant",
"shortDescription": "Route variant.",
"type": "uint64",
"defaultValue": 0
}
]
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment