Skip to content

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

project compiles, not tested

parent cf53d945
Hide whitespace changes
Inline Side-by-side
Showing with 2521 additions and 2799 deletions
src/Snake/snake.cpp
View file @ c6e6a519
......@@ -2,13 +2,13 @@
#include "snake.h"
#include <mapbox/polylabel.hpp>
#include <mapbox/geometry.hpp>
#include <mapbox/polylabel.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000
......@@ -34,467 +34,434 @@ namespace snake {
// Geometry stuff.
//=========================================================================
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center)
{
using namespace mapbox;
if (polygon.outer().empty())
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) {
using namespace mapbox;
if (polygon.outer().empty())
return;
geometry::polygon<double> p;
geometry::linear_ring<double> lr1;
for (size_t i = 0; i < polygon.outer().size(); ++i) {
geometry::point<double> vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>());
geometry::polygon<double> p;
geometry::linear_ring<double> lr1;
for (size_t i = 0; i < polygon.outer().size(); ++i) {
geometry::point<double> vertex(polygon.outer()[i].get<0>(),
polygon.outer()[i].get<1>());
lr1.push_back(vertex);
}
p.push_back(lr1);
geometry::point<double> c = polylabel(p);
}
p.push_back(lr1);
geometry::point<double> c = polylabel(p);
center.set<0>(c.x);
center.set<1>(c.y);
center.set<0>(c.x);
center.set<1>(c.y);
}
void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox)
{
/*
Find the minimum-area bounding box of a set of 2D points
The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
The first and last points points must be the same, making a closed polygon.
This program finds the rotation angles of each edge of the convex polygon,
then tests the area of a bounding box aligned with the unique angles in
90 degrees of the 1st Quadrant.
Returns the
Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
Results verified using Matlab
Copyright (c) 2013, David Butterworth, University of Queensland
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Willow Garage, Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
if (polygon.outer().empty())
return;
BoostPolygon convex_hull;
bg::convex_hull(polygon, convex_hull);
//cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
//# Compute edges (x2-x1,y2-y1)
std::vector<BoostPoint> edges;
auto convex_hull_outer = convex_hull.outer();
for (long i=0; i < long(convex_hull_outer.size())-1; ++i) {
BoostPoint p1 = convex_hull_outer.at(i);
BoostPoint p2 = convex_hull_outer.at(i+1);
double edge_x = p2.get<0>() - p1.get<0>();
double edge_y = p2.get<1>() - p1.get<1>();
edges.push_back(BoostPoint{edge_x, edge_y});
}
// cout << "Edges: ";
// for (auto e : edges)
// cout << e.get<0>() << " " << e.get<1>() << ",";
// cout << endl;
// Calculate unique edge angles atan2(y/x)
double angle_scale = 1e3;
std::set<long> angles_long;
for (auto vertex : edges) {
double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
angle = angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
angles_long.insert(long(round(angle*angle_scale)));
}
std::vector<double> edge_angles;
for (auto a : angles_long)
edge_angles.push_back(double(a)/angle_scale);
// cout << "Unique angles: ";
// for (auto e : edge_angles)
// cout << e*180/M_PI << ",";
// cout << endl;
double min_area = std::numeric_limits<double>::infinity();
// Test each angle to find bounding box with smallest area
// print "Testing", len(edge_angles), "possible rotations for bounding box... \n"
for (double angle : edge_angles){
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
BoostPolygon hull_rotated;
bg::transform(convex_hull, hull_rotated, rotate);
//cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
bg::model::box<BoostPoint> box;
bg::envelope(hull_rotated, box);
// cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
//# print "Rotated hull points are \n", rot_points
BoostPoint min_corner = box.min_corner();
BoostPoint max_corner = box.max_corner();
double min_x = min_corner.get<0>();
double max_x = max_corner.get<0>();
double min_y = min_corner.get<1>();
double max_y = max_corner.get<1>();
// cout << "min_x: " << min_x << endl;
// cout << "max_x: " << max_x << endl;
// cout << "min_y: " << min_y << endl;
// cout << "max_y: " << max_y << endl;
// Calculate height/width/area of this bounding rectangle
double width = max_x - min_x;
double height = max_y - min_y;
double area = width * height;
// cout << "Width: " << width << endl;
// cout << "Height: " << height << endl;
// cout << "area: " << area << endl;
// cout << "angle: " << angle*180/M_PI << endl;
// Store the smallest rect found first (a simple convex hull might have 2 answers with same area)
if (area < min_area){
min_area = area;
minBBox.angle = angle;
minBBox.width = width;
minBBox.height = height;
minBBox.corners.clear();
minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
minBBox.corners.outer().push_back(BoostPoint{min_x, max_y});
minBBox.corners.outer().push_back(BoostPoint{max_x, max_y});
minBBox.corners.outer().push_back(BoostPoint{max_x, min_y});
minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
}
//cout << endl << endl;
}
// Transform corners of minimal bounding box.
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle*180/M_PI);
BoostPolygon rotated_polygon;
bg::transform(minBBox.corners, rotated_polygon, rotate);
minBBox.corners = rotated_polygon;
void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
/*
Find the minimum-area bounding box of a set of 2D points
The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
The first and last points points must be the same, making a closed polygon.
This program finds the rotation angles of each edge of the convex polygon,
then tests the area of a bounding box aligned with the unique angles in
90 degrees of the 1st Quadrant.
Returns the
Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
Results verified using Matlab
Copyright (c) 2013, David Butterworth, University of Queensland
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Willow Garage, Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
if (polygon.outer().empty())
return;
BoostPolygon convex_hull;
bg::convex_hull(polygon, convex_hull);
// cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
//# Compute edges (x2-x1,y2-y1)
std::vector<BoostPoint> edges;
auto convex_hull_outer = convex_hull.outer();
for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) {
BoostPoint p1 = convex_hull_outer.at(i);
BoostPoint p2 = convex_hull_outer.at(i + 1);
double edge_x = p2.get<0>() - p1.get<0>();
double edge_y = p2.get<1>() - p1.get<1>();
edges.push_back(BoostPoint{edge_x, edge_y});
}
// cout << "Edges: ";
// for (auto e : edges)
// cout << e.get<0>() << " " << e.get<1>() << ",";
// cout << endl;
// Calculate unique edge angles atan2(y/x)
double angle_scale = 1e3;
std::set<long> angles_long;
for (auto vertex : edges) {
double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
angle =
angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
angles_long.insert(long(round(angle * angle_scale)));
}
std::vector<double> edge_angles;
for (auto a : angles_long)
edge_angles.push_back(double(a) / angle_scale);
// cout << "Unique angles: ";
// for (auto e : edge_angles)
// cout << e*180/M_PI << ",";
// cout << endl;
double min_area = std::numeric_limits<double>::infinity();
// Test each angle to find bounding box with smallest area
// print "Testing", len(edge_angles), "possible rotations for bounding box...
// \n"
for (double angle : edge_angles) {
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle * 180 /
M_PI);
BoostPolygon hull_rotated;
bg::transform(convex_hull, hull_rotated, rotate);
// cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated)
// << endl;
bg::model::box<BoostPoint> box;
bg::envelope(hull_rotated, box);
// cout << "Bounding box: " <<
// bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
//# print "Rotated hull points are \n", rot_points
BoostPoint min_corner = box.min_corner();
BoostPoint max_corner = box.max_corner();
double min_x = min_corner.get<0>();
double max_x = max_corner.get<0>();
double min_y = min_corner.get<1>();
double max_y = max_corner.get<1>();
// cout << "min_x: " << min_x << endl;
// cout << "max_x: " << max_x << endl;
// cout << "min_y: " << min_y << endl;
// cout << "max_y: " << max_y << endl;
// Calculate height/width/area of this bounding rectangle
double width = max_x - min_x;
double height = max_y - min_y;
double area = width * height;
// cout << "Width: " << width << endl;
// cout << "Height: " << height << endl;
// cout << "area: " << area << endl;
// cout << "angle: " << angle*180/M_PI << endl;
// Store the smallest rect found first (a simple convex hull might have 2
// answers with same area)
if (area < min_area) {
min_area = area;
minBBox.angle = angle;
minBBox.width = width;
minBBox.height = height;
minBBox.corners.clear();
minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
minBBox.corners.outer().push_back(BoostPoint{min_x, max_y});
minBBox.corners.outer().push_back(BoostPoint{max_x, max_y});
minBBox.corners.outer().push_back(BoostPoint{max_x, min_y});
minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
}
// cout << endl << endl;
}
// Transform corners of minimal bounding box.
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle *
180 / M_PI);
BoostPolygon rotated_polygon;
bg::transform(minBBox.corners, rotated_polygon, rotate);
minBBox.corners = rotated_polygon;
}
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset)
{
bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
bg::strategy::buffer::join_miter join_strategy(3);
bg::strategy::buffer::end_flat end_strategy;
bg::strategy::buffer::point_square point_strategy;
bg::strategy::buffer::side_straight side_strategy;
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
double offset) {
bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
bg::strategy::buffer::join_miter join_strategy(3);
bg::strategy::buffer::end_flat end_strategy;
bg::strategy::buffer::point_square point_strategy;
bg::strategy::buffer::side_straight side_strategy;
bg::model::multi_polygon<BoostPolygon> result;
bg::model::multi_polygon<BoostPolygon> result;
bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
if (result.size() > 0)
polygonOffset = result[0];
bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy,
end_strategy, point_strategy);
if (result.size() > 0)
polygonOffset = result[0];
}
void graphFromPolygon(const BoostPolygon &polygon,
const BoostLineString &vertices, Matrix<double> &graph) {
size_t n = graph.getN();
void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix<double> &graph)
{
size_t n = graph.getN();
for (size_t i=0; i < n; ++i) {
BoostPoint v1 = vertices[i];
for (size_t j=i+1; j < n; ++j){
BoostPoint v2 = vertices[j];
BoostLineString path{v1, v2};
for (size_t i = 0; i < n; ++i) {
BoostPoint v1 = vertices[i];
for (size_t j = i + 1; j < n; ++j) {
BoostPoint v2 = vertices[j];
BoostLineString path{v1, v2};
double distance = 0;
if (!bg::within(path, polygon))
distance = std::numeric_limits<double>::infinity();
else
distance = bg::length(path);
graph.set(i, j, distance);
graph.set(j, i, distance);
}
}
double distance = 0;
if (!bg::within(path, polygon))
distance = std::numeric_limits<double>::infinity();
else
distance = bg::length(path);
graph.set(i, j, distance);
graph.set(j, i, distance);
}
}
}
bool dijkstraAlgorithm(const size_t numElements,
size_t startIndex,
size_t endIndex,
std::vector<size_t> &elementPath,
std::function<double (const size_t, const size_t)> distanceDij)
{
if ( startIndex >= numElements
|| endIndex >= numElements
|| endIndex == startIndex) {
return false;
}
// Node struct
// predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
// distance is the distance between the node and the start node
// node number is stored by the position in nodeList
struct Node{
int predecessorIndex = -1;
double distance = std::numeric_limits<double>::infinity();
};
// The list with all Nodes (elements)
std::vector<Node> nodeList(numElements);
// This list will be initalized with indices referring to the elements of nodeList.
// Elements will be successively remove during the execution of the Dijkstra Algorithm.
std::vector<size_t> workingSet(numElements);
//append elements to node list
for (size_t i = 0; i < numElements; ++i) workingSet[i] = i;
nodeList[startIndex].distance = 0;
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
while (workingSet.size() > 0) {
// serach Node with minimal distance
double minDist = std::numeric_limits<double>::infinity();
int minDistIndex_WS = -1; // WS = workinSet
for (size_t i = 0; i < workingSet.size(); ++i) {
const int nodeIndex = workingSet.at(i);
const double dist = nodeList.at(nodeIndex).distance;
if (dist < minDist) {
minDist = dist;
minDistIndex_WS = i;
}
}
if (minDistIndex_WS == -1)
return false;
size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
workingSet.erase(workingSet.begin()+minDistIndex_WS);
if (indexU_NL == endIndex) // shortest path found
break;
const double distanceU = nodeList.at(indexU_NL).distance;
//update distance
for (size_t i = 0; i < workingSet.size(); ++i) {
int indexV_NL = workingSet[i]; // NL = nodeList
Node* v = &nodeList[indexV_NL];
double dist = distanceDij(indexU_NL, indexV_NL);
// is ther an alternative path which is shorter?
double alternative = distanceU + dist;
if (alternative < v->distance) {
v->distance = alternative;
v->predecessorIndex = indexU_NL;
}
}
}
// end Djikstra Algorithm
// reverse assemble path
int e = endIndex;
while (1) {
if (e == -1) {
if (elementPath[0] == startIndex)// check if starting point was reached
break;
return false;
}
elementPath.insert(elementPath.begin(), e);
//Update Node
e = nodeList[e].predecessorIndex;
}
return true;
}
bool dijkstraAlgorithm(
const size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath,
std::function<double(const size_t, const size_t)> distanceDij) {
if (startIndex >= numElements || endIndex >= numElements ||
endIndex == startIndex) {
return false;
}
// Node struct
// predecessorIndex is the index of the predecessor node
// (nodeList[predecessorIndex]) distance is the distance between the node and
// the start node node number is stored by the position in nodeList
struct Node {
int predecessorIndex = -1;
double distance = std::numeric_limits<double>::infinity();
};
// The list with all Nodes (elements)
std::vector<Node> nodeList(numElements);
// This list will be initalized with indices referring to the elements of
// nodeList. Elements will be successively remove during the execution of the
// Dijkstra Algorithm.
std::vector<size_t> workingSet(numElements);
// append elements to node list
for (size_t i = 0; i < numElements; ++i)
workingSet[i] = i;
nodeList[startIndex].distance = 0;
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
while (workingSet.size() > 0) {
// serach Node with minimal distance
double minDist = std::numeric_limits<double>::infinity();
int minDistIndex_WS = -1; // WS = workinSet
for (size_t i = 0; i < workingSet.size(); ++i) {
const int nodeIndex = workingSet.at(i);
const double dist = nodeList.at(nodeIndex).distance;
if (dist < minDist) {
minDist = dist;
minDistIndex_WS = i;
}
}
if (minDistIndex_WS == -1)
return false;
size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
workingSet.erase(workingSet.begin() + minDistIndex_WS);
if (indexU_NL == endIndex) // shortest path found
break;
const double distanceU = nodeList.at(indexU_NL).distance;
// update distance
for (size_t i = 0; i < workingSet.size(); ++i) {
int indexV_NL = workingSet[i]; // NL = nodeList
Node *v = &nodeList[indexV_NL];
double dist = distanceDij(indexU_NL, indexV_NL);
// is ther an alternative path which is shorter?
double alternative = distanceU + dist;
if (alternative < v->distance) {
v->distance = alternative;
v->predecessorIndex = indexU_NL;
}
}
}
// end Djikstra Algorithm
// reverse assemble path
int e = endIndex;
while (1) {
if (e == -1) {
if (elementPath[0] == startIndex) // check if starting point was reached
break;
return false;
}
elementPath.insert(elementPath.begin(), e);
void toDistanceMatrix(Matrix<double> &graph)
{
size_t n = graph.getN();
auto distance = [graph](size_t i, size_t j){
return graph.get(i,j);
};
std::vector<size_t> path;
for (size_t i=0; i < n; ++i) {
for (size_t j=i+1; j < n; ++j){
double d = graph.get(i,j);
if (!std::isinf(d))
continue;
path.clear();
bool ret = dijkstraAlgorithm(n, i, j, path, distance);
assert(ret);
(void)ret;
// cout << "(" << i << "," << j << ") d: " << d << endl;
// cout << "Path size: " << path.size() << endl;
// for (auto idx : path)
// cout << idx << " ";
// cout << endl;
d = 0;
for (long k=0; k < long(path.size())-1; ++k) {
size_t idx0 = path[k];
size_t idx1 = path[k+1];
double d0 = graph.get(idx0, idx1);
assert(std::isinf(d0) == false);
d += d0;
}
graph.set(i, j, d);
graph.set(j, i, d);
}
}
// Update Node
e = nodeList[e].predecessorIndex;
}
return true;
}
void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_t endIndex, std::vector<size_t> &pathIdx)
{
if (!std::isinf(graph.get(startIndex, endIndex))){
pathIdx.push_back(startIndex);
pathIdx.push_back(endIndex);
} else {
auto distance = [graph](size_t i, size_t j){
return graph.get(i, j);
};
bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, distance);
assert(ret);
(void)ret;
}
//=========================================================================
// Scenario calculation.
//=========================================================================
}Scenario::Scenario() :
_tileWidth(5*bu::si::meter)
, _tileHeight(5*bu::si::meter)
, _minTileArea(0*bu::si::meter*bu::si::meter)
, _needsUpdate(true)
{
void toDistanceMatrix(Matrix<double> &graph) {
size_t n = graph.getN();
auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); };
std::vector<size_t> path;
for (size_t i = 0; i < n; ++i) {
for (size_t j = i + 1; j < n; ++j) {
double d = graph.get(i, j);
if (!std::isinf(d))
continue;
path.clear();
bool ret = dijkstraAlgorithm(n, i, j, path, distance);
assert(ret);
(void)ret;
// cout << "(" << i << "," << j << ") d: " << d << endl;
// cout << "Path size: " << path.size() << endl;
// for (auto idx : path)
// cout << idx << " ";
// cout << endl;
d = 0;
for (long k = 0; k < long(path.size()) - 1; ++k) {
size_t idx0 = path[k];
size_t idx1 = path[k + 1];
double d0 = graph.get(idx0, idx1);
assert(std::isinf(d0) == false);
d += d0;
}
graph.set(i, j, d);
graph.set(j, i, d);
}
}
}
void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
size_t endIndex, std::vector<size_t> &pathIdx) {
if (!std::isinf(graph.get(startIndex, endIndex))) {
pathIdx.push_back(startIndex);
pathIdx.push_back(endIndex);
} else {
auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); };
bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx,
distance);
assert(ret);
(void)ret;
}
//=========================================================================
// Scenario calculation.
//=========================================================================
}
Scenario::Scenario()
: _tileWidth(5 * bu::si::meter), _tileHeight(5 * bu::si::meter),
_minTileArea(0 * bu::si::meter * bu::si::meter), _needsUpdate(true) {}
void Scenario::setMeasurementArea(const BoostPolygon &area)
{
_needsUpdate = true;
_mArea = area;
void Scenario::setMeasurementArea(const BoostPolygon &area) {
_needsUpdate = true;
_mArea = area;
}
void Scenario::setServiceArea(const BoostPolygon &area)
{
_needsUpdate = true;
_sArea = area;
void Scenario::setServiceArea(const BoostPolygon &area) {
_needsUpdate = true;
_sArea = area;
}
void Scenario::setCorridor(const BoostPolygon &area)
{
_needsUpdate = true;
_corridor = area;
void Scenario::setCorridor(const BoostPolygon &area) {
_needsUpdate = true;
_corridor = area;
}
BoostPolygon &Scenario::measurementArea() {
_needsUpdate = true;
return _mArea;
_needsUpdate = true;
return _mArea;
}
BoostPolygon &Scenario::serviceArea() {
_needsUpdate = true;
return _sArea;
_needsUpdate = true;
return _sArea;
}
BoostPolygon &Scenario::corridor() {
_needsUpdate = true;
return _corridor;
_needsUpdate = true;
return _corridor;
}
const BoundingBox &Scenario::mAreaBoundingBox() const
{
return _mAreaBoundingBox;
const BoundingBox &Scenario::mAreaBoundingBox() const {
return _mAreaBoundingBox;
}
const BoostPolygon &Scenario::measurementArea() const
{
return _mArea;
}
const BoostPolygon &Scenario::measurementArea() const { return _mArea; }
const BoostPolygon &Scenario::serviceArea() const
{
return _sArea;
}
const BoostPolygon &Scenario::serviceArea() const { return _sArea; }
const BoostPolygon &Scenario::corridor() const
{
return _corridor;
}
const BoostPolygon &Scenario::corridor() const { return _corridor; }
const BoostPolygon &Scenario::joinedArea() const {
return _jArea;
}
const BoostPolygon &Scenario::joinedArea() const { return _jArea; }
const vector<BoostPolygon> &Scenario::tiles() const{
return _tiles;
}
const vector<BoostPolygon> &Scenario::tiles() const { return _tiles; }
const BoostLineString &Scenario::tileCenterPoints() const{
return _tileCenterPoints;
const BoostLineString &Scenario::tileCenterPoints() const {
return _tileCenterPoints;
}
const BoundingBox &Scenario::measurementAreaBBox() const{
return _mAreaBoundingBox;
const BoundingBox &Scenario::measurementAreaBBox() const {
return _mAreaBoundingBox;
}
const BoostPoint &Scenario::homePositon() const{
return _homePosition;
}
const BoostPoint &Scenario::homePositon() const { return _homePosition; }
bool Scenario::update()
{
if ( !_needsUpdate )
return true;
if (!_calculateBoundingBox())
return false;
if (!_calculateTiles())
return false;
if (!_calculateJoinedArea())
return false;
_needsUpdate = false;
bool Scenario::update() {
if (!_needsUpdate)
return true;
if (!_calculateBoundingBox())
return false;
if (!_calculateTiles())
return false;
if (!_calculateJoinedArea())
return false;
_needsUpdate = false;
return true;
}
bool Scenario::_calculateBoundingBox()
{
minimalBoundingBox(_mArea, _mAreaBoundingBox);
return true;
bool Scenario::_calculateBoundingBox() {
minimalBoundingBox(_mArea, _mAreaBoundingBox);
return true;
}
/**
* Devides the (measurement area) bounding box into tiles and clips it to the measurement area.
* Devides the (measurement area) bounding box into tiles and clips it to the
* measurement area.
*
* Devides the (measurement area) bounding box into tiles of width \p tileWidth and height \p tileHeight.
* Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
* The function assumes that \a _mArea and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
* Scenario::_calculateBoundingBox().
* Devides the (measurement area) bounding box into tiles of width \p tileWidth
* and height \p tileHeight. Clips the resulting tiles to the measurement area.
* Tiles are rejected, if their area is smaller than \p minTileArea. The
* function assumes that \a _mArea and \a _mAreaBoundingBox have correct values.
* \see \ref Scenario::_areas2enu() and \ref Scenario::_calculateBoundingBox().
*
* @param tileWidth The width (>0) of a tile.
* @param tileHeight The heigth (>0) of a tile.
......@@ -502,585 +469,585 @@ bool Scenario::_calculateBoundingBox()
*
* @return Returns true if successful.
*/
bool Scenario::_calculateTiles()
{
_tiles.clear();
_tileCenterPoints.clear();
if (_tileWidth <= 0*bu::si::meter
|| _tileHeight <= 0*bu::si::meter
|| _minTileArea <= 0*bu::si::meter*bu::si::meter
)
{
errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
return false;
}
double bboxWidth = _mAreaBoundingBox.width;
double bboxHeight = _mAreaBoundingBox.height;
BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];
//cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(), -origin.get<1>());
BoostPolygon translated_polygon;
BoostPolygon rotated_polygon;
boost::geometry::transform(_mArea, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
//cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth/_tileWidth.value());
size_t jMax = ceil(bboxHeight/_tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width or Tile height to large. "
<< "tile width = " << _tileWidth << ", "
<< "tile height = " << _tileHeight << "." << std::endl;
errorString = ss.str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(), origin.get<1>());
for (size_t i = 0; i < iMax; ++i){
double x_min = _tileWidth.value()*i;
double x_max = x_min + _tileWidth.value();
for (size_t j = 0; j < jMax; ++j){
double y_min = _tileHeight.value()*j;
double y_max = y_min + _tileHeight.value();
BoostPolygon tile_unclipped;
tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
std::deque<BoostPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles))
continue;
for (BoostPolygon t : boost_tiles)
{
if (bg::area(t) > _minTileArea.value()){
// Transform boost_tile to world coordinate system.
BoostPolygon rotated_tile;
BoostPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile, translate_back);
// Store tile and calculate center point.
_tiles.push_back(translated_tile);
BoostPoint tile_center;
polygonCenter(translated_tile, tile_center);
_tileCenterPoints.push_back(tile_center);
}
}
bool Scenario::_calculateTiles() {
_tiles.clear();
_tileCenterPoints.clear();
if (_tileWidth <= 0 * bu::si::meter || _tileHeight <= 0 * bu::si::meter ||
_minTileArea <= 0 * bu::si::meter * bu::si::meter) {
errorString =
"Parameters tileWidth, tileHeight, minTileArea must be positive.";
return false;
}
double bboxWidth = _mAreaBoundingBox.width;
double bboxHeight = _mAreaBoundingBox.height;
BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];
// cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
_mAreaBoundingBox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
-origin.get<1>());
BoostPolygon translated_polygon;
BoostPolygon rotated_polygon;
boost::geometry::transform(_mArea, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth / _tileWidth.value());
size_t jMax = ceil(bboxHeight / _tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width or Tile height to large. "
<< "tile width = " << _tileWidth << ", "
<< "tile height = " << _tileHeight << "." << std::endl;
errorString = ss.str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
-_mAreaBoundingBox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
origin.get<1>());
for (size_t i = 0; i < iMax; ++i) {
double x_min = _tileWidth.value() * i;
double x_max = x_min + _tileWidth.value();
for (size_t j = 0; j < jMax; ++j) {
double y_min = _tileHeight.value() * j;
double y_max = y_min + _tileHeight.value();
BoostPolygon tile_unclipped;
tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
std::deque<BoostPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
boost_tiles))
continue;
for (BoostPolygon t : boost_tiles) {
if (bg::area(t) > _minTileArea.value()) {
// Transform boost_tile to world coordinate system.
BoostPolygon rotated_tile;
BoostPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile,
translate_back);
// Store tile and calculate center point.
_tiles.push_back(translated_tile);
BoostPoint tile_center;
polygonCenter(translated_tile, tile_center);
_tileCenterPoints.push_back(tile_center);
}
}
}
}
if (_tiles.size() < 1){
errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
return false;
}
if (_tiles.size() < 1) {
errorString =
"No tiles calculated. Is the minTileArea parameter large enough?";
return false;
}
return true;
return true;
}
bool Scenario::_calculateJoinedArea()
{
_jArea.clear();
// Measurement area and service area overlapping?
bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false;
bool corridorValid = _corridor.outer().size() > 0 ? true : false;
// Check if corridor is connecting measurement area and service area.
bool corridor_is_connection = false;
if (corridorValid) {
// Corridor overlaping with measurement area?
if ( bg::intersects(_corridor, _mArea) ) {
// Corridor overlaping with service area?
if ( bg::intersects(_corridor, _sArea) ) {
corridor_is_connection = true;
}
}
bool Scenario::_calculateJoinedArea() {
_jArea.clear();
// Measurement area and service area overlapping?
bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false;
bool corridorValid = _corridor.outer().size() > 0 ? true : false;
// Check if corridor is connecting measurement area and service area.
bool corridor_is_connection = false;
if (corridorValid) {
// Corridor overlaping with measurement area?
if (bg::intersects(_corridor, _mArea)) {
// Corridor overlaping with service area?
if (bg::intersects(_corridor, _sArea)) {
corridor_is_connection = true;
}
}
// Are areas joinable?
std::deque<BoostPolygon> sol;
BoostPolygon partialArea = _mArea;
if (overlapingSerMeas){
if(corridor_is_connection){
bg::union_(partialArea, _corridor, sol);
}
} else if (corridor_is_connection){
bg::union_(partialArea, _corridor, sol);
} else {
errorString = "Areas are not overlapping";
return false;
}
if (sol.size() > 0) {
partialArea = sol[0];
sol.clear();
}
// Are areas joinable?
std::deque<BoostPolygon> sol;
BoostPolygon partialArea = _mArea;
if (overlapingSerMeas) {
if (corridor_is_connection) {
bg::union_(partialArea, _corridor, sol);
}
// Join areas.
bg::union_(partialArea, _sArea, sol);
if (sol.size() > 0) {
_jArea = sol[0];
} else {
return false;
}
return true;
} else if (corridor_is_connection) {
bg::union_(partialArea, _corridor, sol);
} else {
errorString = "Areas are not overlapping";
return false;
}
if (sol.size() > 0) {
partialArea = sol[0];
sol.clear();
}
// Join areas.
bg::union_(partialArea, _sArea, sol);
if (sol.size() > 0) {
_jArea = sol[0];
} else {
return false;
}
return true;
}
Area Scenario::minTileArea() const
{
return _minTileArea;
}
Area Scenario::minTileArea() const { return _minTileArea; }
void Scenario::setMinTileArea(Area minTileArea)
{
if ( minTileArea >= 0*bu::si::meter*bu::si::meter){
_needsUpdate = true;
_minTileArea = minTileArea;
}
void Scenario::setMinTileArea(Area minTileArea) {
if (minTileArea >= 0 * bu::si::meter * bu::si::meter) {
_needsUpdate = true;
_minTileArea = minTileArea;
}
}
Length Scenario::tileHeight() const
{
return _tileHeight;
}
Length Scenario::tileHeight() const { return _tileHeight; }
void Scenario::setTileHeight(Length tileHeight)
{
if ( tileHeight > 0*bu::si::meter) {
_needsUpdate = true;
_tileHeight = tileHeight;
}
void Scenario::setTileHeight(Length tileHeight) {
if (tileHeight > 0 * bu::si::meter) {
_needsUpdate = true;
_tileHeight = tileHeight;
}
}
Length Scenario::tileWidth() const
{
return _tileWidth;
}
Length Scenario::tileWidth() const { return _tileWidth; }
void Scenario::setTileWidth(Length tileWidth)
{
if ( tileWidth > 0*bu::si::meter ){
_needsUpdate = true;
_tileWidth = tileWidth;
}
void Scenario::setTileWidth(Length tileWidth) {
if (tileWidth > 0 * bu::si::meter) {
_needsUpdate = true;
_tileWidth = tileWidth;
}
}
//=========================================================================
// Tile calculation.
//=========================================================================
bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
{
if (areas.size() < 1)
return false;
std::deque<std::size_t> idxList;
for(size_t i = 1; i < areas.size(); ++i)
idxList.push_back(i);
joinedArea = areas[0];
std::deque<BoostPolygon> sol;
while (idxList.size() > 0){
bool success = false;
for (auto it = idxList.begin(); it != idxList.end(); ++it){
bg::union_(joinedArea, areas[*it], sol);
if (sol.size() > 0) {
joinedArea = sol[0];
sol.clear();
idxList.erase(it);
success = true;
break;
}
}
if ( !success )
return false;
bool joinAreas(const std::vector<BoostPolygon> &areas,
BoostPolygon &joinedArea) {
if (areas.size() < 1)
return false;
std::deque<std::size_t> idxList;
for (size_t i = 1; i < areas.size(); ++i)
idxList.push_back(i);
joinedArea = areas[0];
std::deque<BoostPolygon> sol;
while (idxList.size() > 0) {
bool success = false;
for (auto it = idxList.begin(); it != idxList.end(); ++it) {
bg::union_(joinedArea, areas[*it], sol);
if (sol.size() > 0) {
joinedArea = sol[0];
sol.clear();
idxList.erase(it);
success = true;
break;
}
}
if (!success)
return false;
}
return true;
return true;
}
BoundingBox::BoundingBox() :
width(0)
, height(0)
, angle(0)
{
BoundingBox::BoundingBox() : width(0), height(0), angle(0) {}
void BoundingBox::clear() {
width = 0;
height = 0;
angle = 0;
corners.clear();
}
void BoundingBox::clear()
{
width = 0;
height = 0;
angle = 0;
corners.clear();
}
bool flight_plan::transectsFromScenario(Length distance,
Length minLength,
Angle angle,
const Scenario &scenario,
bool flight_plan::transectsFromScenario(Length distance, Length minLength,
Angle angle, const Scenario &scenario,
const Progress &p,
flight_plan::Transects &t,
string &errorString)
{
// Rotate measurement area by angle and calculate bounding box.
auto &mArea = scenario.measurementArea();
BoostPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-angle.value()*180/M_PI);
bg::transform(mArea, mAreaRotated, rotate);
BoostBox box;
boost::geometry::envelope(mAreaRotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = int(ceil((y1 - y0)/distance.value())); // number of transects
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(angle.value()*180/M_PI);
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i=0; i < num_t; ++i) {
// calculate transect
BoostPoint v1{x0, y0 + i*distance.value()};
BoostPoint v2{x1, y0 + i*distance.value()};
BoostLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
BoostLineString temp_transect;
bg::transform(transect, temp_transect, rotate_back);
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
if (transectsClipper.size() == 0){
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = " << distance << std::endl;
errorString = ss.str();
return false;
string &errorString) {
// Rotate measurement area by angle and calculate bounding box.
auto &mArea = scenario.measurementArea();
BoostPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-angle.value() *
180 / M_PI);
bg::transform(mArea, mAreaRotated, rotate);
BoostBox box;
boost::geometry::envelope(mAreaRotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
angle.value() * 180 / M_PI);
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
BoostPoint v1{x0, y0 + i * distance.value()};
BoostPoint v2{x1, y0 + i * distance.value()};
BoostLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
BoostLineString temp_transect;
bg::transform(transect, temp_transect, rotate_back);
ClipperLib::IntPoint c1{
static_cast<ClipperLib::cInt>(transect[0].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[0].get<1>() * CLIPPER_SCALE)};
ClipperLib::IntPoint c2{
static_cast<ClipperLib::cInt>(transect[1].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[1].get<1>() * CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
if (transectsClipper.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = " << distance
<< std::endl;
errorString = ss.str();
return false;
}
// Convert measurement area to clipper path.
ClipperLib::Path mAreaClipper;
for (auto vertex : mArea.outer()) {
mAreaClipper.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
auto &transects = clippedTransecs;
bool ignoreProgress = p.size() != scenario.tiles().size();
ClipperLib::PolyTree clippedTransecs2;
if (!ignoreProgress) {
// Calculate processed tiles (_progress[i] == 100) and subtract them from
// measurement area.
size_t numTiles = p.size();
vector<BoostPolygon> processedTiles;
const auto &tiles = scenario.tiles();
for (size_t i = 0; i < numTiles; ++i) {
if (p[i] == 100) {
processedTiles.push_back(tiles[i]);
}
}
// Convert measurement area to clipper path.
ClipperLib::Path mAreaClipper;
for ( auto vertex : mArea.outer() ){
mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
auto &transects = clippedTransecs;
bool ignoreProgress = p.size() != scenario.tiles().size();
ClipperLib::PolyTree clippedTransecs2;
if ( !ignoreProgress ) {
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t numTiles = p.size();
vector<BoostPolygon> processedTiles;
const auto &tiles = scenario.tiles();
for (size_t i=0; i<numTiles; ++i) {
if (p[i] == 100){
processedTiles.push_back(tiles[i]);
}
}
if (processedTiles.size() != numTiles){
vector<ClipperLib::Path> processedTilesClipper;
for (auto t : processedTiles){
ClipperLib::Path path;
for (auto vertex : t.outer()){
path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
}
processedTilesClipper.push_back(path);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (auto &child : clippedTransecs.Childs)
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
clipper.Execute(ClipperLib::ctDifference, clippedTransecs2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
transects = clippedTransecs2;
if (processedTiles.size() != numTiles) {
vector<ClipperLib::Path> processedTilesClipper;
for (auto t : processedTiles) {
ClipperLib::Path path;
for (auto vertex : t.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
processedTilesClipper.push_back(path);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (auto &child : clippedTransecs.Childs)
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
clipper.Execute(ClipperLib::ctDifference, clippedTransecs2,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
transects = clippedTransecs2;
}
// Extract transects from PolyTree and convert them to BoostLineString
for (auto &child : transects.Childs){
auto &clipperTransect = child->Contour;
BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};
BoostLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value())
t.push_back(transect);
}
if (t.size() == 0){
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = " << minLength << std::endl;
errorString = ss.str();
return false;
}
return true;
}
// Extract transects from PolyTree and convert them to BoostLineString
for (auto &child : transects.Childs) {
auto &clipperTransect = child->Contour;
BoostPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
BoostPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
BoostLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value())
t.push_back(transect);
}
if (t.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = " << minLength
<< std::endl;
errorString = ss.str();
return false;
}
return true;
}
struct RoutingDataModel{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
struct RoutingDataModel {
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
void generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
RoutingDataModel &dataModel,
Matrix<double> &graph){
const BoostPolygon &polygonOffset, size_t n0,
RoutingDataModel &dataModel, Matrix<double> &graph) {
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
auto start = std::chrono::high_resolution_clock::now();
#endif
graphFromPolygon(polygonOffset, vertices, graph);
graphFromPolygon(polygonOffset, vertices, graph);
#ifdef 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;
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(graph);
Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
start = std::chrono::high_resolution_clock::now();
#endif
toDistanceMatrix(distanceMatrix);
toDistanceMatrix(distanceMatrix);
#ifdef 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;
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
<< endl;
#endif
dataModel.distanceMatrix.setDimension(n0, n0);
for (size_t i=0; i<n0; ++i){
dataModel.distanceMatrix.set(i, i, 0);
for (size_t j=i+1; j<n0; ++j){
dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
}
dataModel.distanceMatrix.setDimension(n0, n0);
for (size_t i = 0; i < n0; ++i) {
dataModel.distanceMatrix.set(i, i, 0);
for (size_t j = i + 1; j < n0; ++j) {
dataModel.distanceMatrix.set(
i, j, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE));
dataModel.distanceMatrix.set(
j, i, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE));
}
dataModel.numVehicles = 1;
dataModel.depot = 0;
}
dataModel.numVehicles = 1;
dataModel.depot = 0;
}
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){
if (t.size() >= 2){
n0 += 2;
} else {
n0 += 1;
}
}
vertices.reserve(n0);
struct TransectInfo{
TransectInfo(size_t n, bool f)
: index(n)
, front(f)
{}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
size_t idx = 0;
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]);
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) {
if (t.size() >= 2) {
n0 += 2;
} else {
n0 += 1;
}
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
vertices.reserve(n0);
struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
size_t idx = 0;
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});
}
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);
}
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 SHOW_TIME
start = std::chrono::high_resolution_clock::now();
start = std::chrono::high_resolution_clock::now();
#endif
generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
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).
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i=0; i<transects.size(); ++i){
const auto& t = transects[i];
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);
vector<IntVar*> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
solver->AddConstraint(c);
index += 2;
} else {
index += 1;
}
// 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).
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
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);
vector<IntVar *> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
solver->AddConstraint(c);
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.
}
// 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 SHOW_TIME
start = std::chrono::high_resolution_clock::now();
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment* solution = routing.SolveWithParameters(searchParameters);
const Assignment *solution = routing.SolveWithParameters(searchParameters);
#ifdef 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;
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;
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());
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){
// 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);
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);
}
}
}
} // namespace snake
bool boost::geometry::model::operator==(snake::BoostPoint p1,
snake::BoostPoint p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::BoostPoint p1,
snake::BoostPoint p2) {
return !(p1 == p2);
}
src/Snake/snake.h
View file @ c6e6a519
#pragma once
#include <vector>
#include <string>
#include <array>
#include <memory>
#include <string>
#include <vector>
#include <boost/geometry.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include "snake_typedefs.h"
namespace bg = boost::geometry;
namespace bu = boost::units;
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
......@@ -13,256 +21,239 @@
using namespace std;
namespace snake {
//=========================================================================
// Geometry stuff.
//=========================================================================
template<class T>
class Matrix{
typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
// typedef std::vector<BoostPoint> BoostPointList;
typedef bg::model::polygon<BoostPoint> BoostPolygon;
typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef std::vector<std::vector<int64_t>> Int64Matrix;
typedef bg::model::box<BoostPoint> BoostBox;
template <class T> class Matrix {
public:
Matrix() :
_elements(0),
_m(0),
_n(0),
_isInitialized(false)
{
}
Matrix(size_t m, size_t n) : Matrix(m, n, T{0}){}
Matrix(size_t m, size_t n, T value) :
_elements(m*n),
_m(m),
_n(n),
_isInitialized(true)
{
assert((m > 0) || (n > 0));
_matrix.resize(_elements, value);
}
double get(size_t i, size_t j) const
{
assert(_isInitialized);
assert(i < _m);
assert(j < _n);
return _matrix[i*_m+j];
}
size_t getM() const { return _m;}
size_t getN() const { return _n;}
void set(size_t i, size_t j, const T &value)
{
assert(_isInitialized);
assert(i < _m);
assert(j < _n);
_matrix[i*_m+j] = value;
}
void setDimension(size_t m, size_t n, const T &value)
{
assert((m > 0) || (n > 0));
assert(!_isInitialized);
_m = m;
_n = n;
_elements = n*m;
_matrix.resize(_elements, value);
_isInitialized = true;
}
void setDimension(size_t m, size_t n)
{
setDimension(m, n, T{0});
}
Matrix() : _elements(0), _m(0), _n(0), _isInitialized(false) {}
Matrix(size_t m, size_t n) : Matrix(m, n, T{0}) {}
Matrix(size_t m, size_t n, T value)
: _elements(m * n), _m(m), _n(n), _isInitialized(true) {
assert((m > 0) || (n > 0));
_matrix.resize(_elements, value);
}
double get(size_t i, size_t j) const {
assert(_isInitialized);
assert(i < _m);
assert(j < _n);
return _matrix[i * _m + j];
}
size_t getM() const { return _m; }
size_t getN() const { return _n; }
void set(size_t i, size_t j, const T &value) {
assert(_isInitialized);
assert(i < _m);
assert(j < _n);
_matrix[i * _m + j] = value;
}
void setDimension(size_t m, size_t n, const T &value) {
assert((m > 0) || (n > 0));
assert(!_isInitialized);
_m = m;
_n = n;
_elements = n * m;
_matrix.resize(_elements, value);
_isInitialized = true;
}
void setDimension(size_t m, size_t n) { setDimension(m, n, T{0}); }
private:
size_t _elements;
size_t _m;
size_t _n;
bool _isInitialized;
std::vector<T> _matrix;
size_t _elements;
size_t _m;
size_t _n;
bool _isInitialized;
std::vector<T> _matrix;
};
struct BoundingBox{
BoundingBox();
struct BoundingBox {
BoundingBox();
void clear();
void clear();
double width;
double height;
double angle;
BoostPolygon corners;
double width;
double height;
double angle;
BoostPolygon corners;
};
template<class GeoPoint>
void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out)
{
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(),
origin.longitude(),
origin.altitude(),
earth);
double x, y, z;
proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
out.set<0>(x);
out.set<0>(y);
(void)z;
template <class GeoPoint>
void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double x, y, z;
proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
out.set<0>(x);
out.set<0>(y);
(void)z;
}
template<class GeoPoint>
void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out)
{
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(),
origin.longitude(),
origin.altitude(),
earth);
double lat, lon, alt;
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
out.setLatitude(lat);
out.setLongitude(lon);
out.setAltitude(alt);
template <class GeoPoint>
void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double lat, lon, alt;
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
out.setLatitude(lat);
out.setLongitude(lon);
out.setAltitude(alt);
}
void polygonCenter(const BoostPolygon &polygon,
BoostPoint &center);
void minimalBoundingBox(const BoostPolygon &polygon,
BoundingBox &minBBox);
void offsetPolygon(const BoostPolygon &polygon,
BoostPolygon &polygonOffset,
double offset);
void graphFromPolygon(const BoostPolygon &polygon,
const BoostLineString &vertices,
Matrix<double> &graph);
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox);
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
double offset);
void graphFromPolygon(const BoostPolygon &polygon,
const BoostLineString &vertices, Matrix<double> &graph);
void toDistanceMatrix(Matrix<double> &graph);
bool dijkstraAlgorithm(const size_t numElements,
size_t startIndex,
size_t endIndex,
std::vector<size_t> &elementPath,
std::function<double (const size_t, const size_t)> distanceDij);
void shortestPathFromGraph(const Matrix<double> &graph,
size_t startIndex,
size_t endIndex,
std::vector<size_t> &pathIdx);
bool dijkstraAlgorithm(
const size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath,
std::function<double(const size_t, const size_t)> distanceDij);
void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
size_t endIndex, std::vector<size_t> &pathIdx);
//=========================================================================
// Scenario.
//=========================================================================
class Scenario{
public:
Scenario();
void setMeasurementArea (const BoostPolygon &area);
void setServiceArea (const BoostPolygon &area);
void setCorridor (const BoostPolygon &area);
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
const BoundingBox &mAreaBoundingBox() const;
class Scenario {
public:
Scenario();
void setMeasurementArea(const BoostPolygon &area);
void setServiceArea(const BoostPolygon &area);
void setCorridor(const BoostPolygon &area);
const BoostPolygon &measurementArea() const;
const BoostPolygon &serviceArea()const;
const BoostPolygon &corridor()const;
const BoundingBox &mAreaBoundingBox() const;
BoostPolygon &measurementArea();
BoostPolygon &serviceArea();
BoostPolygon &corridor();
const BoostPolygon &measurementArea() const;
const BoostPolygon &serviceArea() const;
const BoostPolygon &corridor() const;
Length tileWidth() const;
void setTileWidth(Length tileWidth);
BoostPolygon &measurementArea();
BoostPolygon &serviceArea();
BoostPolygon &corridor();
Length tileHeight() const;
void setTileHeight(Length tileHeight);
Length tileWidth() const;
void setTileWidth(Length tileWidth);
Area minTileArea() const;
void setMinTileArea(Area minTileArea);
Length tileHeight() const;
void setTileHeight(Length tileHeight);
const BoostPolygon &joinedArea() const;
const vector<BoostPolygon> &tiles() const;
const BoostLineString &tileCenterPoints() const;
const BoundingBox &measurementAreaBBox() const;
const BoostPoint &homePositon() const;
Area minTileArea() const;
void setMinTileArea(Area minTileArea);
bool update();
const BoostPolygon &joinedArea() const;
const vector<BoostPolygon> &tiles() const;
const BoostLineString &tileCenterPoints() const;
const BoundingBox &measurementAreaBBox() const;
const BoostPoint &homePositon() const;
string errorString;
bool update();
string errorString;
private:
bool _calculateBoundingBox();
bool _calculateTiles();
bool _calculateJoinedArea();
bool _calculateBoundingBox();
bool _calculateTiles();
bool _calculateJoinedArea();
Length _tileWidth;
Length _tileHeight;
Area _minTileArea;
Length _tileWidth;
Length _tileHeight;
Area _minTileArea;
mutable bool _needsUpdate;
mutable bool _needsUpdate;
BoostPolygon _mArea;
BoostPolygon _sArea;
BoostPolygon _corridor;
BoostPolygon _jArea;
BoostPolygon _mArea;
BoostPolygon _sArea;
BoostPolygon _corridor;
BoostPolygon _jArea;
BoundingBox _mAreaBoundingBox;
vector<BoostPolygon> _tiles;
BoostLineString _tileCenterPoints;
BoostPoint _homePosition;
BoundingBox _mAreaBoundingBox;
vector<BoostPolygon> _tiles;
BoostLineString _tileCenterPoints;
BoostPoint _homePosition;
};
template<class GeoPoint, template <class, class...> class Container>
void areaToEnu( const GeoPoint &origin,
const Container<GeoPoint> &in,
BoostPolygon &out )
{
for(auto vertex : in) {
BoostPoint p;
toENU(origin, vertex, p);
out.outer().push_back(p);
}
bg::correct(out);
template <class GeoPoint, template <class, class...> class Container>
void areaToEnu(const GeoPoint &origin, const Container<GeoPoint> &in,
BoostPolygon &out) {
for (auto vertex : in) {
BoostPoint p;
toENU(origin, vertex, p);
out.outer().push_back(p);
}
bg::correct(out);
}
template<class GeoPoint, template <class, class...> class Container>
void areaFromEnu( const GeoPoint &origin,
BoostPolygon &in,
const Container<GeoPoint> &out )
{
for(auto vertex : in.outer()) {
GeoPoint p;
fromENU(origin, vertex, p);
out.push_back(p);
}
template <class GeoPoint, template <class, class...> class Container>
void areaFromEnu(const GeoPoint &origin, BoostPolygon &in,
const Container<GeoPoint> &out) {
for (auto vertex : in.outer()) {
GeoPoint p;
fromENU(origin, vertex, p);
out.push_back(p);
}
}
bool joinAreas(const std::vector<BoostPolygon> &areas,
BoostPolygon &joinedArea);
bool joinAreas(const std::vector<BoostPolygon> &areas,
BoostPolygon &joinedArea);
//========================================================================================
// flight_plan
//========================================================================================
namespace flight_plan{
using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = vector<BoostPoint>;
bool transectsFromScenario(Length distance,
Length minLength,
Angle angle,
const Scenario &scenario,
const Progress &p,
Transects &t,
string &errorString);
bool route(const BoostPolygon &area,
const Transects &transects,
Transects &transectsRouted,
Route &route,
string &errorString);
namespace flight_plan {
using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = vector<BoostPoint>;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const Scenario &scenario, const Progress &p,
Transects &t, string &errorString);
bool route(const BoostPolygon &area, const Transects &transects,
Transects &transectsRouted, Route &route, string &errorString);
} // namespace flight_plan
namespace detail {
const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
}
} // namespace snake
// operator== and operator!= for boost point
namespace boost {
namespace geometry {
namespace model {
bool operator==(snake::BoostPoint p1, snake::BoostPoint p2);
bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2);
} // namespace model
} // namespace geometry
} // namespace boost
src/Wima/Snake/SnakeDataManager.cc
View file @ c6e6a519
......@@ -4,32 +4,30 @@
#include <QMutexLocker>
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "QGCToolbox.h"
#include "WimaSettings.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <memory>
#include <shared_mutex>
#include <mutex>
#include <shared_mutex>
#include "snake.h"
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000 // ms
#define TIMEOUT 3000 // ms
using QVariantList = QList<QVariant>;
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
......@@ -38,549 +36,552 @@ using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class SnakeImpl : public QObject{
Q_OBJECT
class SnakeDataManager::SnakeImpl {
public:
SnakeImpl(SnakeDataManager* p);
bool precondition() const;
void resetWaypointData();
bool doTopicServiceSetup();
// Private data.
ROSBridgePtr pRosBridge;
bool rosBridgeEnabeled;
bool topicServiceSetupDone;
QTimer eventTimer;
QTimer timeout;
mutable std::shared_timed_mutex mutex;
// Scenario
snake::Scenario scenario;
Length lineDistance;
Length minTransectLength;
QList<QGeoCoordinate> mArea;
QList<QGeoCoordinate> sArea;
QList<QGeoCoordinate> corridor;
QGeoCoordinate ENUOrigin;
SnakeTiles tiles;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
// Waypoints
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QVector<QPointF> waypointsENU;
QVector<QPointF> arrivalPathENU;
QVector<QPointF> returnPathENU;
QString errorMessage;
// Other
std::atomic_bool calcInProgress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
SnakeDataManager *parent;
SnakeImpl(SnakeDataManager *p)
: rosBridgeEnabeled(false), topicServiceSetupDone(false),
lineDistance(1 * si::meter), minTransectLength(1 * si::meter),
calcInProgress(false), parent(p) {
// ROS Bridge.
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this] {
auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) {
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
QString defaultString("localhost:9090");
qgcApp()->showMessage("ROS Bridge connection string invalid: " +
connectionString);
qgcApp()->showMessage("Resetting connection string to: " +
defaultString);
connectionStringFact->setRawValue(
QVariant(defaultString)); // calls this function recursively
}
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
// Periodic.
connect(&this->eventTimer, &QTimer::timeout, [this] {
if (this->rosBridgeEnabeled) {
if (!this->pRosBridge->isRunning()) {
this->pRosBridge->start();
} else if (this->pRosBridge->isRunning() &&
this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
if (this->doTopicServiceSetup())
this->topicServiceSetupDone = true;
} else if (this->pRosBridge->isRunning() &&
!this->pRosBridge->connected() &&
this->topicServiceSetupDone) {
this->pRosBridge->reset();
this->pRosBridge->start();
this->topicServiceSetupDone = false;
}
} else if (this->pRosBridge->isRunning()) {
this->pRosBridge->reset();
this->topicServiceSetupDone = false;
}
});
this->eventTimer.start(EVENT_TIMER_INTERVAL);
}
bool precondition() const;
void resetOutput();
bool doTopicServiceSetup();
// Private data.
ROSBridgePtr pRosBridge;
bool rosBridgeEnabeled;
bool topicServiceSetupDone;
QTimer eventTimer;
QTimer timeout;
mutable std::shared_timed_mutex mutex;
// Scenario
snake::Scenario scenario;
Length lineDistance;
Length minTransectLength;
QList<QGeoCoordinate> mArea;
QList<QGeoCoordinate> sArea;
QList<QGeoCoordinate> corridor;
QGeoCoordinate ENUOrigin;
SnakeTiles tiles;
QmlObjectListModel tilesQml;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
// Waypoints
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QVector<QPointF> waypointsENU;
QVector<QPointF> arrivalPathENU;
QVector<QPointF> returnPathENU;
QString errorMessage;
// Other
std::atomic_bool calcInProgress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
SnakeDataManager *parent;
};
template<class AtomicType>
class ToggleRAII{
public:
ToggleRAII(AtomicType &t)
: _t(t)
{}
~ToggleRAII()
{
if ( _t.load() ){
_t.store(false);
} else {
_t.store(true);
bool SnakeDataManager::SnakeImpl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
UniqueLock lk(this->mutex);
if (this->tilesENU.polygons().size() == 0)
return false;
// Publish snake origin.
this->pRosBridge->advertiseTopic(
"/snake/origin", geographic_msgs::geo_point::messageType().c_str());
JsonDocUPtr jOrigin(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
jOrigin->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
this->pRosBridge->advertiseTopic(
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
JsonDocUPtr jSnakeTiles(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
// Subscribe nemo progress.
this->pRosBridge->subscribe(
"/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc) {
UniqueLock lk(this->mutex);
int requiredSize = this->tilesENU.polygons().size();
auto &progressMsg = this->qProgress;
if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
// Set progress to default.
progressMsg.progress().fill(0, requiredSize);
// Publish snake origin.
JsonDocUPtr jOrigin(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
}
private:
AtomicType &_t;
};
this->progress.clear();
for (const auto &p : progressMsg.progress()) {
this->progress.push_back(p);
}
lk.unlock();
emit this->parent->nemoProgressChanged();
});
// Subscribe /nemo/heartbeat.
this->pRosBridge->subscribe(
"/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) {
UniqueLock lk(this->mutex);
if (this->timeout.isActive()) {
this->timeout.stop();
}
auto &heartbeatMsg = this->heartbeat;
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
}
this->timeout.singleShot(TIMEOUT, [this] {
UniqueLock lk(this->mutex);
this->heartbeat.setStatus(integral(NemoStatus::Timeout));
emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
});
emit this->parent->nemoStatusChanged(heartbeatMsg.status());
});
// Advertise /snake/get_origin.
this->pRosBridge->advertiseService(
"/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr {
using namespace ros_bridge::messages;
SharedLock lk(this->mutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator());
lk.unlock();
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
return pDoc;
});
// Advertise /snake/get_tiles.
this->pRosBridge->advertiseService(
"/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr {
SharedLock lk(this->mutex);
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent)
, pImpl(std::make_unique<SnakeImpl>(this))
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
bool ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
return pDoc;
});
{
return true;
}
SnakeDataManager::~SnakeDataManager()
{
bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeDataManager::SnakeImpl::resetOutput() {
this->waypoints.clear();
this->arrivalPath.clear();
this->returnPath.clear();
this->ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
this->waypointsENU.clear();
this->arrivalPathENU.clear();
this->returnPathENU.clear();
this->tilesQml.clearAndDeleteContents();
this->tiles.polygons().clear();
this->tileCenterPoints.clear();
this->tilesENU.polygons().clear();
this->tileCenterPointsENU.clear();
this->errorMessage.clear();
}
void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->mArea = measurementArea;
}
template <class Callable> class CommandRAII {
public:
CommandRAII(Callable &fun) : _fun(fun) {}
~CommandRAII() { _fun(); }
void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->sArea = serviceArea;
private:
Callable _fun;
};
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent), pImpl(std::make_unique<SnakeImpl>(this))
{}
SnakeDataManager::~SnakeDataManager() {}
void SnakeDataManager::setMeasurementArea(
const QList<QGeoCoordinate> &measurementArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->mArea = measurementArea;
}
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->corridor = corridor;
void SnakeDataManager::setServiceArea(
const QList<QGeoCoordinate> &serviceArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->sArea = serviceArea;
}
QNemoProgress SnakeDataManager::nemoProgress()
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->qProgress;
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->corridor = corridor;
}
int SnakeDataManager::nemoStatus()
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->heartbeat.status();
const QmlObjectListModel *SnakeDataManager::tiles() const {
SharedLock lk(this->pImpl->mutex);
return &this->pImpl->tilesQml;
}
bool SnakeDataManager::calcInProgress()
{
return this->pImpl->calcInProgress.load();
QVariantList SnakeDataManager::tileCenterPoints() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->tileCenterPoints;
}
Length SnakeDataManager::lineDistance() const
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->lineDistance;
QNemoProgress SnakeDataManager::nemoProgress() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->qProgress;
}
void SnakeDataManager::setLineDistance(Length lineDistance)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->lineDistance = lineDistance;
int SnakeDataManager::nemoStatus() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->heartbeat.status();
}
Area SnakeDataManager::minTileArea() const
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.minTileArea();
bool SnakeDataManager::calcInProgress() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->calcInProgress.load();
}
void SnakeDataManager::setMinTileArea(Area minTileArea)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setMinTileArea(minTileArea);
QString SnakeDataManager::errorMessage() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->errorMessage;
}
Length SnakeDataManager::tileHeight() const
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileHeight();
bool SnakeDataManager::success() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->errorMessage.isEmpty() ? true : false;
}
void SnakeDataManager::setTileHeight(Length tileHeight)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileHeight(tileHeight);
QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->waypoints;
}
Length SnakeDataManager::tileWidth() const
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileWidth();
QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->arrivalPath;
}
void SnakeDataManager::setTileWidth(Length tileWidth)
{
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileWidth(tileWidth);
QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->returnPath;
}
void SnakeDataManager::enableRosBridge()
{
this->pImpl->rosBridgeEnabeled = true;
Length SnakeDataManager::lineDistance() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->lineDistance;
}
void SnakeDataManager::disableRosBride()
{
this->pImpl->rosBridgeEnabeled = false;
void SnakeDataManager::setLineDistance(Length lineDistance) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->lineDistance = lineDistance;
}
bool SnakeImpl::precondition() const
{
return true;
Area SnakeDataManager::minTileArea() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.minTileArea();
}
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeImpl::resetWaypointData()
{
this->waypoints.clear();
this->arrivalPath.clear();
this->returnPath.clear();
this->ENUOrigin = QGeoCoordinate(0.0,0.0,0.0);
this->waypointsENU.clear();
this->arrivalPathENU.clear();
this->returnPathENU.clear();
this->tiles.polygons().clear();
this->tileCenterPoints.clear();
this->tilesENU.polygons().clear();
this->tileCenterPointsENU.clear();
this->errorMessage.clear();
void SnakeDataManager::setMinTileArea(Area minTileArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setMinTileArea(minTileArea);
}
void SnakeDataManager::run()
{
this->pImpl->calcInProgress.store(true);
ToggleRAII<std::atomic_bool> tr(this->pImpl->calcInProgress);
Length SnakeDataManager::tileHeight() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileHeight();
}
UniqueLock lk(this->pImpl->mutex);
this->pImpl->resetWaypointData();
void SnakeDataManager::setTileHeight(Length tileHeight) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileHeight(tileHeight);
}
if ( !this->pImpl->precondition() )
return;
Length SnakeDataManager::tileWidth() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileWidth();
}
if ( this->pImpl->mArea.size() < 3) {
this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
return;
}
if ( this->pImpl->sArea.size() < 3) {
this->pImpl->errorMessage = "Service area invalid: size < 3.";
return;
}
void SnakeDataManager::setTileWidth(Length tileWidth) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileWidth(tileWidth);
}
void SnakeDataManager::enableRosBridge() {
this->pImpl->rosBridgeEnabeled = true;
}
this->pImpl->ENUOrigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUOrigin;
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->mArea;
mAreaEnu.clear();
for (auto geoVertex : mArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
mAreaEnu.outer().push_back(p);
}
void SnakeDataManager::disableRosBride() {
this->pImpl->rosBridgeEnabeled = false;
}
// Update service area.
auto &sAreaEnu = this->pImpl->scenario.serviceArea();
auto &sArea = this->pImpl->sArea;
sAreaEnu.clear();
for (auto geoVertex : sArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
sAreaEnu.outer().push_back(p);
void SnakeDataManager::run() {
this->pImpl->calcInProgress.store(true);
emit calcInProgressChanged(this->pImpl->calcInProgress.load());
auto onExit = [this] {
this->pImpl->calcInProgress.store(false);
emit calcInProgressChanged(this->pImpl->calcInProgress.load());
};
CommandRAII<decltype(onExit)> onExitRAII(onExit);
UniqueLock lk(this->pImpl->mutex);
this->pImpl->resetOutput();
if (!this->pImpl->precondition())
return;
if (this->pImpl->mArea.size() < 3) {
this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
return;
}
if (this->pImpl->sArea.size() < 3) {
this->pImpl->errorMessage = "Service area invalid: size < 3.";
return;
}
this->pImpl->ENUOrigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUOrigin;
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->mArea;
mAreaEnu.clear();
for (auto geoVertex : mArea) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
mAreaEnu.outer().push_back(p);
}
// Update service area.
auto &sAreaEnu = this->pImpl->scenario.serviceArea();
auto &sArea = this->pImpl->sArea;
sAreaEnu.clear();
for (auto geoVertex : sArea) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
sAreaEnu.outer().push_back(p);
}
// Update corridor.
auto &corridorEnu = this->pImpl->scenario.corridor();
auto &corridor = this->pImpl->corridor;
corridor.clear();
for (auto geoVertex : corridor) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
}
if (!this->pImpl->scenario.update()) {
this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
return;
}
// Asynchronously update flightplan.
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;
}
// Update corridor.
auto &corridorEnu = this->pImpl->scenario.corridor();
auto &corridor = this->pImpl->corridor;
corridor.clear();
for (auto geoVertex : corridor){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
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;
}
if ( !this->pImpl->scenario.update() ){
this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
return;
// 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);
}
// Asynchronously update flightplan.
qWarning() << "route calculation missing";
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 )
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 )
return value;
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < 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.
const auto &tiles = this->pImpl->scenario.tiles();
const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
SnakeTile geoTile;
SnakeTileLocal enuTile;
for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) {
auto &p = tile.outer()[i];
QPointF enuVertex(p.get<0>(), p.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, p, geoVertex);
enuTile.polygon().points().push_back(enuVertex);
geoTile.push_back(geoVertex);
}
const auto &boostPoint = centerPoints[i];
QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostPoint, geoVertex);
geoTile.setCenter(geoVertex);
this->pImpl->tiles.polygons().push_back(geoTile);
this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
this->pImpl->tilesENU.polygons().push_back(enuTile);
this->pImpl->tileCenterPointsENU.push_back(enuVertex);
}
// 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);
}
future.wait();
// Trying to generate flight plan.
if ( !future.get() ){
// error
this->pImpl->errorMessage = errorString.c_str();
} else {
// Success!!!
// 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);
}
}
bool SnakeImpl::doTopicServiceSetup()
{
using namespace ros_bridge::messages;
UniqueLock lk(this->mutex);
if ( this->tilesENU.polygons().size() == 0)
return false;
// Publish snake origin.
this->pRosBridge->advertiseTopic("/snake/origin",
geographic_msgs::geo_point::messageType().c_str());
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
this->pRosBridge->advertiseTopic("/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
// Subscribe nemo progress.
this->pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
UniqueLock lk(this->mutex);
int requiredSize = this->tilesENU.polygons().size();
auto& progressMsg = this->qProgress;
if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg)
|| progressMsg.progress().size() != requiredSize ) { // Some error occured.
// Set progress to default.
progressMsg.progress().fill(0, requiredSize);
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
this->progress.clear();
for (const auto&p : progressMsg.progress()){
this->progress.push_back(p);
}
lk.unlock();
emit this->parent->nemoProgressChanged();
});
// Subscribe /nemo/heartbeat.
this->pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc){
UniqueLock lk(this->mutex);
if ( this->timeout.isActive() ) {
this->timeout.stop();
}
auto &heartbeatMsg = this->heartbeat;
if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) {
heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
}
this->timeout.singleShot(TIMEOUT, [this]{
UniqueLock lk(this->mutex);
this->heartbeat.setStatus(integral(NemoStatus::Timeout));
emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
});
emit this->parent->nemoStatusChanged(heartbeatMsg.status());
});
// Advertise /snake/get_origin.
this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr
{
using namespace ros_bridge::messages;
SharedLock lk(this->mutex);
JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
bool ret = geographic_msgs::geo_point::toJson(
origin, jOrigin, pDoc->GetAllocator());
lk.unlock();
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
return pDoc;
});
// Advertise /snake/get_tiles.
this->pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr
{
SharedLock lk(this->mutex);
JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
bool ret = jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
return pDoc;
});
return true;
}
SnakeImpl::SnakeImpl(SnakeDataManager *p) :
rosBridgeEnabeled(false)
, topicServiceSetupDone(false)
, lineDistance(1*si::meter)
, minTransectLength(1*si::meter)
, calcInProgress(false)
, parent(p)
{
// ROS Bridge.
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this]{
auto connectionString = connectionStringFact->rawValue().toString();
if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
this->pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
QString defaultString("localhost:9090");
qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
qgcApp()->showMessage("Resetting connection string to: " + defaultString);
connectionStringFact->setRawValue(QVariant(defaultString)); // calls this function recursively
}
};
connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
setConnectionString();
// Periodic.
connect(&this->eventTimer, &QTimer::timeout, [this]{
if ( this->rosBridgeEnabeled ) {
if ( !this->pRosBridge->isRunning()) {
this->pRosBridge->start();
} else if ( this->pRosBridge->isRunning()
&& this->pRosBridge->connected()
&& !this->topicServiceSetupDone) {
if ( this->doTopicServiceSetup() )
this->topicServiceSetupDone = true;
} else if ( this->pRosBridge->isRunning()
&& !this->pRosBridge->connected()
&& this->topicServiceSetupDone){
this->pRosBridge->reset();
this->pRosBridge->start();
this->topicServiceSetupDone = false;
}
} else if ( this->pRosBridge->isRunning() ) {
this->pRosBridge->reset();
this->topicServiceSetupDone = false;
}
});
this->eventTimer.start(EVENT_TIMER_INTERVAL);
});
// Continue with storing scenario data in the mean time.
{
// Get tiles.
const auto &tiles = this->pImpl->scenario.tiles();
const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
for (unsigned int i = 0; i < tiles.size(); ++i) {
const auto &tile = tiles[i];
SnakeTile geoTile;
SnakeTileLocal enuTile;
for (size_t i = tile.outer().size(); i < tile.outer().size() - 1; ++i) {
auto &p = tile.outer()[i];
QPointF enuVertex(p.get<0>(), p.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, p, geoVertex);
enuTile.polygon().points().push_back(enuVertex);
geoTile.push_back(geoVertex);
}
const auto &boostPoint = centerPoints[i];
QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostPoint, geoVertex);
geoTile.setCenter(geoVertex);
this->pImpl->tilesQml.append(new SnakeTile(geoTile));
this->pImpl->tiles.polygons().push_back(geoTile);
this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
this->pImpl->tilesENU.polygons().push_back(enuTile);
this->pImpl->tileCenterPointsENU.push_back(enuVertex);
}
}
future.wait();
// Trying to generate flight plan.
if (!future.get()) {
// error
this->pImpl->errorMessage = errorString.c_str();
} else {
// Success!!!
}
}
src/Wima/Snake/SnakeDataManager.h
View file @ c6e6a519
#pragma once
#include <QGeoCoordinate>
#include <QList>
#include <QObject>
#include <QThread>
#include <QList>
#include <QGeoCoordinate>
#include "QNemoProgress.h"
#include "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "SnakeTiles.h"
#include <boost/units/systems/si.hpp>
using namespace boost::units;
using Length = quantity<si::length>;
using Area = quantity<si::area>;
class SnakeImpl;
enum class NemoStatus{
NotConnected = 0,
Connected = 1,
Timeout = -1,
InvalidHeartbeat = -2
enum class NemoStatus {
NotConnected = 0,
Connected = 1,
Timeout = -1,
InvalidHeartbeat = -2
};
class SnakeDataManager : public QThread{
Q_OBJECT
class SnakeDataManager : public QThread {
Q_OBJECT
public:
using SnakeImplPtr = std::unique_ptr<SnakeImpl>;
class SnakeImpl;
using SnakeImplPtr = std::unique_ptr<SnakeImpl>;
SnakeDataManager(QObject *parent = nullptr);
~SnakeDataManager() override;
public:
SnakeDataManager(QObject *parent = nullptr);
~SnakeDataManager() override;
void setMeasurementArea(const QList<QGeoCoordinate> &measurementArea);
void setServiceArea(const QList<QGeoCoordinate> &serviceArea);
void setCorridor(const QList<QGeoCoordinate> &corridor);
void setMeasurementArea (const QList<QGeoCoordinate> &measurementArea);
void setServiceArea (const QList<QGeoCoordinate> &serviceArea);
void setCorridor (const QList<QGeoCoordinate> &corridor);
const QmlObjectListModel *tiles() const;
QVariantList tileCenterPoints() const;
QNemoProgress nemoProgress() const;
int nemoStatus() const;
bool calcInProgress() const;
QString errorMessage() const;
bool success() const;
QNemoProgress nemoProgress();
int nemoStatus();
bool calcInProgress();
QVector<QGeoCoordinate> waypoints() const;
QVector<QGeoCoordinate> arrivalPath() const;
QVector<QGeoCoordinate> returnPath() const;
Length lineDistance() const;
void setLineDistance(Length lineDistance);
Length lineDistance() const;
void setLineDistance(Length lineDistance);
Length minTransectLength() const;
void setMinTransectLength(Length minTransectLength);
Length minTransectLength() const;
void setMinTransectLength(Length minTransectLength);
Area minTileArea() const;
void setMinTileArea(Area minTileArea);
Area minTileArea() const;
void setMinTileArea(Area minTileArea);
Length tileHeight() const;
void setTileHeight(Length tileHeight);
Length tileHeight() const;
void setTileHeight(Length tileHeight);
Length tileWidth() const;
void setTileWidth(Length tileWidth);
Length tileWidth() const;
void setTileWidth(Length tileWidth);
void enableRosBridge();
void disableRosBride();
void enableRosBridge();
void disableRosBride();
signals:
void nemoProgressChanged();
void nemoStatusChanged(int status);
void calcInProgressChanged(bool inProgress);
void nemoProgressChanged();
void nemoStatusChanged(int status);
void calcInProgressChanged(bool inProgress);
protected:
void run() override;
void run() override;
private:
SnakeImplPtr pImpl;
SnakeImplPtr pImpl;
};
src/Wima/WimaController.cc
View file @ c6e6a519
......@@ -2,37 +2,38 @@
#include "utilities.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoProgress.h"
#include "QVector3D"
#include <QScopedPointer>
#include <QtConcurrentRun>
#include <memory>
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
const char* WimaController::areaItemsName = "AreaItems";
const char* WimaController::missionItemsName = "MissionItems";
const char* WimaController::settingsGroup = "WimaController";
const char* WimaController::enableWimaControllerName = "EnableWimaController";
const char* WimaController::overlapWaypointsName = "OverlapWaypoints";
const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase";
const char* WimaController::startWaypointIndexName = "StartWaypointIndex";
const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems";
const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems";
const char* WimaController::flightSpeedName = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char* WimaController::altitudeName = "Altitude";
const char* WimaController::snakeTileWidthName = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength";
#define EVENT_TIMER_INTERVAL 50 // ms
const char *WimaController::areaItemsName = "AreaItems";
const char *WimaController::missionItemsName = "MissionItems";
const char *WimaController::settingsGroup = "WimaController";
const char *WimaController::enableWimaControllerName = "EnableWimaController";
const char *WimaController::overlapWaypointsName = "OverlapWaypoints";
const char *WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase";
const char *WimaController::startWaypointIndexName = "StartWaypointIndex";
const char *WimaController::showAllMissionItemsName = "ShowAllMissionItems";
const char *WimaController::showCurrentMissionItemsName =
"ShowCurrentMissionItems";
const char *WimaController::flightSpeedName = "FlightSpeed";
const char *WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char *WimaController::altitudeName = "Altitude";
const char *WimaController::snakeTileWidthName = "SnakeTileWidth";
const char *WimaController::snakeTileHeightName = "SnakeTileHeight";
const char *WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char *WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char *WimaController::snakeMinTransectLengthName =
"SnakeMinTransectLength";
WimaController::StatusMap WimaController::_nemoStatusMap{
std::make_pair<int, QString>(0, "No Heartbeat"),
......@@ -40,909 +41,754 @@ WimaController::StatusMap WimaController::_nemoStatusMap{
std::make_pair<int, QString>(-1, "Timeout"),
std::make_pair<int, QString>(-2, "Error")};
using namespace snake;
WimaController::WimaController(QObject *parent)
: QObject (parent)
, _joinedArea ()
, _measurementArea ()
, _serviceArea ()
, _corridor ()
, _localPlanDataValid (false)
, _areaInterface (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _managerSettings ()
, _defaultManager (_managerSettings, _areaInterface)
, _snakeManager (_managerSettings, _areaInterface)
, _rtlManager (_managerSettings, _areaInterface)
, _currentManager (&_defaultManager)
, _managerList {&_defaultManager, &_snakeManager, &_rtlManager}
, _metaDataMap (
FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
, _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
, _nextPhaseStartWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName])
, _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName])
, _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName])
, _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName])
, _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
, _altitude (settingsGroup, _metaDataMap[altitudeName])
, _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName])
, _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName])
, _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName])
, _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName])
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _lowBatteryHandlingTriggered (false)
, _measurementPathLength (-1)
, _snakeCalcInProgress (false)
, _nemoHeartbeat (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
, _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/)
, _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
, _topicServiceSetupDone (false)
{
// Set up facts.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
// Init waypoint managers.
bool value;
size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
Q_ASSERT(value);
(void)value;
for (auto manager : _managerList){
manager->setOverlap(overlap);
manager->setN(N);
manager->setStartIndex(startIdx);
}
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
//_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// Snake Worker Thread.
connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit);
// Snake.
auto configRosBridge = [this]{
if ( this->_enableSnake.rawValue().toBool() ){
this->_snakeDataManager.enableRosBridge();
} else {
this->_snakeDataManager.disableRosBride();
}
};
connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge);
configRosBridge();
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
_switchSnakeManager(_enableSnake.rawValue());
: QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(),
_corridor(), _localPlanDataValid(false),
_areaInterface(&_measurementArea, &_serviceArea, &_corridor,
&_joinedArea),
_WMSettings(), _defaultWM(_WMSettings, _areaInterface),
_snakeWM(_WMSettings, _areaInterface),
_rtlWM(_WMSettings, _areaInterface),
_currentWM(&_defaultWM), _WMList{&_defaultWM, &_snakeWM, &_rtlWM},
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
_enableWimaController(settingsGroup,
_metaDataMap[enableWimaControllerName]),
_overlapWaypoints(settingsGroup, _metaDataMap[overlapWaypointsName]),
_maxWaypointsPerPhase(settingsGroup,
_metaDataMap[maxWaypointsPerPhaseName]),
_nextPhaseStartWaypointIndex(settingsGroup,
_metaDataMap[startWaypointIndexName]),
_showAllMissionItems(settingsGroup,
_metaDataMap[showAllMissionItemsName]),
_showCurrentMissionItems(settingsGroup,
_metaDataMap[showCurrentMissionItemsName]),
_flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
_arrivalReturnSpeed(settingsGroup, _metaDataMap[arrivalReturnSpeedName]),
_altitude(settingsGroup, _metaDataMap[altitudeName]),
_snakeTileWidth(settingsGroup, _metaDataMap[snakeTileWidthName]),
_snakeTileHeight(settingsGroup, _metaDataMap[snakeTileHeightName]),
_snakeMinTileArea(settingsGroup, _metaDataMap[snakeMinTileAreaName]),
_snakeLineDistance(settingsGroup, _metaDataMap[snakeLineDistanceName]),
_snakeMinTransectLength(settingsGroup,
_metaDataMap[snakeMinTransectLengthName]),
_lowBatteryHandlingTriggered(false), _measurementPathLength(-1),
_currentDM(&_emptyDM),
_batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
// Set up facts.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
&WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
&WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
&WimaController::_setStartIndex);
connect(&_flightSpeed, &Fact::rawValueChanged, this,
&WimaController::_updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this,
&WimaController::_updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this,
&WimaController::_updateAltitude);
// Init waypoint managers.
bool value;
size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
Q_ASSERT(value);
(void)value;
for (auto manager : _WMList) {
manager->setOverlap(overlap);
manager->setN(N);
manager->setStartIndex(startIdx);
}
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this,
&WimaController::_eventTimerHandler);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// Snake Data Manager.
connect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
&WimaController::_progressChangedHandler);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusChanged);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusStringChanged);
connect(this, &QObject::destroyed, &this->_snakeDM, &SnakeDataManager::quit);
connect(this, &QObject::destroyed, &this->_emptyDM, &SnakeDataManager::quit);
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_enableSnakeChangedHandler);
_enableSnakeChangedHandler();
// Snake Waypoint Manager.
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_switchToSnakeWaypointManager);
_switchToSnakeWaypointManager(_enableSnake.rawValue());
}
PlanMasterController *WimaController::masterController() {
return _masterController;
return _masterController;
}
MissionController *WimaController::missionController() {
return _missionController;
return _missionController;
}
QmlObjectListModel *WimaController::visualItems() {
return &_areas;
}
QmlObjectListModel *WimaController::visualItems() { return &_areas; }
QmlObjectListModel *WimaController::missionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager->missionItems());
return const_cast<QmlObjectListModel *>(&_currentWM->missionItems());
}
QmlObjectListModel *WimaController::currentMissionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
return const_cast<QmlObjectListModel *>(&_currentWM->currentMissionItems());
}
QVariantList WimaController::waypointPath()
{
return const_cast<QVariantList&>(_currentManager->waypointsVariant());
QVariantList WimaController::waypointPath() {
return const_cast<QVariantList &>(_currentWM->waypointsVariant());
}
QVariantList WimaController::currentWaypointPath()
{
return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
QVariantList WimaController::currentWaypointPath() {
return const_cast<QVariantList &>(_currentWM->currentWaypointsVariant());
}
Fact *WimaController::enableWimaController() {
return &_enableWimaController;
}
Fact *WimaController::enableWimaController() { return &_enableWimaController; }
Fact *WimaController::overlapWaypoints() {
return &_overlapWaypoints;
}
Fact *WimaController::overlapWaypoints() { return &_overlapWaypoints; }
Fact *WimaController::maxWaypointsPerPhase() {
return &_maxWaypointsPerPhase;
}
Fact *WimaController::maxWaypointsPerPhase() { return &_maxWaypointsPerPhase; }
Fact *WimaController::startWaypointIndex() {
return &_nextPhaseStartWaypointIndex;
return &_nextPhaseStartWaypointIndex;
}
Fact *WimaController::showAllMissionItems() {
return &_showAllMissionItems;
}
Fact *WimaController::showAllMissionItems() { return &_showAllMissionItems; }
Fact *WimaController::showCurrentMissionItems() {
return &_showCurrentMissionItems;
return &_showCurrentMissionItems;
}
Fact *WimaController::flightSpeed() {
return &_flightSpeed;
}
Fact *WimaController::flightSpeed() { return &_flightSpeed; }
Fact *WimaController::arrivalReturnSpeed() {
return &_arrivalReturnSpeed;
}
Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() {
return &_altitude;
}
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles()
{
static QmlObjectListModel list;
qWarning() << "using snake tile dummy";
return &list;
QmlObjectListModel *WimaController::snakeTiles() {
return const_cast<QmlObjectListModel *>(this->_snakeDM.tiles());
}
QVariantList WimaController::snakeTileCenterPoints()
{
qWarning() << "using snakeTileCenterPoints dummy";
return QVariantList();
QVariantList WimaController::snakeTileCenterPoints() {
return _currentDM->tileCenterPoints();
}
QVector<int> WimaController::nemoProgress()
{
qWarning() << "using nemoProgress dummy";
return QVector<int>();
QVector<int> WimaController::nemoProgress() {
return _currentDM->nemoProgress().progress();
}
double WimaController::phaseDistance() const
{
return 0.0;
double WimaController::phaseDistance() const {
qWarning() << "using phaseDistance dummy";
return 0.0;
}
double WimaController::phaseDuration() const
{
return 0.0;
double WimaController::phaseDuration() const {
qWarning() << "using phaseDuration dummy";
return 0.0;
}
int WimaController::nemoStatus() const
{
qWarning() << "using nemoStatus dummy";
return 0;
}
int WimaController::nemoStatus() const { return _currentDM->nemoStatus(); }
QString WimaController::nemoStatusString() const
{
return _nemoStatusMap.at(nemoStatus());
QString WimaController::nemoStatusString() const {
return _nemoStatusMap.at(nemoStatus());
}
bool WimaController::snakeCalcInProgress() const
{
qWarning() << "using snakeCalcInProgress dummy";
return false;
bool WimaController::snakeCalcInProgress() const {
return _currentDM->calcInProgress();
}
void WimaController::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
_managerSettings.setMasterController(masterC);
emit masterControllerChanged();
void WimaController::setMasterController(PlanMasterController *masterC) {
_masterController = masterC;
_WMSettings.setMasterController(masterC);
emit masterControllerChanged();
}
void WimaController::setMissionController(MissionController *missionC)
{
_missionController = missionC;
_managerSettings.setMissionController(missionC);
emit missionControllerChanged();
void WimaController::setMissionController(MissionController *missionC) {
_missionController = missionC;
_WMSettings.setMissionController(missionC);
emit missionControllerChanged();
}
void WimaController::nextPhase()
{
_calcNextPhase();
}
void WimaController::nextPhase() { _calcNextPhase(); }
void WimaController::previousPhase()
{
if ( !_currentManager->previous() ) {
Q_ASSERT(false);
}
void WimaController::previousPhase() {
if (!_currentWM->previous()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::resetPhase()
{
if ( !_currentManager->reset() ) {
Q_ASSERT(false);
}
void WimaController::resetPhase() {
if (!_currentWM->reset()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::requestSmartRTL()
{
QString errorString("Smart RTL requested. ");
if ( !_checkSmartRTLPreCondition(errorString) ){
qgcApp()->showMessage(errorString);
return;
}
emit smartRTLRequestConfirm();
void WimaController::requestSmartRTL() {
QString errorString("Smart RTL requested. ");
if (!_checkSmartRTLPreCondition(errorString)) {
qgcApp()->showMessage(errorString);
return;
}
emit smartRTLRequestConfirm();
}
bool WimaController::upload()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
&& currentMissionItems.count() > 0) {
emit forceUploadConfirm();
return false;
}
bool WimaController::upload() {
auto &currentMissionItems = _defaultWM.currentMissionItems();
if (!_serviceArea.containsCoordinate(
_masterController->managerVehicle()->coordinate()) &&
currentMissionItems.count() > 0) {
emit forceUploadConfirm();
return false;
}
return forceUpload();
return forceUpload();
}
bool WimaController::forceUpload()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
if (currentMissionItems.count() < 1)
return false;
bool WimaController::forceUpload() {
auto &currentMissionItems = _defaultWM.currentMissionItems();
if (currentMissionItems.count() < 1)
return false;
_missionController->removeAll();
// Set homeposition of settingsItem.
QmlObjectListModel* visuals = _missionController->visualItems();
MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateCurrentMissionItems(): nullptr");
return false;
}
settingsItem->setCoordinate(_managerSettings.homePosition());
_missionController->removeAll();
// Set homeposition of settingsItem.
QmlObjectListModel *visuals = _missionController->visualItems();
MissionSettingsItem *settingsItem = visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateCurrentMissionItems(): nullptr");
return false;
}
settingsItem->setCoordinate(_WMSettings.homePosition());
// Copy mission items to _missionController.
for (int i = 1; i < currentMissionItems.count(); i++){
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
}
// Copy mission items to _missionController.
for (int i = 1; i < currentMissionItems.count(); i++) {
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
}
_masterController->sendToVehicle();
_masterController->sendToVehicle();
return true;
return true;
}
void WimaController::removeFromVehicle()
{
_masterController->removeAllFromVehicle();
_missionController->removeAll();
void WimaController::removeFromVehicle() {
_masterController->removeAllFromVehicle();
_missionController->removeAll();
}
void WimaController::executeSmartRTL()
{
forceUpload();
masterController()->managerVehicle()->startMission();
void WimaController::executeSmartRTL() {
forceUpload();
masterController()->managerVehicle()->startMission();
}
void WimaController::initSmartRTL()
{
_initSmartRTL();
}
void WimaController::initSmartRTL() { _initSmartRTL(); }
void WimaController::removeVehicleTrajectoryHistory()
{
Vehicle *managerVehicle = masterController()->managerVehicle();
managerVehicle->trajectoryPoints()->clear();
void WimaController::removeVehicleTrajectoryHistory() {
Vehicle *managerVehicle = masterController()->managerVehicle();
managerVehicle->trajectoryPoints()->clear();
}
bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
bool WimaController::setWimaPlanData(const WimaPlanData &planData)
{
// reset visual items
_areas.clear();
_defaultManager.clear();
_snakeManager.clear();
_snakeTiles.polygons().clear();
_snakeTilesLocal.polygons().clear();
_snakeTileCenterPoints.clear();
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
_localPlanDataValid = false;
// extract list with WimaAreas
QList<const WimaAreaData*> areaList = planData.areaList();
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0, 0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2D;
int areaCounter = 0;
const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i];
bool retVal =
PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
areaCounter++;
_areas.append(&_serviceArea);
continue;
}
if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea = *qobject_cast<const WimaMeasurementAreaData*>(areaData);
areaCounter++;
_areas.append(&_measurementArea);
return retVal;
}
continue;
}
bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
// reset visual items
_areas.clear();
_defaultWM.clear();
_snakeWM.clear();
if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
_corridor = *qobject_cast<const WimaCorridorData*>(areaData);
areaCounter++;
//_visualItems.append(&_corridor); // not needed
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
continue;
}
_localPlanDataValid = false;
if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast<const WimaJoinedAreaData*>(areaData);
areaCounter++;
_areas.append(&_joinedArea);
// extract list with WimaAreas
QList<const WimaAreaData *> areaList = planData.areaList();
continue;
}
int areaCounter = 0;
const int numAreas = 4; // extract only numAreas Areas, if there are more they
// are invalid and ignored
for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i];
if (areaCounter >= numAreas)
break;
}
if (areaData->type() ==
WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast<const WimaServiceAreaData *>(areaData);
areaCounter++;
_areas.append(&_serviceArea);
if (areaCounter != numAreas) {
Q_ASSERT(false);
return false;
continue;
}
emit visualItemsChanged();
// extract mission items
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1) {
qWarning("WimaController: Mission items from WimaPlaner empty!");
return false;
}
if (areaData->type() ==
WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea =
*qobject_cast<const WimaMeasurementAreaData *>(areaData);
areaCounter++;
_areas.append(&_measurementArea);
for (auto item : tempMissionItems) {
_defaultManager.push_back(item.coordinate());
continue;
}
_managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
_serviceArea.center().longitude(),
0) );
if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
_corridor = *qobject_cast<const WimaCorridorData *>(areaData);
areaCounter++;
//_visualItems.append(&_corridor); // not needed
if( !_defaultManager.reset() ){
Q_ASSERT(false);
return false;
continue;
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
// Initialize _scenario.
Area mArea;
for (auto variant : _measurementArea.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
mArea.type = AreaType::MeasurementArea;
if (areaData->type() ==
WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast<const WimaJoinedAreaData *>(areaData);
areaCounter++;
_areas.append(&_joinedArea);
Area sArea;
for (auto variant : _serviceArea.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
continue;
}
sArea.type = AreaType::ServiceArea;
Area corridor;
for (auto variant : _corridor.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
corridor.type = AreaType::Corridor;
_scenario.addArea(mArea);
_scenario.addArea(sArea);
_scenario.addArea(corridor);
// Check if scenario is defined.
if ( !_scenario.defined(_snakeTileWidth.rawValue().toUInt(),
_snakeTileHeight.rawValue().toUInt(),
_snakeMinTileArea.rawValue().toUInt()) ) {
Q_ASSERT(false);
return false;
}
{
// Get tiles and origin.
auto origin = _scenario.getOrigin();
_snakeOrigin.setLatitude(origin[0]);
_snakeOrigin.setLongitude(origin[1]);
_snakeOrigin.setAltitude(origin[2]);
const auto &tiles = _scenario.getTiles();
const auto &cps = _scenario.getTileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
SnakeTile Tile;
for ( const auto &vertex : tile) {
QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
Tile.append(QVertex);
}
const auto &centerPoint = cps[i];
QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
Tile.setCenter(QCenterPoint);
_snakeTiles.polygons().append(Tile);
_snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint));
}
}
{
// Get local tiles.
const auto &tiles = _scenario.getTilesENU();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
Polygon2D Tile;
for ( const auto &vertex : tile.outer()) {
QPointF QVertex(vertex.get<0>(), vertex.get<1>());
Tile.path().append(QVertex);
}
_snakeTilesLocal.polygons().append(Tile);
}
}
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
if ( _enableSnake.rawValue().toBool()
&& _pRosBridge->isRunning()
&& _pRosBridge->connected()
&& _topicServiceSetupDone
&& _snakeTilesLocal.polygons().size() > 0
)
{
using namespace ros_bridge::messages;
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
Q_ASSERT(ret);
_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
_localPlanDataValid = true;
return true;
}
WimaController *WimaController::thisPointer()
{
return this;
}
bool WimaController::_calcNextPhase()
{
if ( !_currentManager->next() ) {
Q_ASSERT(false);
return false;
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
if (areaCounter >= numAreas)
break;
}
bool WimaController::_setStartIndex()
{
bool value;
_currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
Q_ASSERT(value);
(void)value;
if (areaCounter != numAreas) {
Q_ASSERT(false);
return false;
}
if ( !_currentManager->update() ) {
Q_ASSERT(false);
return false;
}
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
// extract mission items
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1) {
qWarning("WimaController: Mission items from WimaPlaner empty!");
return false;
}
return true;
}
for (auto item : tempMissionItems) {
_defaultWM.push_back(item.coordinate());
}
void WimaController::_recalcCurrentPhase()
{
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
_WMSettings.setHomePosition(QGeoCoordinate(
_serviceArea.center().latitude(), _serviceArea.center().longitude(), 0));
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
if (!_defaultWM.reset()) {
Q_ASSERT(false);
return false;
}
void WimaController::_updateOverlap()
{
bool value;
_currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
if ( !_currentManager->update() ) {
assert(false);
}
// Update Snake Data Manager
_snakeDM.setMeasurementArea(_measurementArea.coordinateList());
_snakeDM.setServiceArea(_serviceArea.coordinateList());
_snakeDM.setCorridor(_corridor.coordinateList());
_currentDM->start();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
_localPlanDataValid = true;
return true;
}
void WimaController::_updateMaxWaypoints()
{
bool value;
_currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
WimaController *WimaController::thisPointer() { return this; }
void WimaController::_updateflightSpeed()
{
bool value;
_managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
bool WimaController::_calcNextPhase() {
if (!_currentWM->next()) {
Q_ASSERT(false);
return false;
}
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
void WimaController::_updateArrivalReturnSpeed()
{
bool value;
_managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
bool WimaController::_setStartIndex() {
bool value;
_currentWM->setStartIndex(
_nextPhaseStartWaypointIndex.rawValue().toUInt(&value) - 1);
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateAltitude()
{
bool value;
_managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
Q_ASSERT(false);
return false;
}
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
void WimaController::_checkBatteryLevel()
{
Vehicle *managerVehicle = masterController()->managerVehicle();
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
if (managerVehicle != nullptr && enabled == true) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
void WimaController::_recalcCurrentPhase() {
if (!_currentWM->update()) {
Q_ASSERT(false);
}
if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
&& battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
if (!_lowBatteryHandlingTriggered) {
_lowBatteryHandlingTriggered = true;
if ( !(_missionController->remainingTime() <= minTime) ) {
requestSmartRTL();
}
}
}
else {
_lowBatteryHandlingTriggered = false;
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateOverlap() {
bool value;
_currentWM->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateMaxWaypoints() {
bool value;
_currentWM->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateflightSpeed() {
bool value;
_WMSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateArrivalReturnSpeed() {
bool value;
_WMSettings.setArrivalReturnSpeed(
_arrivalReturnSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateAltitude() {
bool value;
_WMSettings.setAltitude(_altitude.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if (!_currentWM->update()) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_checkBatteryLevel() {
Vehicle *managerVehicle = masterController()->managerVehicle();
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
int batteryThreshold =
wimaSettings->lowBatteryThreshold()->rawValue().toInt();
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime =
wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
if (managerVehicle != nullptr && enabled == true) {
Fact *battery1percentRemaining =
managerVehicle->battery1FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining =
managerVehicle->battery2FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold &&
battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
if (!_lowBatteryHandlingTriggered) {
_lowBatteryHandlingTriggered = true;
if (!(_missionController->remainingTime() <= minTime)) {
requestSmartRTL();
}
}
} else {
_lowBatteryHandlingTriggered = false;
}
}
}
void WimaController::_eventTimerHandler()
{
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
if ( enableLowBatteryHandling->rawValue().toBool() && _batteryLevelTicker.ready() )
_checkBatteryLevel();
// Snake flight plan update necessary?
// if ( snakeEventLoopTicker.ready() ) {
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
// }
// }
if ( _nemoTimeoutTicker.ready() && _enableSnake.rawValue().toBool() ) {
this->_nemoHeartbeat.setStatus(_fallbackStatus);
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
}
void WimaController::_eventTimerHandler() {
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()
->toolbox()
->settingsManager()
->wimaSettings()
->enableLowBatteryHandling();
if (enableLowBatteryHandling->rawValue().toBool() &&
_batteryLevelTicker.ready())
_checkBatteryLevel();
}
void WimaController::_smartRTLCleanUp(bool flying)
{
if ( !flying) { // vehicle has landed
_switchWaypointManager(_defaultManager);
_missionController->removeAllFromVehicle();
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
}
}
void WimaController::_setPhaseDistance(double distance)
{
(void)distance;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// _phaseDistance = distance;
// emit phaseDistanceChanged();
// }
void WimaController::_smartRTLCleanUp(bool flying) {
if (!flying) { // vehicle has landed
_switchWaypointManager(_defaultWM);
_missionController->removeAllFromVehicle();
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged,
this, &WimaController::_smartRTLCleanUp);
}
}
void WimaController::_setPhaseDuration(double duration)
{
(void)duration;
// if (!qFuzzyCompare(duration, _phaseDuration)) {
// _phaseDuration = duration;
void WimaController::_setPhaseDistance(double distance) {
(void)distance;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// _phaseDistance = distance;
// emit phaseDurationChanged();
// }
// emit phaseDistanceChanged();
// }
}
bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
{
if (!_localPlanDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
return false;
}
void WimaController::_setPhaseDuration(double duration) {
(void)duration;
// if (!qFuzzyCompare(duration, _phaseDuration)) {
// _phaseDuration = duration;
return _rtlManager.checkPrecondition(errorString);
// emit phaseDurationChanged();
// }
}
void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
{
if (_currentManager != &manager) {
_currentManager = &manager;
disconnect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
_maxWaypointsPerPhase.setRawValue(_currentManager->N());
_overlapWaypoints.setRawValue(_currentManager->overlap());
_nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
connect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
}
}
bool WimaController::_checkSmartRTLPreCondition(QString &errorString) {
if (!_localPlanDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a "
"measurement and a service area."));
return false;
}
void WimaController::_initSmartRTL()
{
QString errorString;
static int attemptCounter = 0;
attemptCounter++;
if ( _checkSmartRTLPreCondition(errorString) ) {
_masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
if ( _rtlManager.update() ) { // Calculate return path.
_switchWaypointManager(_rtlManager);
attemptCounter = 0;
emit smartRTLPathConfirm();
return;
}
} else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
}
return _rtlWM.checkPrecondition(errorString);
}
void WimaController::_setSnakeCalcInProgress(bool inProgress)
{
if (_snakeCalcInProgress != inProgress){
_snakeCalcInProgress = inProgress;
emit snakeCalcInProgressChanged();
}
}
void WimaController::_switchWaypointManager(
WaypointManager::ManagerBase &manager) {
if (_currentWM != &manager) {
_currentWM = &manager;
void WimaController::_snakeStoreWorkerResults()
{
_setSnakeCalcInProgress(false);
auto start = std::chrono::high_resolution_clock::now();
_snakeManager.clear();
disconnect(&_overlapWaypoints, &Fact::rawValueChanged, this,
&WimaController::_updateOverlap);
disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
&WimaController::_updateMaxWaypoints);
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
&WimaController::_setStartIndex);
const auto &r = _snakeDataManager.getResult();
if (!r.success) {
//qgcApp()->showMessage(r.errorMessage);
return;
}
// create Mission items from r.waypoints
long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
if (n < 1) {
return;
}
// Create QVector<QGeoCoordinate> containing all waypoints;
unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first();
for (unsigned long i = startIdx; i <= endIdx; ++i) {
_snakeManager.push_back(r.waypoints[int(i)].value<QGeoCoordinate>());
}
_maxWaypointsPerPhase.setRawValue(_currentWM->N());
_overlapWaypoints.setRawValue(_currentWM->overlap());
_nextPhaseStartWaypointIndex.setRawValue(_currentWM->startIndex());
_snakeManager.update();
connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
&WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
&WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
&WimaController::_setStartIndex);
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
auto end = std::chrono::high_resolution_clock::now();
double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
qWarning()
<< "WimaController::_switchWaypointManager: statistics update missing.";
}
}
void WimaController::_initSmartRTL() {
QString errorString;
static int attemptCounter = 0;
attemptCounter++;
if (_checkSmartRTLPreCondition(errorString)) {
_masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this,
&WimaController::_smartRTLCleanUp);
if (_rtlWM.update()) { // Calculate return path.
_switchWaypointManager(_rtlWM);
attemptCounter = 0;
emit smartRTLPathConfirm();
return;
}
} else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
errorString.append(
tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this,
&WimaController::_initSmartRTL);
}
}
void WimaController::_DMFinishedHandler() {
if (!_snakeDM.success()) {
// qgcApp()->showMessage(r.errorMessage);
return;
}
// Copy waypoints to waypoint manager.
_snakeWM.clear();
auto waypoints = _snakeDM.waypoints();
if (waypoints.size() < 1) {
return;
}
for (auto &vertex : waypoints) {
_snakeWM.push_back(vertex);
}
// Do update.
auto fut = QtConcurrent::run([this] {
this->_snakeWM.update(); // this can take a while (ca. 200ms)
emit this->missionItemsChanged();
emit this->currentMissionItemsChanged();
emit this->currentWaypointPathChanged();
emit this->waypointPathChanged();
});
(void)fut;
}
void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
if (variant.value<bool>()) {
_switchWaypointManager(_snakeWM);
} else {
_switchWaypointManager(_defaultWM);
}
}
void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
if (_currentDM != &dataManager) {
disconnect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
disconnect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
&WimaController::_progressChangedHandler);
disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusChanged);
disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusStringChanged);
_currentDM = &dataManager;
connect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
&WimaController::_progressChangedHandler);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusChanged);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
&WimaController::nemoStatusStringChanged);
emit snakeConnectionStatusChanged();
emit snakeCalcInProgressChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
emit nemoProgressChanged();
emit nemoStatusChanged();
emit nemoStatusStringChanged();
}
}
void WimaController::_initStartSnakeWorker()
{
if ( !_enableSnake.rawValue().toBool() )
return;
// Stop worker thread if running.
if ( _snakeDataManager.isRunning() ) {
_snakeDataManager.quit();
}
// Initialize _snakeWorker.
_snakeDataManager.setProgress(
_nemoProgress.progress());
_snakeDataManager.setLineDistance(
_snakeLineDistance.rawValue().toDouble());
_snakeDataManager.setMinTransectLength(
_snakeMinTransectLength.rawValue().toDouble());
_snakeDataManager.setTileHeight(
_snakeTileHeight.rawValue().toDouble());
_snakeDataManager.setTileWidth(
_snakeTileWidth.rawValue().toDouble());
_snakeDataManager.setMinTileArea(
_snakeMinTileArea.rawValue().toDouble());
_setSnakeCalcInProgress(true);
// Start worker thread.
_snakeDataManager.start();
}
void WimaController::_switchSnakeManager(QVariant variant)
{
if (variant.value<bool>()){
_switchWaypointManager(_snakeManager);
} else {
_switchWaypointManager(_defaultManager);
}
void WimaController::_progressChangedHandler() {
emit this->nemoProgressChanged();
this->_currentDM->start();
}
void WimaController::_enableSnakeChangedHandler() {
if (this->_enableSnake.rawValue().toBool()) {
this->_snakeDM.enableRosBridge();
_switchDataManager(_snakeDM);
_currentDM->start();
} else {
this->_snakeDM.disableRosBride();
_switchDataManager(_emptyDM);
}
}
src/Wima/WimaController.h
View file @ c6e6a519
......@@ -7,32 +7,29 @@
#include "QmlObjectListModel.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "PlanMasterController.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "JsonHelper.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "SimpleMissionItem.h"
#include "SurveyComplexItem.h"
#include "snake.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
#include "Geometry/GeoPoint3D.h"
#include "ros_bridge/include/ros_bridge.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
......@@ -41,363 +38,285 @@
#include <map>
using namespace snake;
typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
class WimaController : public QObject
{
Q_OBJECT
enum FileType {WimaFile, PlanFile};
public:
WimaController(QObject *parent = nullptr);
class WimaController : public QObject {
Q_OBJECT
enum FileType { WimaFile, PlanFile };
// Controllers.
Q_PROPERTY(PlanMasterController* masterController
READ masterController
WRITE setMasterController
NOTIFY masterControllerChanged
)
Q_PROPERTY(MissionController* missionController
READ missionController
WRITE setMissionController
NOTIFY missionControllerChanged
)
// Wima Data.
Q_PROPERTY(QmlObjectListModel* visualItems
READ visualItems
NOTIFY visualItemsChanged
)
Q_PROPERTY(QmlObjectListModel* missionItems
READ missionItems
NOTIFY missionItemsChanged
)
Q_PROPERTY(QmlObjectListModel* currentMissionItems
READ currentMissionItems
NOTIFY currentMissionItemsChanged
)
Q_PROPERTY(QVariantList waypointPath
READ waypointPath
NOTIFY waypointPathChanged
)
Q_PROPERTY(QVariantList currentWaypointPath
READ currentWaypointPath
NOTIFY currentWaypointPathChanged
)
Q_PROPERTY(Fact* enableWimaController
READ enableWimaController
CONSTANT)
// Waypoint navigaton.
Q_PROPERTY(Fact* overlapWaypoints
READ overlapWaypoints
CONSTANT
)
Q_PROPERTY(Fact* maxWaypointsPerPhase
READ maxWaypointsPerPhase
CONSTANT
)
Q_PROPERTY(Fact* startWaypointIndex
READ startWaypointIndex
CONSTANT
)
Q_PROPERTY(Fact* showAllMissionItems
READ showAllMissionItems
CONSTANT
)
Q_PROPERTY(Fact* showCurrentMissionItems
READ showCurrentMissionItems
CONSTANT
)
// Waypoint settings.
Q_PROPERTY(Fact* flightSpeed
READ flightSpeed
CONSTANT
)
Q_PROPERTY(Fact* altitude
READ altitude
CONSTANT
)
Q_PROPERTY(Fact* arrivalReturnSpeed
READ arrivalReturnSpeed
CONSTANT
)
// Waypoint statistics.
Q_PROPERTY(double phaseDistance
READ phaseDistance
NOTIFY phaseDistanceChanged
)
Q_PROPERTY(double phaseDuration
READ phaseDuration
NOTIFY phaseDurationChanged
)
// Snake
Q_PROPERTY(Fact* enableSnake
READ enableSnake
CONSTANT
)
Q_PROPERTY(int nemoStatus
READ nemoStatus
NOTIFY nemoStatusChanged
)
Q_PROPERTY(QString nemoStatusString
READ nemoStatusString
NOTIFY nemoStatusStringChanged
)
Q_PROPERTY(bool snakeCalcInProgress
READ snakeCalcInProgress
NOTIFY snakeCalcInProgressChanged
)
Q_PROPERTY(Fact* snakeTileWidth
READ snakeTileWidth
CONSTANT
)
Q_PROPERTY(Fact* snakeTileHeight
READ snakeTileHeight
CONSTANT
)
Q_PROPERTY(Fact* snakeMinTileArea
READ snakeMinTileArea
CONSTANT
)
Q_PROPERTY(Fact* snakeLineDistance
READ snakeLineDistance
CONSTANT
)
Q_PROPERTY(Fact* snakeMinTransectLength
READ snakeMinTransectLength
CONSTANT
)
Q_PROPERTY(QmlObjectListModel* snakeTiles
READ snakeTiles
NOTIFY snakeTilesChanged
)
Q_PROPERTY(QVariantList snakeTileCenterPoints
READ snakeTileCenterPoints
NOTIFY snakeTileCenterPointsChanged
)
Q_PROPERTY(QVector<int> nemoProgress
READ nemoProgress
NOTIFY nemoProgressChanged
)
// Property accessors
// Controllers.
PlanMasterController* masterController (void);
MissionController* missionController (void);
// Wima Data
QmlObjectListModel* visualItems (void);
QGCMapPolygon joinedArea (void) const;
// Waypoints.
QmlObjectListModel* missionItems (void);
QmlObjectListModel* currentMissionItems (void);
QVariantList waypointPath (void);
QVariantList currentWaypointPath (void);
// Settings facts.
Fact* enableWimaController (void);
Fact* overlapWaypoints (void);
Fact* maxWaypointsPerPhase (void);
Fact* startWaypointIndex (void);
Fact* showAllMissionItems (void);
Fact* showCurrentMissionItems(void);
Fact* flightSpeed (void);
Fact* arrivalReturnSpeed (void);
Fact* altitude (void);
// Snake settings facts.
Fact* enableSnake (void) { return &_enableSnake; }
Fact* snakeTileWidth (void) { return &_snakeTileWidth;}
Fact* snakeTileHeight (void) { return &_snakeTileHeight;}
Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;}
Fact* snakeLineDistance (void) { return &_snakeLineDistance;}
Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;}
// Snake data.
QmlObjectListModel* snakeTiles (void);
QVariantList snakeTileCenterPoints (void);
QVector<int> nemoProgress (void);
int nemoStatus (void) const;
QString nemoStatusString (void) const;
bool snakeCalcInProgress (void) const;
// Smart RTL.
bool uploadOverrideRequired (void) const;
bool vehicleHasLowBattery (void) const;
// Waypoint statistics.
double phaseDistance (void) const;
double phaseDuration (void) const;
// Property setters
void setMasterController (PlanMasterController* masterController);
void setMissionController (MissionController* missionController);
bool setWimaPlanData (const WimaPlanData &planData);
// Member Methodes
Q_INVOKABLE WimaController *thisPointer();
// Waypoint navigation.
Q_INVOKABLE void nextPhase();
Q_INVOKABLE void previousPhase();
Q_INVOKABLE void resetPhase();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload();
Q_INVOKABLE bool forceUpload();
Q_INVOKABLE void removeFromVehicle();
// static Members
static const char* areaItemsName;
static const char* missionItemsName;
static const char* settingsGroup;
static const char* endWaypointIndexName;
static const char* enableWimaControllerName;
static const char* overlapWaypointsName;
static const char* maxWaypointsPerPhaseName;
static const char* startWaypointIndexName;
static const char* showAllMissionItemsName;
static const char* showCurrentMissionItemsName;
static const char* flightSpeedName;
static const char* arrivalReturnSpeedName;
static const char* altitudeName;
static const char* snakeTileWidthName;
static const char* snakeTileHeightName;
static const char* snakeMinTileAreaName;
static const char* snakeLineDistanceName;
static const char* snakeMinTransectLengthName;
public:
WimaController(QObject *parent = nullptr);
// Controllers.
Q_PROPERTY(PlanMasterController *masterController READ masterController WRITE
setMasterController NOTIFY masterControllerChanged)
Q_PROPERTY(MissionController *missionController READ missionController WRITE
setMissionController NOTIFY missionControllerChanged)
// Wima Data.
Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY
visualItemsChanged)
Q_PROPERTY(QmlObjectListModel *missionItems READ missionItems NOTIFY
missionItemsChanged)
Q_PROPERTY(QmlObjectListModel *currentMissionItems READ currentMissionItems
NOTIFY currentMissionItemsChanged)
Q_PROPERTY(
QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged)
Q_PROPERTY(QVariantList currentWaypointPath READ currentWaypointPath NOTIFY
currentWaypointPathChanged)
Q_PROPERTY(Fact *enableWimaController READ enableWimaController CONSTANT)
// Waypoint navigaton.
Q_PROPERTY(Fact *overlapWaypoints READ overlapWaypoints CONSTANT)
Q_PROPERTY(Fact *maxWaypointsPerPhase READ maxWaypointsPerPhase CONSTANT)
Q_PROPERTY(Fact *startWaypointIndex READ startWaypointIndex CONSTANT)
Q_PROPERTY(Fact *showAllMissionItems READ showAllMissionItems CONSTANT)
Q_PROPERTY(
Fact *showCurrentMissionItems READ showCurrentMissionItems CONSTANT)
// Waypoint settings.
Q_PROPERTY(Fact *flightSpeed READ flightSpeed CONSTANT)
Q_PROPERTY(Fact *altitude READ altitude CONSTANT)
Q_PROPERTY(Fact *arrivalReturnSpeed READ arrivalReturnSpeed CONSTANT)
// Waypoint statistics.
Q_PROPERTY(
double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged)
Q_PROPERTY(
double phaseDuration READ phaseDuration NOTIFY phaseDurationChanged)
// Snake
Q_PROPERTY(Fact *enableSnake READ enableSnake CONSTANT)
Q_PROPERTY(int nemoStatus READ nemoStatus NOTIFY nemoStatusChanged)
Q_PROPERTY(QString nemoStatusString READ nemoStatusString NOTIFY
nemoStatusStringChanged)
Q_PROPERTY(bool snakeCalcInProgress READ snakeCalcInProgress NOTIFY
snakeCalcInProgressChanged)
Q_PROPERTY(Fact *snakeTileWidth READ snakeTileWidth CONSTANT)
Q_PROPERTY(Fact *snakeTileHeight READ snakeTileHeight CONSTANT)
Q_PROPERTY(Fact *snakeMinTileArea READ snakeMinTileArea CONSTANT)
Q_PROPERTY(Fact *snakeLineDistance READ snakeLineDistance CONSTANT)
Q_PROPERTY(Fact *snakeMinTransectLength READ snakeMinTransectLength CONSTANT)
Q_PROPERTY(
QmlObjectListModel *snakeTiles READ snakeTiles NOTIFY snakeTilesChanged)
Q_PROPERTY(QVariantList snakeTileCenterPoints READ snakeTileCenterPoints
NOTIFY snakeTileCenterPointsChanged)
Q_PROPERTY(
QVector<int> nemoProgress READ nemoProgress NOTIFY nemoProgressChanged)
// Property accessors
// Controllers.
PlanMasterController *masterController(void);
MissionController *missionController(void);
// Wima Data
QmlObjectListModel *visualItems(void);
QGCMapPolygon joinedArea(void) const;
// Waypoints.
QmlObjectListModel *missionItems(void);
QmlObjectListModel *currentMissionItems(void);
QVariantList waypointPath(void);
QVariantList currentWaypointPath(void);
// Settings facts.
Fact *enableWimaController(void);
Fact *overlapWaypoints(void);
Fact *maxWaypointsPerPhase(void);
Fact *startWaypointIndex(void);
Fact *showAllMissionItems(void);
Fact *showCurrentMissionItems(void);
Fact *flightSpeed(void);
Fact *arrivalReturnSpeed(void);
Fact *altitude(void);
// Snake settings facts.
Fact *enableSnake(void) { return &_enableSnake; }
Fact *snakeTileWidth(void) { return &_snakeTileWidth; }
Fact *snakeTileHeight(void) { return &_snakeTileHeight; }
Fact *snakeMinTileArea(void) { return &_snakeMinTileArea; }
Fact *snakeLineDistance(void) { return &_snakeLineDistance; }
Fact *snakeMinTransectLength(void) { return &_snakeMinTransectLength; }
// Snake data.
QmlObjectListModel *snakeTiles(void);
QVariantList snakeTileCenterPoints(void);
QVector<int> nemoProgress(void);
int nemoStatus(void) const;
QString nemoStatusString(void) const;
bool snakeCalcInProgress(void) const;
// Smart RTL.
bool uploadOverrideRequired(void) const;
bool vehicleHasLowBattery(void) const;
// Waypoint statistics.
double phaseDistance(void) const;
double phaseDuration(void) const;
// Property setters
void setMasterController(PlanMasterController *masterController);
void setMissionController(MissionController *missionController);
bool setWimaPlanData(const WimaPlanData &planData);
// Member Methodes
Q_INVOKABLE WimaController *thisPointer();
// Waypoint navigation.
Q_INVOKABLE void nextPhase();
Q_INVOKABLE void previousPhase();
Q_INVOKABLE void resetPhase();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload();
Q_INVOKABLE bool forceUpload();
Q_INVOKABLE void removeFromVehicle();
// static Members
static const char *areaItemsName;
static const char *missionItemsName;
static const char *settingsGroup;
static const char *endWaypointIndexName;
static const char *enableWimaControllerName;
static const char *overlapWaypointsName;
static const char *maxWaypointsPerPhaseName;
static const char *startWaypointIndexName;
static const char *showAllMissionItemsName;
static const char *showCurrentMissionItemsName;
static const char *flightSpeedName;
static const char *arrivalReturnSpeedName;
static const char *altitudeName;
static const char *snakeTileWidthName;
static const char *snakeTileHeightName;
static const char *snakeMinTileAreaName;
static const char *snakeLineDistanceName;
static const char *snakeMinTransectLengthName;
signals:
// Controllers.
void masterControllerChanged (void);
void missionControllerChanged (void);
// Wima data.
void visualItemsChanged (void);
// Waypoints.
void missionItemsChanged (void);
void currentMissionItemsChanged (void);
void waypointPathChanged (void);
void currentWaypointPathChanged (void);
// Smart RTL.
void smartRTLRequestConfirm (void);
void smartRTLPathConfirm (void);
// Upload.
void forceUploadConfirm (void);
// Waypoint statistics.
void phaseDistanceChanged (void);
void phaseDurationChanged (void);
// Snake.
void snakeConnectionStatusChanged (void);
void snakeCalcInProgressChanged (void);
void snakeTilesChanged (void);
void snakeTileCenterPointsChanged (void);
void nemoProgressChanged (void);
void nemoStatusChanged (void);
void nemoStatusStringChanged (void);
// Controllers.
void masterControllerChanged(void);
void missionControllerChanged(void);
// Wima data.
void visualItemsChanged(void);
// Waypoints.
void missionItemsChanged(void);
void currentMissionItemsChanged(void);
void waypointPathChanged(void);
void currentWaypointPathChanged(void);
// Smart RTL.
void smartRTLRequestConfirm(void);
void smartRTLPathConfirm(void);
// Upload.
void forceUploadConfirm(void);
// Waypoint statistics.
void phaseDistanceChanged(void);
void phaseDurationChanged(void);
// Snake.
void snakeConnectionStatusChanged(void);
void snakeCalcInProgressChanged(void);
void snakeTilesChanged(void);
void snakeTileCenterPointsChanged(void);
void nemoProgressChanged(void);
void nemoStatusChanged(void);
void nemoStatusStringChanged(void);
private slots:
// Waypoint navigation / helper.
bool _calcNextPhase (void);
void _recalcCurrentPhase (void);
bool _calcShortestPath (const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
// Slicing parameters
bool _setStartIndex (void);
void _updateOverlap (void);
void _updateMaxWaypoints (void);
// Waypoint settings.
void _updateflightSpeed (void);
void _updateArrivalReturnSpeed (void);
void _updateAltitude (void);
// Waypoint Statistics.
void _setPhaseDistance (double distance);
void _setPhaseDuration (double duration);
// SMART RTL
void _checkBatteryLevel (void);
bool _checkSmartRTLPreCondition (QString &errorString);
void _initSmartRTL ();
void _smartRTLCleanUp (bool flying);
// Snake.
void _setSnakeCalcInProgress (bool inProgress);
void _snakeStoreWorkerResults ();
void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant);
bool _doTopicServiceSetup();
// Periodic tasks.
void _eventTimerHandler (void);
// Waypoint manager handling.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
// Waypoint navigation / helper.
bool _calcNextPhase(void);
void _recalcCurrentPhase(void);
bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
// Slicing parameters
bool _setStartIndex(void);
void _updateOverlap(void);
void _updateMaxWaypoints(void);
// Waypoint settings.
void _updateflightSpeed(void);
void _updateArrivalReturnSpeed(void);
void _updateAltitude(void);
// Waypoint Statistics.
void _setPhaseDistance(double distance);
void _setPhaseDuration(double duration);
// SMART RTL
void _checkBatteryLevel(void);
bool _checkSmartRTLPreCondition(QString &errorString);
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake.
void _DMFinishedHandler();
void _switchToSnakeWaypointManager(QVariant variant);
void _switchDataManager(SnakeDataManager &dataManager);
void _progressChangedHandler();
void _enableSnakeChangedHandler();
// Periodic tasks.
void _eventTimerHandler(void);
// Waypoint manager handling.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
private:
using StatusMap = std::map<int, QString>;
// Controllers.
PlanMasterController *_masterController;
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid;
// Waypoint Managers.
WaypointManager::AreaInterface _areaInterface;
WaypointManager::Settings _managerSettings;
WaypointManager::DefaultManager _defaultManager;
WaypointManager::DefaultManager _snakeManager;
WaypointManager::RTLManager _rtlManager;
WaypointManager::ManagerBase *_currentManager;
using ManagerList = QList<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts.
QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _enableWimaController; // enables or disables the wimaControler
SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints per phase
SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
// defining the first element of the next phase
SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not.
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _arrivalReturnSpeed; // arrival and return path speed
SettingsFact _altitude; // mission altitude
SettingsFact _enableSnake; // Enable Snake (see snake.h)
SettingsFact _snakeTileWidth;
SettingsFact _snakeTileHeight;
SettingsFact _snakeMinTileArea;
SettingsFact _snakeLineDistance;
SettingsFact _snakeMinTransectLength;
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Waypoint statistics.
double _measurementPathLength; // the lenght of the phase in meters
// Snake
SnakeDataManager _snakeDataManager;
int _fallbackStatus;
static StatusMap _nemoStatusMap;
// Periodic tasks.
QTimer _eventTimer;
EventTicker _batteryLevelTicker;
EventTicker _snakeTicker;
EventTicker _nemoTimeoutTicker;
using StatusMap = std::map<int, QString>;
// Controllers.
PlanMasterController *_masterController;
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData
_joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid;
// Waypoint Managers.
WaypointManager::AreaInterface _areaInterface;
WaypointManager::Settings _WMSettings; // Waypoint Manager Settings
WaypointManager::DefaultManager _defaultWM;
WaypointManager::DefaultManager _snakeWM;
WaypointManager::RTLManager _rtlWM;
WaypointManager::ManagerBase *_currentWM;
using ManagerList = QList<WaypointManager::ManagerBase *>;
ManagerList _WMList;
// Settings Facts.
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _enableWimaController; // enables or disables the wimaControler
SettingsFact
_overlapWaypoints; // determines the number of overlapping waypoints
// between two consecutive mission phases
SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints
// per phase
SettingsFact
_nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get
// index of item in _missionItems) of the
// mission item defining the first element
// of the next phase
SettingsFact
_showAllMissionItems; // bool value, Determines whether the mission items
// of the overall mission are displayed or not.
SettingsFact _showCurrentMissionItems; // bool value, Determines whether the
// mission items of the current mission
// phase are displayed or not.
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _arrivalReturnSpeed; // arrival and return path speed
SettingsFact _altitude; // mission altitude
SettingsFact _enableSnake; // Enable Snake (see snake.h)
SettingsFact _snakeTileWidth;
SettingsFact _snakeTileHeight;
SettingsFact _snakeMinTileArea;
SettingsFact _snakeLineDistance;
SettingsFact _snakeMinTransectLength;
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Waypoint statistics.
double _measurementPathLength; // the lenght of the phase in meters
// Snake
SnakeDataManager _snakeDM; // Snake Data Manager
SnakeDataManager _emptyDM;
SnakeDataManager *_currentDM;
static StatusMap _nemoStatusMap;
// Periodic tasks.
QTimer _eventTimer;
EventTicker _batteryLevelTicker;
};
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @ c6e6a519
......@@ -17,7 +17,7 @@ std::string messageType();
//! @brief C++ representation of nemo_msgs/Heartbeat message
class Heartbeat{
public:
Heartbeat(){}
Heartbeat() : _status(0){}
Heartbeat(int status) :_status(status){}
Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()){}
......
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