Newer
Older
#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp"
#include "geometry/snake.h"
#include "nemo_interface/SnakeTile.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
// boost
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
QGC_LOGGING_CATEGORY(MeasurementComplexItemLog, "MeasurementComplexItemLog")
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
const char *MeasurementComplexItem::settingsGroup = "MeasurementComplexItem";
const char *MeasurementComplexItem::jsonComplexItemTypeValue =
"MeasurementComplexItem";
const char *MeasurementComplexItem::variantName = "Variant";
const char *MeasurementComplexItem::altitudeName = "Altitude";
const QString MeasurementComplexItem::name(tr("Measurement"));
MeasurementComplexItem::MeasurementComplexItem(
PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
: ComplexMissionItem(masterController, flyView, settingsGroup, parent),
_masterController(masterController), _sequenceNumber(0),
_followTerrain(false), _state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/MeasurementComplexItem.SettingsGroup.json"),
this)),
_variant(settingsGroup, _metaDataMap[variantName]),
_areaData(new AreaData(this)), _editorData(new AreaData(this)),
_currentData(_areaData), _pWorker(new RoutingThread(this)) {
// Connect facts.
connect(&this->_variant, &Fact::rawValueChanged, this,
connect(this->_pWorker, &RoutingThread::result, this,
connect(this->_pWorker, &RoutingThread::calculatingChanged, this,
auto lg = new routing::LinearGenerator(this->_areaData, this);
auto cg = new routing::CircularGenerator(this->_areaData, this);
qCritical() << "ToDo: _altitude connections missing.";
qCritical() << "ToDo: add generator saveing.";
AreaData *MeasurementComplexItem::areaData() { return this->_currentData; }
QStringList MeasurementComplexItem::variantNames() const {
return _variantNames;
}
bool MeasurementComplexItem::load(const QJsonObject &complexObject,
int sequenceNumber, QString &errorString) {
// We need to pull version first to determine what validation/conversion
// needs to be performed
QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
{JsonHelper::jsonVersionKey, QJsonValue::Double, true},
};
if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList,
errorString)) {
return false;
}
int version = complexObject[JsonHelper::jsonVersionKey].toInt();
if (version != 1) {
errorString = tr("Survey items do not support version %1").arg(version);
return false;
}
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
{ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true},
{variantName, QJsonValue::Double, false},
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}
QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
QString complexType =
complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
if (itemType != VisualMissionItem::jsonTypeComplexItemValue ||
complexType != jsonComplexItemTypeValue) {
errorString = tr("%1 does not support loading this complex mission item "
"type: %2:%3")
.arg(qgcApp()->applicationName())
.arg(itemType)
.arg(complexType);
return false;
}
_ignoreRecalc = true;
setSequenceNumber(sequenceNumber);
if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */,
errorString)) {
_surveyAreaPolygon.clear();
return false;
}
if (!load(complexObject, sequenceNumber, errorString)) {
_ignoreRecalc = false;
return false;
}
_variant.setRawValue(complexObject[variantName].toInt());
_altitude.setRawValue(complexObject[altitudeName].toDouble());
_ignoreRecalc = false;
if (_cameraShots == 0) {
// Shot count was possibly not available from plan file
_recalcCameraShots();
}
return true;
}
bool MeasurementComplexItem::dirty() const { return _dirty; }
bool MeasurementComplexItem::isSimpleItem() const { return false; }
bool MeasurementComplexItem::isStandaloneCoordinate() const { return false; }
QString MeasurementComplexItem::mapVisualQML() const {
return QStringLiteral("MeasurementItemMapVisual.qml");
QJsonObject saveObject;
_save(saveObject);
saveObject[JsonHelper::jsonVersionKey] = 1;
saveObject[VisualMissionItem::jsonTypeKey] =
VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =
jsonComplexItemTypeValue;
saveObject[variantName] = double(_variant.rawValue().toUInt());
saveObject[altitudeName] = double(_altitude.rawValue().toUInt());
// Polygon shape
_surveyAreaPolygon.saveToJson(saveObject);
planItems.append(saveObject);
}
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
double MeasurementComplexItem::amslEntryAlt() const {
return _altitude.rawValue().toDouble() +
this->_masterController->missionController()
->plannedHomePosition()
.altitude();
}
double MeasurementComplexItem::amslExitAlt() const { return amslEntryAlt(); }
double MeasurementComplexItem::minAMSLAltitude() const {
return amslEntryAlt();
}
double MeasurementComplexItem::maxAMSLAltitude() const {
return amslEntryAlt();
}
bool MeasurementComplexItem::specifiesCoordinate() const {
return _route.count() > 0;
}
bool MeasurementComplexItem::specifiesAltitudeOnly() const { return false; }
QGeoCoordinate MeasurementComplexItem::coordinate() const {
return this->_route.size() > 0 ? _route.first() : QGeoCoordinate();
}
QGeoCoordinate MeasurementComplexItem::exitCoordinate() const {
return this->_route.size() > 0 ? _route.last() : QGeoCoordinate();
}
int MeasurementComplexItem::sequenceNumber() const { return _sequenceNumber; }
double MeasurementComplexItem::specifiedFlightSpeed() {
return std::numeric_limits<double>::quiet_NaN();
}
double MeasurementComplexItem::specifiedGimbalYaw() {
return std::numeric_limits<double>::quiet_NaN();
}
double MeasurementComplexItem::specifiedGimbalPitch() {
return std::numeric_limits<double>::quiet_NaN();
}
void MeasurementComplexItem::appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
qCDebug(MeasurementComplexItemLog) << "appendMissionItems()";
int seqNum = this->_sequenceNumber;
MAV_FRAME mavFrame =
followTerrain() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
int transectIndex = 0;
for (const auto &vertex : this->_route) {
MissionItem *item = new MissionItem(
seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
0, // hold time
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
vertex.latitude(), vertex.longitude(), vertex.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
void MeasurementComplexItem::setMissionFlightStatus(
const MissionController::MissionFlightStatus_t &missionFlightStatus) {
ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
void MeasurementComplexItem::applyNewAltitude(double newAltitude) {
this->_altitude->setRawValue(newAltitude);
}
double MeasurementComplexItem::additionalTimeDelay() const { return 0; }
bool MeasurementComplexItem::_setGenerator(PtrGenerator newG) {
if (this->_pGenerator != newG) {
if (this->_pGenerator != nullptr) {
disconnect(this->_pGenerator, &routing::GeneratorBase::generatorChanged,
connect(this->_pGenerator, &routing::GeneratorBase::generatorChanged, this,
emit generatorChanged();
this->_setState(STATE::IDLE);
return true;
} else {
return false;
}
}
void MeasurementComplexItem::_setState(MeasurementComplexItem::STATE state) {
if (this->_state != state) {
auto oldState = this->_state;
this->_state = state;
if (_calculating(oldState) != _calculating(state)) {
emit calculatingChanged();
}
if (_editing(oldState) != _editing(state)) {
emit editingChanged();
}
bool MeasurementComplexItem::_calculating(
MeasurementComplexItem::STATE state) const {
void MeasurementComplexItem::_setAreaData(
MeasurementComplexItem::PtrAreaData data) {
if (_currentData != data) {
_currentData = data;
emit areaDataChanged();
}
}
// Reset data.
this->_transects.clear();
this->_variantVector.clear();
this->_variantNames.clear();
emit variantNamesChanged();
if (this->_areaData->isValid()) {
// Prepare data.
auto origin = this->_areaData->origin();
origin.setAltitude(0);
if (!origin.isValid()) {
<< "_updateWorker(): origin invalid." << origin;
return false;
}
// Convert safe area.
auto serviceArea =
getGeoArea<const SafeArea *>(*this->_areaData->areaList());
auto geoSafeArea = serviceArea->coordinateList();
<< "_updateWorker(): safe area invalid." << geoSafeArea;
return false;
}
for (auto &v : geoSafeArea) {
if (v.isValid()) {
v.setAltitude(0);
} else {
<< "_updateWorker(): safe area contains invalid coordinate."
<< geoSafeArea;
return false;
}
}
// Routing par.
RoutingParameter par;
par.numSolutions = 5;
auto &safeAreaENU = par.safeArea;
snake::areaToEnu(origin, geoSafeArea, safeAreaENU);
// Create generator.
if (this->_pGenerator) {
routing::GeneratorBase::Generator g; // Transect generator.
if (this->_pGenerator->get(g)) {
// Start/Restart routing worker.
this->_pWorker->route(par, g);
return true;
} else {
<< "_updateWorker(): generator creation failed.";
return false;
}
} else {
<< "_updateWorker(): pGenerator == nullptr, number of registered "
"generators: "
<< this->_generatorList.size();
return false;
}
} else {
qCDebug(MeasurementComplexItemLog) << "_updateWorker(): plan data invalid.";
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
auto variant = this->_variant.rawValue().toUInt();
// Find old variant and run. Old run corresponts with empty list.
std::size_t old_variant = std::numeric_limits<std::size_t>::max();
for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) {
const auto &variantCoordinates = this->_variantVector.at(i);
if (variantCoordinates.isEmpty()) {
old_variant = i;
break;
}
}
// Swap route.
if (variant != old_variant) {
// Swap in new variant.
if (variant < std::size_t(this->_variantVector.size())) {
if (old_variant != std::numeric_limits<std::size_t>::max()) {
// this->_transects containes a route, swap it back to
// this->_solutionVector
auto &oldVariantCoordinates = this->_variantVector[old_variant];
oldVariantCoordinates.swap(this->_transects);
}
auto &newVariantCoordinates = this->_variantVector[variant];
this->_transects.swap(newVariantCoordinates);
} else { // error
<< "Variant out of bounds (variant =" << variant << ").";
qCDebug(MeasurementComplexItemLog) << "Resetting variant to zero.";
disconnect(&this->_variant, &Fact::rawValueChanged, this,
this->_variant.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
if (this->_variantVector.size() > 0) {
this->_changeVariantWorker();
}
}
}
}
if (this->_transects.size() > 0) {
auto &t = this->_transects.front();
std::reverse(t.begin(), t.end());
}
}
double MeasurementComplexItem::timeBetweenShots() { return 0; }
QString MeasurementComplexItem::commandDescription() const {
return tr("Route");
}
QString MeasurementComplexItem::commandName() const { return tr("Route"); }
QString MeasurementComplexItem::abbreviation() const { return tr("R"); }
TransectStyleComplexItem::ReadyForSaveState
if (TransectStyleComplexItem::readyForSaveState() ==
TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) {
if (this->_state == STATE::IDLE) {
return ReadyForSaveState::ReadyForSave;
} else {
return ReadyForSaveState::NotReadyForSaveData;
}
} else {
return TransectStyleComplexItem::readyForSaveState();
}
}
bool MeasurementComplexItem::exitCoordinateSameAsEntry() const {
return this->_route.size() > 0 ? this->_route.first() == this->_route.last()
: false;
}
void MeasurementComplexItem::setDirty(bool dirty) {
if (this->_dirty != dirty) {
this->_dirty = dirty;
emit dirtyChanged(this->_dirty);
}
}
void MeasurementComplexItem::setCoordinate(const QGeoCoordinate &coordinate) {
Q_UNUSED(coordinate);
}
void MeasurementComplexItem::setSequenceNumber(int sequenceNumber) {
if (this->_sequenceNumber != sequenceNumber) {
this->_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(this->_sequenceNumber);
}
}
QString MeasurementComplexItem::patternName() const { return name; }
bool MeasurementComplexItem::registerGenerator(const QString &name,
routing::GeneratorBase *g) {
qCDebug(MeasurementComplexItemLog)
<< "registerGenerator(): empty name string.";
qCDebug(MeasurementComplexItemLog)
<< "registerGenerator(): empty generator.";
return false;
}
if (this->_generatorNameList.contains(name)) {
qCDebug(MeasurementComplexItemLog) << "registerGenerator(): generator "
"already registered.";
return false;
} else {
this->_generatorNameList.push_back(name);
this->_generatorList.push_back(g);
if (this->_generatorList.size() == 1) {
}
emit generatorNameListChanged();
return true;
}
}
bool MeasurementComplexItem::unregisterGenerator(const QString &name) {
auto index = this->_generatorNameList.indexOf(name);
if (index >= 0) {
// Is this the current generator?
const auto &g = this->_generatorList.at(index);
if (g == this->_pGenerator) {
if (index > 0) {
_setGenerator(this->_generatorList.at(index - 1));
<< "unregisterGenerator(): last generator unregistered.";
}
}
this->_generatorNameList.removeAt(index);
auto gen = this->_generatorList.takeAt(index);
gen->deleteLater();
emit generatorNameListChanged();
return true;
} else {
<< "unregisterGenerator(): generator " << name << " not registered.";
return false;
}
}
bool MeasurementComplexItem::unregisterGenerator(int index) {
if (index > 0 && index < this->_generatorNameList.size()) {
return unregisterGenerator(this->_generatorNameList.at(index));
} else {
qCDebug(MeasurementComplexItemLog)
<< "unregisterGenerator(): index (" << index
<< ") out"
"of bounds ( "
<< this->_generatorList.size() << " ).";
bool MeasurementComplexItem::switchToGenerator(const QString &name) {
auto index = this->_generatorNameList.indexOf(name);
if (index >= 0) {
_setGenerator(this->_generatorList.at(index));
<< "switchToGenerator(): generator " << name << " not registered.";
return false;
}
}
_setGenerator(this->_generatorList.at(index));
qCDebug(MeasurementComplexItemLog)
<< "unregisterGenerator(): index (" << index
<< ") out"
"of bounds ( "
<< this->_generatorNameList.size() << " ).";
routing::GeneratorBase *MeasurementComplexItem::generator() {
return _pGenerator;
}
return this->_generatorList.indexOf(this->_pGenerator);
}
void MeasurementComplexItem::editingStart() {
if (!_editing(this->_state)) {
*_editorData = *_areaData;
_setAreaData(_editorData);
void MeasurementComplexItem::editingStop() {
if (_editing(this->_state)) {
if (_editorData->isValid()) {
*_areaData = *_editorData;
}
_setAreaData(_areaData);
auto start = std::chrono::high_resolution_clock::now();
switch (this->_state) {
case STATE::SKIPP:
qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: skipp.";
this->_setState(STATE::IDLE);
break;
case STATE::CHANGE_VARIANT:
qCDebug(MeasurementComplexItemLog)
<< "rebuildTransectsPhase1: variant change.";
this->_changeVariantWorker();
this->_setState(STATE::IDLE);
break;
case STATE::REVERT_PATH:
qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: reverse.";
this->_reverseWorker();
this->_setState(STATE::IDLE);
break;
case STATE::IDLE:
case STATE::ROUTING:
this->_setState(STATE::ROUTING);
qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: update.";
if (!this->_updateRouteWorker()) {
this->_setState(STATE::IDLE);
}
break;
}
<< "rebuildTransectsPhase1(): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
void MeasurementComplexItem::_recalcCameraShots() { _cameraShots = 0; }
void MeasurementComplexItem::_setTransects(
MeasurementComplexItem::PtrRoutingData pRoute) {
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
// Store solutions.
auto ori = this->_areaData->origin();
ori.setAltitude(0);
const auto &transectsENU = pRoute->transects;
QVector<Variant> variantVector;
const auto nSolutions = pRoute->solutionVector.size();
for (std::size_t j = 0; j < nSolutions; ++j) {
Variant var{QList<CoordInfo_t>()};
const auto &solution = pRoute->solutionVector.at(j);
if (solution.size() > 0) {
const auto &route = solution.at(0);
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 = info.at(1);
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) {
idxFirst = i;
break;
}
}
// 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 = var.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 {
<< "_setTransects(): lastTransect.size() == 0";
}
} else {
<< "_setTransects(): firstTransect.size() == 0";
}
} else {
<< "_setTransects(): transectsInfo.size() <= 1";
}
} else {
qCDebug(MeasurementComplexItemLog)
<< "_setTransects(): solution.size() == 0";
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
}
if (var.size() > 0 && var.front().size() > 0) {
variantVector.push_back(std::move(var));
}
}
// Assign routes if no error occured.
if (variantVector.size() > 0) {
// Swap first route to _transects.
this->_variantVector.swap(variantVector);
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
_loadedMissionItems.clear();
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
// Add route variant names.
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();
disconnect(&this->_variant, &Fact::rawValueChanged, this,
this->_variant.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
this->_changeVariantWorker();
this->_setState(STATE::SKIPP);
<< "_setTransects(): failed, variantVector empty.";
this->_setState(STATE::IDLE);
}
}
Fact *MeasurementComplexItem::variant() { return &_variant; }
Fact *MeasurementComplexItem::altitude() { return &this->_altitude; }
return this->_calculating(this->_state);
}
bool MeasurementComplexItem::editing() const { return _editing(this->_state); }
bool MeasurementComplexItem::followTerrain() const { return _followTerrain; }