Skip to content
GitLab
Commit 01c12f05 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

circular survey: runs working

parent 9282a10c
Expand all Show whitespace changes
Inline Side-by-side
Paths/KlingenbachTest.wima
View file @ 01c12f05
This diff is collapsed.
src/PlanView/CircularSurveyItemEditor.qml
View file @ 01c12f05
......@@ -130,7 +130,7 @@ Rectangle {
columnSpacing: _margin
rowSpacing: _margin
columns: 4
columns: 6
Repeater{
id: variantRepeater
......@@ -141,7 +141,7 @@ Rectangle {
property int len: missionItem.variantNames.length
model: len
QGCRadioButton {
delegate: QGCRadioButton {
checked: index === variantRepeater.variant
text: variantRepeater.names[index]
......@@ -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 {
......
src/Wima/CircularSurvey.cc
View file @ 01c12f05
......@@ -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<RoutingThread>()), _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<QString> CircularSurvey::variantNames() const { return _variantNames; }
QList<QString> CircularSurvey::runNames() const { return _runNames; }
QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }
QList<QGeoCoordinate> 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];
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<std::size_t>::max();
std::size_t old_run = std::numeric_limits<std::size_t>::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;
break;
old_run = j;
// break
i = std::numeric_limits<std::size_t>::max() - 1;
j = std::numeric_limits<std::size_t>::max() - 1;
}
}
}
// Swap route.
if (variant != old_variant || run != old_run) {
// 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]);
if (variant < std::size_t(this->_variantVector.size()) &&
run < std::size_t(this->_variantVector.at(variant).size())) {
if (old_variant != std::numeric_limits<std::size_t>::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,25 +570,29 @@ void CircularSurvey::_storeWorker() {
}
}
// 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 = pRoutingData->routeVector.at(k);
// Store solutions.
QVector<Runs> 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<CoordInfo_t>()});
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 ? transectsInfo.at(1) : transectsInfo.at(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 < 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;
......@@ -498,14 +600,14 @@ void CircularSurvey::_storeWorker() {
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.at(transectsInfo.size() - 2);
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 = 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;
......@@ -513,9 +615,9 @@ void CircularSurvey::_storeWorker() {
}
// Convert to geo coordinates.
auto &list = routes[k].front();
auto &list = runs[k].front();
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto &vertex = route[i];
auto &vertex = path[i];
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
list.append(CoordInfo_t{c, CoordTypeInterior});
......@@ -530,33 +632,59 @@ void CircularSurvey::_storeWorker() {
qWarning() << "CS::_storeWorker(): transectsInfo.size() <= 1";
}
}
// Remove empty routes.
// Remove empty runs.
bool error = true;
std::size_t n = 0;
for (auto it = routes.begin(); it < routes.end();) {
for (auto it = runs.begin(); it < runs.end();) {
if (it->size() > 0 && it->front().size() > 0) {
error = false;
++it;
++n;
} else {
it = routes.erase(it);
it = runs.erase(it);
}
}
if (!error) {
solutionVector.push_back(std::move(runs));
}
}
// Remove empty solutions.
std::size_t nSol = 0;
for (auto it = solutionVector.begin(); it < solutionVector.end();) {
if (it->size() > 0 && it->front().size() > 0) {
++it;
++nSol;
} else {
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: " <<
......
src/Wima/CircularSurvey.h
View file @ 01c12f05
......@@ -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<QString> variantNames READ variantNames NOTIFY variantNamesChanged)
Q_PROPERTY(QList<QString> 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<QString> variantNames() const;
QList<QString> runNames() const;
QGeoCoordinate depot() const;
QList<QGeoCoordinate> safeArea() const;
const QList<QList<QGeoCoordinate>> &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<MissionItem *> &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<QString> _variantNames;
SettingsFact _numRuns;
SettingsFact _run;
QList<QString> _runNames;
// Worker
using PtrWorker = std::shared_ptr<RoutingThread>;
PtrWorker _pWorker;
PtrRoutingData _workerOutput;
PtrRoutingData _pRoutingData;
// Data and State.
// Routing data.
QGeoCoordinate _depot;
QList<QGeoCoordinate> _safeArea;
QList<QList<QGeoCoordinate>> _rawTransects;
QVector<Transects> _routes;
using Runs = QVector<Transects>;
QVector<Runs> _variantVector;
// State.
enum class STATE {
DEFAULT,
STORE,
REVERSE,
VARIANT_CHANGE,
RUN_CHANGE,
};
STATE _state;
......
src/Wima/RoutingThread.cpp
View file @ 01c12f05
......@@ -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.";
......
src/Wima/RoutingThread.h
View file @ 01c12f05
......@@ -12,8 +12,7 @@
struct RoutingData {
snake::Transects transects;
std::vector<snake::Route> routeVector;
std::vector<snake::RouteInfo> routeInfoVector;
std::vector<snake::Solution> solutionVector;
std::string errorString;
};
......
src/Wima/Snake/snake.cpp
View file @ 01c12f05
......@@ -627,8 +627,7 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
}
bool route(const FPolygon &area, const Transects &transects,
std::vector<RouteInfo> &routeInfoVector,
std::vector<Route> &routeVector, const RouteParameter &par) {
std::vector<Solution> &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<std::size_t>(1, par.minNumTransectsPerRun);
auto maxRuns = std::max<std::size_t>(
1, std::floor(double(transects.size()) / minNumTransectsPerRun));
auto numRuns = std::max<std::size_t>(1, par.numRuns);
numRuns = std::min<std::size_t>(numRuns, maxRuns);
RoutingIndexManager::NodeIndex depot(0);
RoutingIndexManager manager(nNodes, numVehicles, depot);
// std::vector<RoutingIndexManager::NodeIndex> 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<std::size_t>(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,11 +905,14 @@ bool route(const FPolygon &area, const Transects &transects,
par.errorString = ss.str();
continue;
}
//================================================================
// Construc route.
//================================================================
// 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(0);
auto index = routing.Start(vehicle);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
......@@ -901,32 +930,33 @@ bool route(const FPolygon &area, const Transects &transects,
}
std::cout << std::endl;
#endif
if (route_idx.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Error while assembling route " << counter << "."
<< std::endl;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
// Construct route.
// Assemble route.
Route r;
RouteInfo routeInfo;
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];
routeInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed);
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) {
r.push_back(*it);
path.push_back(*it);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
r.push_back(*it);
path.push_back(*it);
}
}
// Connect transects.
......@@ -935,8 +965,9 @@ bool route(const FPolygon &area, const Transects &transects,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString << "Error while assembling route " << counter
<< "." << std::endl;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
......@@ -945,248 +976,42 @@ bool route(const FPolygon &area, const Transects &transects,
}
for (auto idx : idxList) {
auto p = int2Float(vertices[idx]);
r.push_back(p);
path.push_back(p);
}
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
routeInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed);
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
if (r.size() < 2 || routeInfo.size() < 2) {
if (path.size() < 2 || info.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Route " << counter << " empty." << std::endl;
ss << par.errorString << "Route empty (solution = " << counter
<< ", run = " << vehicle << ")." << 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<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (routeVector.size() > 0 && routeVector.size() == routeInfoVector.size()) {
return true;
if (routeVector.size() > 0) {
solutionVector.push_back(std::move(routeVector));
} else {
return false;
}
}
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool()> 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<std::size_t>(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<LocalInfo> 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]);
std::stringstream ss;
ss << par.errorString << "Solution " << counter << " empty." << std::endl;
par.errorString = ss.str();
}
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
Matrix<double> 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::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time graphFromPolygon(): " << delta.count() << " ms"
<< endl;
#endif
Matrix<double> 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::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
<< endl;
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
Matrix<int64_t> 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;
if (solutionVector.size() > 0) {
return true;
} 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::milliseconds>(
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<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());
}
// Helper Lambda.
auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
std::vector<FPoint> &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<size_t> 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> &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); }
......
src/Wima/Snake/snake.h
View file @ 01c12f05
......@@ -215,7 +215,6 @@ bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
using Transects = vector<FLineString>;
using Progress = vector<int>;
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<TransectInfo>;
struct Route {
FLineString path;
std::vector<TransectInfo> info;
};
using Solution =
std::vector<Route>; // 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<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<RouteInfo> &routeInfoVector,
std::vector<Route> &routeVector,
std::vector<Solution> &solutionVector,
const RouteParameter &par = RouteParameter());
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
string &errorString);
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool(void)> stop, string &errorString);
namespace detail {
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
......
src/Wima/WimaController.cc
View file @ 01c12f05
......@@ -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);
......
src/Wima/json/CircularSurvey.SettingsGroup.json
View file @ 01c12f05
......@@ -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
}
]
src/WimaView/WimaMeasurementAreaEditor.qml
View file @ 01c12f05
......@@ -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")
......
Supports Markdown
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