Skip to content

Q
qgroundcontrol
Commit 625b439c authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

123

parent 7271b853
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 830 additions and 583 deletions
src/Wima/CS_old.cc 0 → 100644
View file @ 625b439c
This diff is collapsed.
src/Wima/CircularSurvey.cc
View file @ 625b439c
......@@ -387,11 +387,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
snake::BoostPolygon polygonENU;
snake::BoostPoint originENU{0, 0};
snake::areaToEnu(origin, polygon, polygonENU);
std::string error;
// Check validity.
if (!bg::is_valid(polygonENU)) {
if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"invalid polygon.";
qWarning() << error.c_str();
#endif
return PtrTransects();
} else {
......@@ -401,21 +403,27 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
std::vector<snake::Angle> angles;
angles.reserve(polygonENU.outer().size());
for (const auto &p : polygonENU.outer()) {
distances.push_back(bg::distance(originENU, p) * si::meter);
angles.push_back((std::atan2(p.get<1>(), p.get<0>()) + M_PI) *
si::radian);
snake::Length distance = bg::distance(originENU, p) * si::meter;
distances.push_back(distance);
snake::Angle alpha =
(std::atan2(p.get<1>(), p.get<0>())) * si::radian;
alpha =
alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
angles.push_back(alpha);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "distances, angles, coordinates:";
qWarning() << to_string(distance).c_str();
qWarning() << to_string(snake::Degree(alpha)).c_str();
qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
#endif
}
auto rMin = deltaR; // minimal circle radius
snake::Angle alpha1(0 * degree::degree);
snake::Angle alpha2(360 * degree::degree);
bool refInside = true;
// Determine r_min by successive approximation
if (!bg::within(originENU, polygonENU)) {
rMin = bg::distance(originENU, polygonENU) * si::meter;
alpha1 = (*std::min_element(angles.begin(), angles.end()));
alpha2 = (*std::max_element(angles.begin(), angles.end()));
refInside = false;
}
auto rMax =
......@@ -431,44 +439,31 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
ClipperLib::cInt(std::round(originENU.get<0>())),
ClipperLib::cInt(std::round(originENU.get<1>()))};
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto s1 = std::chrono::high_resolution_clock::now();
#endif
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::floor(((rMax - rMin) / deltaR).value()));
const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "sector parameres:";
qWarning() << "alpha1: " << to_string(alpha1).c_str();
qWarning() << "alpha2: " << to_string(alpha2).c_str();
qWarning() << "alpha1: " << 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: " << to_string(rMin).c_str();
qWarning() << "rMax: " << to_string(rMax).c_str();
qWarning() << "nTran: " << nTran;
qWarning() << "refInside: " << refInside;
#endif
using ClipperCircle =
GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
// Helper lambda.
std::function<void(ClipperCircle & circle,
ClipperLib::Path & sectors)>
approx;
if (refInside) {
approx = [nSectors](ClipperCircle &circle,
ClipperLib::Path &sector) {
approximate(circle, nSectors, sector);
};
} else {
approx = [nSectors, alpha1, alpha2](ClipperCircle &circle,
ClipperLib::Path &sector) {
approximate(circle, nSectors, alpha1, alpha2, sector);
};
}
for (auto &sector : sectors) {
ClipperCircle circle(rScaled, originScaled);
approx(circle, sector);
approximate(circle, nSectors, sector);
rScaled += deltaRScaled;
}
// Clip sectors to polygonENU.
......@@ -502,6 +497,21 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
transectsENU.push_back(transect);
}
}
// Join sectors which where slit due to clipping.
static_assert(false, "continue here.");
for (std::size_t i = 0; i < transectsENU.size() - 1; ++i) {
const auto &t1 = transectsENU[i];
for (std::size_t j = i + 1; j < transectsENU.size(); ++j) {
const auto &t2 = transectsENU[j];
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CircularSurvey: concurrent update transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
<< " ms";
#endif
if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
......@@ -512,10 +522,10 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
}
// Route transects;
snake::Transects transectsRouted;
std::vector<snake::TransectInfo> transectsInfo;
snake::Route route;
std::string errorString;
bool success = snake::route(polygonENU, transectsENU, transectsRouted,
bool success = snake::route(polygonENU, transectsENU, transectsInfo,
route, errorString);
if (!success) {
#ifdef DEBUG_CIRCULAR_SURVEY
......@@ -525,6 +535,18 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
return PtrTransects();
}
// Remove return path.
const auto &info = transectsInfo.back();
const auto &lastTransect = transectsENU[info.index];
const auto &lastWaypoint =
info.reversed ? lastTransect.front() : lastTransect.back();
auto &wp = route.back();
while (wp != lastWaypoint) {
route.pop_back();
wp = route.back();
}
// Convert to geo coordinates.
QList<CoordInfo_t> transectList;
transectList.reserve(route.size());
for (const auto &vertex : route) {
......@@ -533,13 +555,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
CoordInfo_t coordinfo = {c, CoordTypeInterior};
transectList.append(coordinfo);
}
PtrTransects transects(new Transects());
transects->append(transectList);
PtrTransects pTransects(new Transects());
pTransects->append(transectList);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"concurrent update success.";
#endif
return transects;
return pTransects;
}
}); // QtConcurrent::run()
_watcher.setFuture(future);
......
src/Wima/Snake/SnakeThread.cc
View file @ 625b439c
......@@ -360,7 +360,7 @@ void SnakeThread::run() {
bool waypointsValid = true;
snake::Transects transects;
snake::Transects transectsRouted;
std::vector<snake::TransectInfo> transectsInfo;
snake::Route route;
bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
this->pImpl->input.routeParametersChanged ||
......@@ -385,8 +385,8 @@ void SnakeThread::run() {
waypointsValid = false;
} else if (transects.size() > 1) {
// Route transects.
success = snake::route(this->pImpl->jAreaENU, transects,
transectsRouted, route, errorString);
success = snake::route(this->pImpl->jAreaENU, transects, transectsInfo,
route, errorString);
if (!success) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
......@@ -481,7 +481,10 @@ void SnakeThread::run() {
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
const auto &info = transectsInfo.front();
const auto &firstTransect = transects[info.index];
const auto &firstWaypoint =
info.reversed ? firstTransect.front() : firstTransect.back();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
......@@ -497,7 +500,10 @@ void SnakeThread::run() {
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
const auto &info2 = transectsInfo.back();
const auto &lastTransect = transects[info2.index];
const auto &lastWaypoint =
info2.reversed ? lastTransect.front() : lastTransect.back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
......
src/Wima/Snake/route.cpp deleted 100644 → 0
View file @ 7271b853
bool flight_plan::route(const BoostPolygon &area,
const flight_plan::Transects &transects,
Transects &transectsRouted, flight_plan::Route &route,
string &errorString) {
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Create vertex list;
BoostLineString vertices;
size_t n0 = 0;
for (const auto &t : transects) {
n0 += std::min<std::size_t>(t.size(), 2);
}
vertices.reserve(n0);
struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
vertices.push_back(t.front());
transectInfoList.push_back(TransectInfo{i, true});
if (t.size() >= 2) {
vertices.push_back(t.back());
transectInfoList.push_back(TransectInfo{i, false});
}
}
for (long i = 0; i < long(area.outer().size()) - 1; ++i) {
vertices.push_back(area.outer()[i]);
}
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1);
// Offset joined area.
BoostPolygon areaOffset;
offsetPolygon(area, areaOffset, detail::offsetConstant);
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
<< endl;
#endif
// Create Routing Index Manager.
RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
dataModel.numVehicles, dataModel.depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&dataModel, &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 dataModel.distanceMatrix.get(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;
} else {
index += 1;
}
}
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
google::protobuf::Duration *tMax =
new google::protobuf::Duration(); // seconds
tMax->set_seconds(10);
searchParameters.set_allocated_time_limit(tMax);
// 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;
}
// 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<BoostPoint> &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 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx1];
if (info1.index == info2.index) { // same transect?
if (!info1.front) { // transect reversal needed?
BoostLineString reversedTransect;
const auto &t = transects[info1.index];
for (auto it = t.end() - 1; it >= t.begin(); --it) {
reversedTransect.push_back(*it);
}
transectsRouted.push_back(reversedTransect);
for (auto it = reversedTransect.begin();
it < reversedTransect.end() - 1; ++it) {
route.push_back(*it);
}
} else {
const auto &t = transects[info1.index];
for (auto it = t.begin(); it < t.end() - 1; ++it) {
route.push_back(*it);
}
transectsRouted.push_back(t);
}
} else {
std::vector<size_t> idxList;
shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
idx2Vertex(idxList, route);
}
}
return true;
}
src/Wima/Snake/snake.cpp
View file @ 625b439c
......@@ -781,7 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
}
bool route(const BoostPolygon &area, const Transects &transects,
Transects &transectsRouted, Route &route, string &errorString) {
std::vector<TransectInfo> &transectInfo, Route &route,
string &errorString) {
//=======================================
// Route Transects using Google or-tools.
//=======================================
......@@ -794,19 +795,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
}
vertices.reserve(n0);
struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {}
struct LocalInfo {
LocalInfo(size_t n, bool f) : index(n), front(f) {}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
std::vector<LocalInfo> localTransectInfo;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
vertices.push_back(t.front());
transectInfoList.push_back(TransectInfo{i, true});
localTransectInfo.push_back(LocalInfo{i, true});
if (t.size() >= 2) {
vertices.push_back(t.back());
transectInfoList.push_back(TransectInfo{i, false});
localTransectInfo.push_back(LocalInfo{i, false});
}
}
......@@ -929,16 +930,18 @@ bool route(const BoostPolygon &area, const Transects &transects,
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 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx1];
const auto &info1 = localTransectInfo[idx0];
const auto &info2 = localTransectInfo[idx1];
if (info1.index == info2.index) { // same transect?
if (!info1.front) { // transect reversal needed?
TransectInfo trInfo(info1.index,
!info1.front ? true : false /*transect reversed?*/);
transectInfo.push_back(trInfo);
if (!info1.front) { // transect reversal needed?
BoostLineString reversedTransect;
const auto &t = transects[info1.index];
for (auto it = t.end() - 1; it >= t.begin(); --it) {
reversedTransect.push_back(*it);
}
transectsRouted.push_back(reversedTransect);
for (auto it = reversedTransect.begin();
it < reversedTransect.end() - 1; ++it) {
route.push_back(*it);
......@@ -948,7 +951,6 @@ bool route(const BoostPolygon &area, const Transects &transects,
for (auto it = t.begin(); it < t.end() - 1; ++it) {
route.push_back(*it);
}
transectsRouted.push_back(t);
}
} else {
std::vector<size_t> idxList;
......
src/Wima/Snake/snake.h
View file @ 625b439c
......@@ -98,7 +98,9 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
origin.altitude(), earth);
double x = 0, y = 0, z = 0;
proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
auto alt = in.altitude();
alt = std::isnan(alt) ? 0 : alt;
proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
out.set<0>(x);
out.set<1>(y);
(void)z;
......@@ -156,6 +158,8 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
typedef bu::quantity<bu::si::plane_angle> Radian;
typedef bu::quantity<bu::degree::plane_angle> Degree;
bool joinedArea(const std::vector<BoostPolygon *> &areas, BoostPolygon &jArea);
bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
......@@ -174,12 +178,19 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const std::vector<BoostPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString);
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
bool route(const BoostPolygon &area, const Transects &transects,
Transects &transectsRouted, Route &route, string &errorString);
std::vector<TransectInfo> &transectInfo, Route &route,
string &errorString);
namespace detail {
const double offsetConstant =
1; // meter, polygon offset to compenstate for numerical inaccurracies.
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
......
src/Wima/Snake/snake_old.cpp deleted 100644 → 0
View file @ 7271b853
This diff is collapsed.
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