Commit 990aa5eb authored by Valentin Platzgummer's avatar Valentin Platzgummer

temp

parent c6e6a519
#include <algorithm>
#include <iostream> #include <iostream>
#include "snake.h" #include "snake.h"
...@@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) { ...@@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) {
center.set<1>(c.y); center.set<1>(c.y);
} }
void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
/* /*
Find the minimum-area bounding box of a set of 2D points Find the minimum-area bounding box of a set of 2D points
...@@ -94,8 +95,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { ...@@ -94,8 +95,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
POSSIBILITY OF SUCH DAMAGE. POSSIBILITY OF SUCH DAMAGE.
*/ */
if (polygon.outer().empty()) if (polygon.outer().empty() || polygon.outer().size() < 3)
return; return false;
BoostPolygon convex_hull; BoostPolygon convex_hull;
bg::convex_hull(polygon, convex_hull); bg::convex_hull(polygon, convex_hull);
...@@ -198,6 +199,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { ...@@ -198,6 +199,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
BoostPolygon rotated_polygon; BoostPolygon rotated_polygon;
bg::transform(minBBox.corners, rotated_polygon, rotate); bg::transform(minBBox.corners, rotated_polygon, rotate);
minBBox.corners = rotated_polygon; minBBox.corners = rotated_polygon;
return true;
} }
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
...@@ -438,19 +441,21 @@ const BoostPoint &Scenario::homePositon() const { return _homePosition; } ...@@ -438,19 +441,21 @@ const BoostPoint &Scenario::homePositon() const { return _homePosition; }
bool Scenario::update() { bool Scenario::update() {
if (!_needsUpdate) if (!_needsUpdate)
return true; return true;
bg::correct(_mArea);
bg::correct(_sArea);
bg::correct(_corridor);
if (!_calculateJoinedArea())
return false;
if (!_calculateBoundingBox()) if (!_calculateBoundingBox())
return false; return false;
if (!_calculateTiles()) if (!_calculateTiles())
return false; return false;
if (!_calculateJoinedArea())
return false;
_needsUpdate = false; _needsUpdate = false;
return true; return true;
} }
bool Scenario::_calculateBoundingBox() { bool Scenario::_calculateBoundingBox() {
minimalBoundingBox(_mArea, _mAreaBoundingBox); return minimalBoundingBox(_mArea, _mAreaBoundingBox);
return true;
} }
/** /**
...@@ -474,9 +479,12 @@ bool Scenario::_calculateTiles() { ...@@ -474,9 +479,12 @@ bool Scenario::_calculateTiles() {
_tileCenterPoints.clear(); _tileCenterPoints.clear();
if (_tileWidth <= 0 * bu::si::meter || _tileHeight <= 0 * bu::si::meter || if (_tileWidth <= 0 * bu::si::meter || _tileHeight <= 0 * bu::si::meter ||
_minTileArea <= 0 * bu::si::meter * bu::si::meter) { _minTileArea < 0 * bu::si::meter * bu::si::meter) {
errorString = std::stringstream ss;
"Parameters tileWidth, tileHeight, minTileArea must be positive."; ss << "Parameters tileWidth (" << _tileWidth << "), tileHeight ("
<< _tileHeight << "), minTileArea (" << _minTileArea
<< ") must be positive.";
errorString = ss.str();
return false; return false;
} }
...@@ -504,9 +512,8 @@ bool Scenario::_calculateTiles() { ...@@ -504,9 +512,8 @@ bool Scenario::_calculateTiles() {
if (iMax < 1 || jMax < 1) { if (iMax < 1 || jMax < 1) {
std::stringstream ss; std::stringstream ss;
ss << "Tile width or Tile height to large. " ss << "Tile width (" << _tileWidth << ") or tile height (" << _tileHeight
<< "tile width = " << _tileWidth << ", " << ") to large for measurement area.";
<< "tile height = " << _tileHeight << "." << std::endl;
errorString = ss.str(); errorString = ss.str();
return false; return false;
} }
...@@ -554,8 +561,10 @@ bool Scenario::_calculateTiles() { ...@@ -554,8 +561,10 @@ bool Scenario::_calculateTiles() {
} }
if (_tiles.size() < 1) { if (_tiles.size() < 1) {
errorString = std::stringstream ss;
"No tiles calculated. Is the minTileArea parameter large enough?"; ss << "No tiles calculated. Is the minTileArea (" << _minTileArea
<< ") parameter large enough?";
errorString = ss.str();
return false; return false;
} }
...@@ -590,7 +599,21 @@ bool Scenario::_calculateJoinedArea() { ...@@ -590,7 +599,21 @@ bool Scenario::_calculateJoinedArea() {
} else if (corridor_is_connection) { } else if (corridor_is_connection) {
bg::union_(partialArea, _corridor, sol); bg::union_(partialArea, _corridor, sol);
} else { } else {
errorString = "Areas are not overlapping"; std::stringstream ss;
auto printPoint = [&ss](const BoostPoint &p) {
ss << " (" << p.get<0>() << ", " << p.get<1>() << ")";
};
ss << "Areas are not overlapping." << std::endl;
ss << "Measurement area:";
bg::for_each_point(_mArea, printPoint);
ss << std::endl;
ss << "Service area:";
bg::for_each_point(_sArea, printPoint);
ss << std::endl;
ss << "Corridor:";
bg::for_each_point(_corridor, printPoint);
ss << std::endl;
errorString = ss.str();
return false; return false;
} }
...@@ -872,21 +895,16 @@ bool flight_plan::route(const BoostPolygon &area, ...@@ -872,21 +895,16 @@ bool flight_plan::route(const BoostPolygon &area,
BoostLineString vertices; BoostLineString vertices;
size_t n0 = 0; size_t n0 = 0;
for (const auto &t : transects) { for (const auto &t : transects) {
if (t.size() >= 2) { n0 += std::min<std::size_t>(t.size(), 2);
n0 += 2;
} else {
n0 += 1;
}
} }
vertices.reserve(n0); vertices.reserve(n0);
struct TransectInfo { struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {} TransectInfo(size_t n, bool f) : index(n), front(f) {}
size_t index; size_t index;
bool front; bool front;
}; };
std::vector<TransectInfo> transectInfoList; std::vector<TransectInfo> transectInfoList;
size_t idx = 0;
for (size_t i = 0; i < transects.size(); ++i) { for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i]; const auto &t = transects[i];
vertices.push_back(t.front()); vertices.push_back(t.front());
...@@ -1038,6 +1056,7 @@ bool flight_plan::route(const BoostPolygon &area, ...@@ -1038,6 +1056,7 @@ bool flight_plan::route(const BoostPolygon &area,
idx2Vertex(idxList, route); idx2Vertex(idxList, route);
} }
} }
return true;
} }
} // namespace snake } // namespace snake
......
...@@ -97,10 +97,10 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) { ...@@ -97,10 +97,10 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth); origin.altitude(), earth);
double x, y, z; double x = 0, y = 0, z = 0;
proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z); proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
out.set<0>(x); out.set<0>(x);
out.set<0>(y); out.set<1>(y);
(void)z; (void)z;
} }
template <class GeoPoint> template <class GeoPoint>
...@@ -110,7 +110,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { ...@@ -110,7 +110,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth); origin.altitude(), earth);
double lat, lon, alt; double lat = 0, lon = 0, alt = 0;
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
out.setLatitude(lat); out.setLatitude(lat);
out.setLongitude(lon); out.setLongitude(lon);
...@@ -118,7 +118,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { ...@@ -118,7 +118,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
} }
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center); void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox);
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
double offset); double offset);
...@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects, ...@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects,
namespace detail { namespace detail {
const double offsetConstant = const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies. 0.5; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail } // namespace detail
} // namespace snake } // namespace snake
......
...@@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea( ...@@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea(
const QList<QGeoCoordinate> &measurementArea) { const QList<QGeoCoordinate> &measurementArea) {
UniqueLock lk(this->pImpl->mutex); UniqueLock lk(this->pImpl->mutex);
this->pImpl->mArea = measurementArea; this->pImpl->mArea = measurementArea;
for (auto &vertex : this->pImpl->mArea) {
vertex.setAltitude(0);
}
} }
void SnakeDataManager::setServiceArea( void SnakeDataManager::setServiceArea(
const QList<QGeoCoordinate> &serviceArea) { const QList<QGeoCoordinate> &serviceArea) {
UniqueLock lk(this->pImpl->mutex); UniqueLock lk(this->pImpl->mutex);
this->pImpl->sArea = serviceArea; this->pImpl->sArea = serviceArea;
for (auto &vertex : this->pImpl->sArea) {
vertex.setAltitude(0);
}
} }
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) { void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
UniqueLock lk(this->pImpl->mutex); UniqueLock lk(this->pImpl->mutex);
this->pImpl->corridor = corridor; this->pImpl->corridor = corridor;
for (auto &vertex : this->pImpl->corridor) {
vertex.setAltitude(0);
}
} }
const QmlObjectListModel *SnakeDataManager::tiles() const { const QmlObjectListModel *SnakeDataManager::tiles() const {
...@@ -418,9 +427,21 @@ void SnakeDataManager::disableRosBride() { ...@@ -418,9 +427,21 @@ void SnakeDataManager::disableRosBride() {
} }
void SnakeDataManager::run() { void SnakeDataManager::run() {
#ifndef NDEBUG
auto startTime = std::chrono::high_resolution_clock::now();
qDebug() << "SnakeDataManager::run()";
#endif
this->pImpl->calcInProgress.store(true); this->pImpl->calcInProgress.store(true);
emit calcInProgressChanged(this->pImpl->calcInProgress.load()); emit calcInProgressChanged(this->pImpl->calcInProgress.load());
auto onExit = [this] { auto onExit = [this, &startTime] {
#ifndef NDEBUG
qDebug() << "SnakeDataManager::run() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - startTime)
.count()
<< " ms";
#endif
this->pImpl->calcInProgress.store(false); this->pImpl->calcInProgress.store(false);
emit calcInProgressChanged(this->pImpl->calcInProgress.load()); emit calcInProgressChanged(this->pImpl->calcInProgress.load());
}; };
...@@ -443,6 +464,8 @@ void SnakeDataManager::run() { ...@@ -443,6 +464,8 @@ void SnakeDataManager::run() {
this->pImpl->ENUOrigin = this->pImpl->mArea.front(); this->pImpl->ENUOrigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUOrigin; auto &origin = this->pImpl->ENUOrigin;
qDebug() << "SnakeDataManager::run(): origin: " << origin.latitude() << " "
<< origin.longitude() << " " << origin.altitude();
// Update measurement area. // Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea(); auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->mArea; auto &mArea = this->pImpl->mArea;
...@@ -478,75 +501,7 @@ void SnakeDataManager::run() { ...@@ -478,75 +501,7 @@ void SnakeDataManager::run() {
return; return;
} }
// Asynchronously update flightplan. // Store scenario data.
std::string errorString;
auto future = std::async([this, &errorString, &origin] {
snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle *
degree::degree);
snake::flight_plan::Transects transects;
transects.push_back(bg::model::linestring<snake::BoostPoint>{
this->pImpl->scenario.homePositon()});
bool value = snake::flight_plan::transectsFromScenario(
this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha,
this->pImpl->scenario, this->pImpl->progress, transects, errorString);
if (!value) {
this->pImpl->errorMessage = "Not able to generate transects.";
return value;
}
snake::flight_plan::Transects transectsRouted;
snake::flight_plan::Route route;
value =
snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects,
transectsRouted, route, errorString);
if (!value) {
this->pImpl->errorMessage = "Routing error.";
return value;
}
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->arrivalPathENU.push_back(enuVertex);
this->pImpl->arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->returnPathENU.push_back(enuVertex);
this->pImpl->returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->waypointsENU.push_back(enuVertex);
this->pImpl->waypoints.push_back(geoVertex);
}
return true;
});
// Continue with storing scenario data in the mean time.
{ {
// Get tiles. // Get tiles.
const auto &tiles = this->pImpl->scenario.tiles(); const auto &tiles = this->pImpl->scenario.tiles();
...@@ -576,12 +531,69 @@ void SnakeDataManager::run() { ...@@ -576,12 +531,69 @@ void SnakeDataManager::run() {
} }
} }
future.wait(); // Create transects.
// Trying to generate flight plan. std::string errorString;
if (!future.get()) { snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle *
// error degree::degree);
snake::flight_plan::Transects transects;
transects.push_back(bg::model::linestring<snake::BoostPoint>{
this->pImpl->scenario.homePositon()});
bool value = snake::flight_plan::transectsFromScenario(
this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha,
this->pImpl->scenario, this->pImpl->progress, transects, errorString);
if (!value) {
this->pImpl->errorMessage = errorString.c_str();
return;
}
// Route transects
snake::flight_plan::Transects transectsRouted;
snake::flight_plan::Route route;
value =
snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects,
transectsRouted, route, errorString);
if (!value) {
this->pImpl->errorMessage = errorString.c_str(); this->pImpl->errorMessage = errorString.c_str();
} else { return;
// Success!!! }
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->arrivalPathENU.push_back(enuVertex);
this->pImpl->arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->returnPathENU.push_back(enuVertex);
this->pImpl->returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->waypointsENU.push_back(enuVertex);
this->pImpl->waypoints.push_back(geoVertex);
} }
} }
...@@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; } ...@@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; } Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() { QmlObjectListModel *WimaController::snakeTiles() {
return const_cast<QmlObjectListModel *>(this->_snakeDM.tiles()); return const_cast<QmlObjectListModel *>(this->_currentDM->tiles());
} }
QVariantList WimaController::snakeTileCenterPoints() { QVariantList WimaController::snakeTileCenterPoints() {
...@@ -710,7 +710,8 @@ void WimaController::_initSmartRTL() { ...@@ -710,7 +710,8 @@ void WimaController::_initSmartRTL() {
void WimaController::_DMFinishedHandler() { void WimaController::_DMFinishedHandler() {
if (!_snakeDM.success()) { if (!_snakeDM.success()) {
// qgcApp()->showMessage(r.errorMessage); qDebug() << _snakeDM.errorMessage();
// qgcApp()->showMessage(_snakeDM.errorMessage());
return; return;
} }
...@@ -784,10 +785,12 @@ void WimaController::_progressChangedHandler() { ...@@ -784,10 +785,12 @@ void WimaController::_progressChangedHandler() {
void WimaController::_enableSnakeChangedHandler() { void WimaController::_enableSnakeChangedHandler() {
if (this->_enableSnake.rawValue().toBool()) { if (this->_enableSnake.rawValue().toBool()) {
qDebug() << "WimaController: enabling snake.";
this->_snakeDM.enableRosBridge(); this->_snakeDM.enableRosBridge();
_switchDataManager(_snakeDM); _switchDataManager(_snakeDM);
_currentDM->start(); _currentDM->start();
} else { } else {
qDebug() << "WimaController: disabling snake.";
this->_snakeDM.disableRosBride(); this->_snakeDM.disableRosBride();
_switchDataManager(_emptyDM); _switchDataManager(_emptyDM);
} }
......
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