Skip to content

Q
qgroundcontrol
Commit 65210eff authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

some bugs solved

parent 5e62e791
Hide whitespace changes
Inline Side-by-side
Showing with 97 additions and 67 deletions
src/Wima/CircularSurvey.cc
View file @ 65210eff
......@@ -36,13 +36,15 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
snake::Angle deltaAlpha, snake::Length minLength,
snake::Transects &transects);
bool circularTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects);
bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
snake::Angle angle, snake::Length minLength,
snake::Transects &transects);
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects);
const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::transectDistanceName = "TransectDistance";
......@@ -84,6 +86,8 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_rebuildTransects);
connect(this, &CircularSurvey::refPointChanged, this,
&CircularSurvey::_rebuildTransects);
connect(this, &CircularSurvey::depotChanged, this,
&CircularSurvey::_rebuildTransects);
connect(&this->_type, &Fact::rawValueChanged, this,
&CircularSurvey::_rebuildTransects);
connect(&this->_variant, &Fact::rawValueChanged, this,
......@@ -137,6 +141,8 @@ QList<QString> CircularSurvey::variantNames() const { return _variantNames; }
QList<QString> CircularSurvey::runNames() const { return _runNames; }
QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }
const QList<QList<QGeoCoordinate>> &CircularSurvey::rawTransects() const {
return this->_rawTransects;
}
......@@ -176,6 +182,15 @@ void CircularSurvey::setJoinedArea(const WimaJoinedArea &jArea) {
}
}
void CircularSurvey::setDepot(const QGeoCoordinate &depot) {
if (this->_depot.latitude() != depot.latitude() ||
this->_depot.longitude() != depot.longitude()) {
this->_depot = depot;
this->_depot.setAltitude(0);
emit depotChanged();
}
}
bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) {
// We need to pull version first to determine what validation/conversion
......@@ -392,6 +407,7 @@ void CircularSurvey::_updateWorker() {
// Progress and tiles.
const auto &progress = this->_mArea.progress();
const auto *tiles = this->_mArea.tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
......@@ -399,32 +415,36 @@ void CircularSurvey::_updateWorker() {
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(ref, tile->coordinateList(), tileENU);
pPolygon->inners().push_back(std::move(tileENU.outer()));
pTiles->push_back(std::move(tileENU));
} else {
qCWarning(CircularSurveyLog)
<< "CS::_updateWorker(): progress.size() != tiles->count().";
<< "_updateWorker(): progress.size() != tiles->count().";
return;
}
}
}
} else {
qCWarning(CircularSurveyLog)
<< "CS::_updateWorker(): progress.size() != tiles->count().";
<< "_updateWorker(): progress.size() != tiles->count().";
return;
}
bg::correct(*pPolygon);
// Convert safe area.
auto geoDepot = this->_jArea.center();
auto geoDepot = this->_depot;
auto geoSafeArea = this->_jArea.coordinateList();
if (!geoDepot.isValid() || !(geoSafeArea.size() >= 3)) {
if (!geoDepot.isValid()) {
qCWarning(CircularSurveyLog)
<< "_updateWorker(): depot invalid." << geoDepot;
return;
}
if (!(geoSafeArea.size() >= 3)) {
qCWarning(CircularSurveyLog)
<< "_updateWorker(): safe area invalid." << geoSafeArea;
return;
}
for (auto &v : geoSafeArea) {
v.setAltitude(0);
}
snake::FPolygon safeArea;
snake::areaToEnu(ref, geoSafeArea, safeArea);
snake::FPoint depot;
snake::toENU(ref, geoDepot, depot);
......@@ -458,11 +478,12 @@ void CircularSurvey::_updateWorker() {
// Clip angle.
if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
alpha <= snake::Angle(45 * bu::degree::degree)) {
auto generator = [depot, pPolygon, distance, alpha,
auto generator = [depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
transects.push_back(snake::FLineString{depot});
return circularTransects(*pPolygon, distance, alpha, minLength,
transects);
bool value = circularTransects(*pPolygon, *pTiles, distance, alpha,
minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
return value;
};
// Start routing worker.
this->_pWorker->route(par, generator);
......@@ -474,10 +495,12 @@ void CircularSurvey::_updateWorker() {
}
}
} else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
auto generator = [depot, pPolygon, distance, alpha,
auto generator = [depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
transects.push_back(snake::FLineString{depot});
return linearTransects(*pPolygon, distance, alpha, minLength, transects);
bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
return value;
};
// Start routing worker.
this->_pWorker->route(par, generator);
......@@ -571,13 +594,7 @@ void CircularSurvey::_changeVariantRunWorker() {
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);
std::reverse(t.begin(), t.end());
}
}
......@@ -595,7 +612,7 @@ void CircularSurvey::_storeWorker() {
const auto &ori = this->_referencePoint;
const auto &transectsENU = pRoutingData->transects;
QList<QList<QGeoCoordinate>> rawTransects;
for (std::size_t i = 0; i < transectsENU.size(); ++i) {
for (std::size_t i = 1; i < transectsENU.size(); ++i) {
const auto &t = transectsENU[i];
rawTransects.append(QList<QGeoCoordinate>());
auto trGeo = rawTransects.back();
......@@ -660,15 +677,15 @@ void CircularSurvey::_storeWorker() {
}
} else {
qCWarning(CircularSurveyLog)
<< "CS::_storeWorker(): lastTransect.size() == 0";
<< "_storeWorker(): lastTransect.size() == 0";
}
} else {
qCWarning(CircularSurveyLog)
<< "CS::_storeWorker(): firstTransect.size() == 0";
<< "_storeWorker(): firstTransect.size() == 0";
}
} else {
qCWarning(CircularSurveyLog)
<< "CS::_storeWorker(): transectsInfo.size() <= 1";
<< "_storeWorker(): transectsInfo.size() <= 1";
}
}
// Remove empty runs.
......@@ -756,31 +773,30 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
switch (this->_state) {
case STATE::STORE:
qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: store.";
qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: store.";
this->_storeWorker();
break;
case STATE::VARIANT_CHANGE:
qCWarning(CircularSurveyLog)
<< "CS::rebuildTransectsPhase1: variant change.";
qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: variant change.";
this->_changeVariantRunWorker();
break;
case STATE::RUN_CHANGE:
qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: run change.";
qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: run change.";
this->_changeVariantRunWorker();
break;
case STATE::REVERSE:
qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: reverse.";
qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: reverse.";
this->_reverseWorker();
break;
case STATE::DEFAULT:
qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: update.";
qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: update.";
this->_updateWorker();
break;
}
this->_state = STATE::DEFAULT;
qCWarning(CircularSurveyLog)
<< "CS::rebuildTransectsPhase1(): "
<< "rebuildTransectsPhase1(): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
......@@ -824,9 +840,10 @@ bool CircularSurvey::calculating() const {
return this->_pWorker->calculating();
}
bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
snake::Angle deltaAlpha, snake::Length minLength,
snake::Transects &transects) {
bool circularTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects) {
auto s1 = std::chrono::high_resolution_clock::now();
// Check preconitions
......@@ -837,7 +854,7 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
std::string error;
// Check validity.
if (!bg::is_valid(polygon, error)) {
qCWarning(CircularSurveyLog) << "CS::circularTransects(): "
qCWarning(CircularSurveyLog) << "circularTransects(): "
"invalid polygon.";
qCWarning(CircularSurveyLog) << error.c_str();
std::stringstream ss;
......@@ -850,7 +867,7 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
std::vector<snake::Angle> angles;
angles.reserve(polygon.outer().size());
//#ifdef DEBUG_CIRCULAR_SURVEY
// qCWarning(CircularSurveyLog) << "CS::circularTransects():";
// qCWarning(CircularSurveyLog) << "circularTransects():";
//#endif
for (const auto &p : polygon.outer()) {
snake::Length distance = bg::distance(origin, p) * si::meter;
......@@ -895,7 +912,7 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
//#ifdef DEBUG_CIRCULAR_SURVEY
// qCWarning(CircularSurveyLog) << "CS::circularTransects(): sector
// qCWarning(CircularSurveyLog) << "circularTransects(): sector
// parameres:"; qCWarning(CircularSurveyLog) << "alpha1: " <<
// to_string(snake::Degree(alpha1)).c_str();
// qCWarning(CircularSurveyLog) << "alpha2:
......@@ -935,16 +952,16 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Subtract holes.
if (polygon.inners().size() > 0) {
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
for (const auto &h : polygon.inners()) {
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : h) {
for (const auto &v : tile.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
}
processedTiles.push_back(path);
processedTiles.push_back(std::move(path));
}
clipper.Clear();
......@@ -1027,7 +1044,7 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
}
qCWarning(CircularSurveyLog)
<< "CS::circularTransects(): transect gen. time: "
<< "circularTransects(): transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
......@@ -1038,9 +1055,10 @@ bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR,
return false;
}
bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
snake::Angle angle, snake::Length minLength,
snake::Transects &transects) {
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects) {
namespace tr = bg::strategy::transform;
auto s1 = std::chrono::high_resolution_clock::now();
......@@ -1053,7 +1071,7 @@ bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
std::stringstream ss;
ss << bg::wkt(polygon);
qCWarning(CircularSurveyLog) << "CS::circularTransects(): "
qCWarning(CircularSurveyLog) << "linearTransects(): "
"invalid polygon. "
<< error.c_str() << ss.str().c_str();
} else {
......@@ -1103,7 +1121,7 @@ bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
ss << "Not able to generate transects. Parameter: distance = "
<< distance << std::endl;
qCWarning(CircularSurveyLog)
<< "CircularSurvey::linearTransects(): " << ss.str().c_str();
<< "linearTransects(): " << ss.str().c_str();
return false;
}
......@@ -1128,16 +1146,16 @@ bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Subtract holes.
if (polygon.inners().size() > 0) {
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
for (const auto &h : polygon.inners()) {
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : h) {
for (const auto &v : tile.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
}
processedTiles.push_back(path);
processedTiles.push_back(std::move(path));
}
clipper.Clear();
......@@ -1168,14 +1186,14 @@ bool linearTransects(const snake::FPolygon &polygon, snake::Length distance,
if (transects.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = "
ss << "Not able to generatetransects. Parameter: minLength = "
<< minLength << std::endl;
qCWarning(CircularSurveyLog)
<< "CircularSurvey::linearTransects(): " << ss.str().c_str();
<< "linearTransects(): " << ss.str().c_str();
return false;
}
qCWarning(CircularSurveyLog)
<< "CS::circularTransects(): transect gen. time: "
<< "linearTransects(): transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
......
src/Wima/CircularSurvey.h
View file @ 65210eff
......@@ -57,6 +57,7 @@ public:
void setJoinedArea(const WimaJoinedAreaData &jArea);
void setMeasurementArea(const WimaMeasurementArea &mArea);
void setJoinedArea(const WimaJoinedArea &jArea);
void setDepot(const QGeoCoordinate &depot);
// Property getters
QGeoCoordinate refPoint() const;
......@@ -155,6 +156,7 @@ private:
// Area data
WimaMeasurementAreaData _mArea;
WimaJoinedAreaData _jArea;
QGeoCoordinate _depot;
// Worker
using PtrWorker = std::shared_ptr<RoutingThread>;
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 65210eff
......@@ -240,10 +240,11 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
}
bool WimaMeasurementArea::setProgress(const QVector<int> &p) {
if (!ready()) {
if (ready()) {
if (p.size() == this->tiles()->count() && this->_progress != p) {
this->_progress = p;
emit progressChanged();
emit progressAccepted();
return true;
}
}
......
src/Wima/Geometry/WimaMeasurementArea.h
View file @ 65210eff
......@@ -92,6 +92,7 @@ signals:
void tilesChanged();
void maxTilesChanged();
void progressChanged();
void progressAccepted();
void readyChanged();
public slots:
......
src/Wima/WimaPlaner.cc
View file @ 65210eff
......@@ -61,6 +61,7 @@ WimaPlaner::WimaPlaner(QObject *parent)
// NemoInterface
connect(&this->_nemoInterface, &NemoInterface::progressChanged, [this] {
this->_measurementArea.setProgress(this->_nemoInterface.progress());
this->_update();
});
// StateMachine
......@@ -340,6 +341,7 @@ void WimaPlaner::_update() {
&WimaPlaner::CSCalculatingChangedHandler);
_survey->setMeasurementArea(this->_measurementArea);
_survey->setJoinedArea(this->_joinedArea);
_survey->setDepot(this->_serviceArea.depot());
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
......@@ -491,6 +493,8 @@ void WimaPlaner::mAreaProgressChangedHandler() {
this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED);
}
void WimaPlaner::mAreaProgressAcceptedHandler() { this->_update(); }
void WimaPlaner::sAreaPathChangedHandler() {
this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED);
}
......@@ -776,6 +780,8 @@ void WimaPlaner::enableMonitoring() {
&WimaPlaner::mAreaTilesChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, this,
&WimaPlaner::mAreaProgressChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted, this,
&WimaPlaner::mAreaProgressAcceptedHandler);
connect(&this->_serviceArea, &WimaArea::pathChanged, this,
&WimaPlaner::sAreaPathChangedHandler);
connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
......
src/Wima/WimaPlaner.h
View file @ 65210eff
......@@ -126,6 +126,7 @@ private slots:
void mAreaPathChangedHandler();
void mAreaTilesChangedHandler();
void mAreaProgressChangedHandler();
void mAreaProgressAcceptedHandler();
void sAreaPathChangedHandler();
void corridorPathChangedHandler();
void depotChangedHandler();
......
src/WimaView/WimaMeasurementAreaMapVisual.qml
View file @ 65210eff
......@@ -132,11 +132,11 @@ Item {
}
}
function getColor(progress) {
if (progress < 25)
if (progress === 0)
return "transparent"
if (progress < 50)
if (progress < 33)
return "orange"
if (progress < 75)
if (progress < 66)
return "yellow"
if (progress < 100)
return "greenyellow"
......@@ -150,7 +150,8 @@ Item {
Item{
property var _tileComponent
property int _progress: _root.areaItem.progress.length
property int _progress: _root.areaItem.progress[index] ?
_root.areaItem.progress[index] : 0
Component.onCompleted: {
_tileComponent = tileComponent.createObject(map)
......
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