Commit 9282a10c authored by Valentin Platzgummer's avatar Valentin Platzgummer

route variants added to circular survey

parent 053f9768
{
"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
}
......@@ -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})
}
}
}
}
......
......@@ -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);