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
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 881 additions and 303 deletions
Paths/temp0.plan 0 → 100644
View file @ 9282a10c
This diff is collapsed.
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
This diff is collapsed.
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