Skip to content

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

1243

parent 65679db5
Hide whitespace changes
Inline Side-by-side
Showing with 967 additions and 419 deletions
qgroundcontrol.pro
View file @ 50929c20
......@@ -28,11 +28,13 @@ QGCROOT = $$PWD
DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += SNAKE_SHOW_TIME
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
#DEFINES += ROS_BRIDGE_DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += NDEBUG
}
......@@ -438,11 +440,11 @@ HEADERS += \
src/Wima/Snake/QNemoHeartbeat.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/SnakeThread.h \
src/Wima/Snake/SnakeTile.h \
src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/Snake/SnakeDataManager.h \
src/Wima/WaypointManager/AreaInterface.h \
src/Wima/WaypointManager/DefaultManager.h \
src/Wima/WaypointManager/GenericWaypointManager.h \
......@@ -503,7 +505,7 @@ SOURCES += \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeDataManager.cc \
src/Wima/Snake/SnakeThread.cc \
src/Wima/Snake/SnakeTile.cpp \
src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \
......
src/Snake/snake.cpp
View file @ 50929c20
......@@ -22,7 +22,7 @@
using namespace operations_research;
#ifndef NDEBUG
//#define SHOW_TIME
//#define SNAKE_SHOW_TIME
#endif
namespace bg = boost::geometry;
......@@ -104,7 +104,7 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
//# Compute edges (x2-x1,y2-y1)
std::vector<BoostPoint> edges;
auto convex_hull_outer = convex_hull.outer();
const 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);
......@@ -711,13 +711,12 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
// 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() *
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>();
......@@ -725,8 +724,6 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
// 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) {
......@@ -736,17 +733,20 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
BoostLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
BoostLineString temp_transect;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI);
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)};
// to clipper
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
temp_transect[0].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[0].get<1>() * CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
temp_transect[1].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[1].get<1>() * CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
......@@ -852,22 +852,22 @@ void generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset, size_t n0,
RoutingDataModel &dataModel, Matrix<double> &graph) {
#ifdef SHOW_TIME
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
graphFromPolygon(polygonOffset, vertices, graph);
#ifdef SHOW_TIME
#ifdef SNAKE_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;
#endif
Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
toDistanceMatrix(distanceMatrix);
#ifdef SHOW_TIME
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
......@@ -934,12 +934,12 @@ bool flight_plan::route(const BoostPolygon &area,
// Offset joined area.
BoostPolygon areaOffset;
offsetPolygon(area, areaOffset, detail::offsetConstant);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
<< endl;
......@@ -995,11 +995,11 @@ bool flight_plan::route(const BoostPolygon &area,
searchParameters.set_allocated_time_limit(tMax);
// Solve the problem.
#ifdef SHOW_TIME
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment *solution = routing.SolveWithParameters(searchParameters);
#ifdef SHOW_TIME
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time routing.SolveWithParameters(): " << delta.count()
......
src/Snake/snake.h
View file @ 50929c20
......@@ -173,7 +173,7 @@ public:
const BoundingBox &measurementAreaBBox() const;
const BoostPoint &homePositon() const;
bool update() const;
bool update();
mutable string errorString;
......
src/Wima/Snake/NemoInterface.cpp
View file @ 50929c20
......@@ -213,6 +213,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
this->pRosBridge->subscribe(
"/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
UniqueLock lk(this->heartbeatMutex);
auto &heartbeatMsg = this->heartbeat;
......@@ -223,8 +224,15 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
this->nextTimeout =
std::chrono::high_resolution_clock::now() + timeoutInterval;
}
lk.unlock();
emit this->parent->statusChanged();
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
// std::cout << "/nemo/heartbeat callback time: " <<
// delta.count() << " ms"
// << std::endl;
});
// Advertise /snake/get_origin.
......@@ -289,11 +297,12 @@ void NemoInterface::Impl::loop() {
}
// Check if heartbeat timeout occured.
if (this->running && this->topicServiceSetupDone &&
this->nextTimeout != TimePoint::max()) {
if (this->nextTimeout < std::chrono::high_resolution_clock::now()) {
UniqueLock lk(this->heartbeatMutex);
if (this->running && this->topicServiceSetupDone) {
UniqueLock lk(this->heartbeatMutex);
if (this->nextTimeout != TimePoint::max() &&
this->nextTimeout < std::chrono::high_resolution_clock::now()) {
this->heartbeat.setStatus(integral(NemoStatus::Timeout));
lk.unlock();
emit this->parent->statusChanged();
}
}
......@@ -321,6 +330,13 @@ void NemoInterface::Impl::publishENUOrigin() {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
}
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface(QObject *parent)
: QObject(parent), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}
NemoInterface::~NemoInterface() {}
void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
......@@ -333,8 +349,8 @@ void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
this->pImpl->setENUOrigin(ENUOrigin);
}
NemoInterface::NemoStatus NemoInterface::status() {
NemoInterface::NemoStatus NemoInterface::status() const {
return this->pImpl->status();
}
QVector<int> NemoInterface::progress() { return this->pImpl->progress(); }
QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }
src/Wima/Snake/NemoInterface.h
View file @ 50929c20
......@@ -21,6 +21,7 @@ public:
};
explicit NemoInterface(QObject *parent = nullptr);
~NemoInterface() override;
void start();
void stop();
......@@ -28,8 +29,8 @@ public:
void setTilesENU(const SnakeTilesLocal &tilesENU);
void setENUOrigin(const QGeoCoordinate &ENUOrigin);
NemoStatus status();
QVector<int> progress();
NemoStatus status() const;
QVector<int> progress() const;
signals:
void statusChanged();
......
src/Wima/Snake/SnakeDataManager.cc → src/Wima/Snake/SnakeThread.cc
View file @ 50929c20
#include "SnakeDataManager.h"
#include "SnakeThread.h"
#include <QGeoCoordinate>
#include <QMutexLocker>
......@@ -18,20 +18,30 @@
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"
template <class Callable> class CommandRAII {
public:
CommandRAII(Callable &fun) : _fun(fun) {}
~CommandRAII() { _fun(); }
private:
Callable _fun;
};
using QVariantList = QList<QVariant>;
using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
class SnakeDataManager::Impl {
class SnakeThread::Impl {
public:
struct Input {
Input()
: tileWidth(5 * si::meter), tileHeight(5 * si::meter),
minTileArea(1 * si::meter * si::meter), scenarioChanged(true),
lineDistance(2 * si::meter), minTransectLength(1 * si::meter),
routeParametersChanged(true) {}
minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter),
minTransectLength(1 * si::meter), scenarioChanged(true),
progressChanged(true), routeParametersChanged(true) {}
QList<QGeoCoordinate> mArea;
QGeoCoordinate ENUOrigin;
......@@ -40,17 +50,23 @@ public:
Length tileWidth;
Length tileHeight;
Area minTileArea;
std::atomic_bool scenarioChanged;
Length lineDistance;
Length minTransectLength;
QNemoProgress progress;
std::atomic_bool scenarioChanged;
std::atomic_bool progressChanged;
std::atomic_bool routeParametersChanged;
mutable std::shared_timed_mutex mutex;
};
struct Output {
Output() : calcInProgress(false) {}
Output()
: calcInProgress(false), tilesUpdated(false), waypointsUpdated(false),
progressUpdated(false) {}
SnakeTiles tiles;
QVariantList tileCenterPoints;
......@@ -68,22 +84,21 @@ public:
QString errorMessage;
std::atomic_bool calcInProgress;
std::atomic_bool tilesUpdated;
std::atomic_bool waypointsUpdated;
std::atomic_bool progressUpdated;
bool tilesUpdated;
bool waypointsUpdated;
bool progressUpdated;
QNemoProgress progress;
mutable std::shared_timed_mutex mutex;
};
Impl(SnakeDataManager *p);
Impl(SnakeThread *p);
bool precondition() const;
bool doTopicServiceSetup();
// Internal data.
snake::Scenario scenario;
SnakeDataManager *parent;
SnakeThread *parent;
// Input
Input input;
......@@ -91,24 +106,14 @@ public:
Output output;
};
SnakeDataManager::Impl::Impl(SnakeDataManager *p) : parent(p) {}
bool SnakeDataManager::Impl::precondition() const { return true; }
SnakeThread::Impl::Impl(SnakeThread *p) : parent(p) {}
template <class Callable> class CommandRAII {
public:
CommandRAII(Callable &fun) : _fun(fun) {}
~CommandRAII() { _fun(); }
private:
Callable _fun;
};
SnakeDataManager::SnakeDataManager(QObject *parent)
SnakeThread::SnakeThread(QObject *parent)
: QThread(parent), pImpl(std::make_unique<Impl>(this)) {}
void SnakeDataManager::setMeasurementArea(
SnakeThread::~SnakeThread() {}
void SnakeThread::setMeasurementArea(
const QList<QGeoCoordinate> &measurementArea) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
......@@ -118,8 +123,7 @@ void SnakeDataManager::setMeasurementArea(
}
}
void SnakeDataManager::setServiceArea(
const QList<QGeoCoordinate> &serviceArea) {
void SnakeThread::setServiceArea(const QList<QGeoCoordinate> &serviceArea) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.sArea = serviceArea;
......@@ -128,7 +132,7 @@ void SnakeDataManager::setServiceArea(
}
}
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
void SnakeThread::setCorridor(const QList<QGeoCoordinate> &corridor) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.corridor = corridor;
......@@ -137,251 +141,295 @@ void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
}
}
SnakeTiles SnakeDataManager::tiles() const {
void SnakeThread::setProgress(const QVector<int> &progress) {
this->pImpl->input.progressChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.progress.progress() = progress;
}
SnakeTiles SnakeThread::tiles() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tiles;
}
QVariantList SnakeDataManager::tileCenterPoints() const {
SnakeTilesLocal SnakeThread::tilesENU() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tilesENU;
}
QGeoCoordinate SnakeThread::ENUOrigin() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.ENUOrigin;
}
QVariantList SnakeThread::tileCenterPoints() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tileCenterPoints;
}
QNemoProgress SnakeDataManager::progress() const {
QVector<int> SnakeThread::progress() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.progress;
return this->pImpl->input.progress.progress();
}
bool SnakeDataManager::calcInProgress() const {
bool SnakeThread::calcInProgress() const {
return this->pImpl->output.calcInProgress.load();
}
QString SnakeDataManager::errorMessage() const {
QString SnakeThread::errorMessage() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.errorMessage;
}
bool SnakeDataManager::success() const {
bool SnakeThread::tilesUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tilesUpdated;
}
bool SnakeThread::waypointsUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.waypointsUpdated;
}
bool SnakeThread::progressUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.errorMessage.isEmpty() ? true : false;
return this->pImpl->output.progressUpdated;
}
QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
QVector<QGeoCoordinate> SnakeThread::waypoints() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.waypoints;
}
QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
QVector<QGeoCoordinate> SnakeThread::arrivalPath() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.arrivalPath;
}
QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
QVector<QGeoCoordinate> SnakeThread::returnPath() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.returnPath;
}
Length SnakeDataManager::lineDistance() const {
Length SnakeThread::lineDistance() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->input.lineDistance;
}
void SnakeDataManager::setLineDistance(Length lineDistance) {
void SnakeThread::setLineDistance(Length lineDistance) {
this->pImpl->input.routeParametersChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.routeParametersChanged = true;
this->pImpl->input.lineDistance = lineDistance;
}
Area SnakeDataManager::minTileArea() const {
Area SnakeThread::minTileArea() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->scenario.minTileArea();
}
void SnakeDataManager::setMinTileArea(Area minTileArea) {
void SnakeThread::setMinTileArea(Area minTileArea) {
this->pImpl->input.scenarioChanged = true;
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->scenario.setMinTileArea(minTileArea);
}
Length SnakeDataManager::tileHeight() const {
Length SnakeThread::tileHeight() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->scenario.tileHeight();
}
void SnakeDataManager::setTileHeight(Length tileHeight) {
void SnakeThread::setTileHeight(Length tileHeight) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->scenario.setTileHeight(tileHeight);
}
Length SnakeDataManager::tileWidth() const {
Length SnakeThread::tileWidth() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->scenario.tileWidth();
}
void SnakeDataManager::setTileWidth(Length tileWidth) {
UniqueLock lk(this->pImpl->input.mutex);
void SnakeThread::setTileWidth(Length tileWidth) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->scenario.setTileWidth(tileWidth);
}
void SnakeDataManager::run() {
void SnakeThread::run() {
#ifndef NDEBUG
auto startTime = std::chrono::high_resolution_clock::now();
#endif
// Signal that calculation is in progress.
this->pImpl->output.calcInProgress.store(true);
emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
#ifndef NDEBUG
auto onExit = [this, &startTime] {
#else
auto onExit = [this] {
#endif
#ifndef NDEBUG
qDebug() << "SnakeDataManager::run() execution time: "
qDebug() << "SnakeThread::run() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - startTime)
.count()
<< " ms";
#endif
this->pImpl->output.calcInProgress.store(true);
this->pImpl->output.calcInProgress.store(false);
emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
};
CommandRAII<decltype(onExit)> onExitRAII(onExit);
this->pImpl->output.tilesUpdated = false;
this->pImpl->output.waypointsUpdated = false;
this->pImpl->output.progressUpdated = false;
bool tilesValid = true;
QGeoCoordinate origin;
{
// Check preconditions.
SharedLock lk(this->pImpl->input.mutex);
if (!this->pImpl->precondition())
return;
if (this->pImpl->input.mArea.size() < 3) {
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = "Measurement area invalid: size < 3.";
return;
}
if (this->pImpl->input.sArea.size() < 3) {
tilesValid = false;
} else if (this->pImpl->input.sArea.size() < 3) {
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = "Service area invalid: size < 3.";
return;
}
// Update Scenario if necessary
if (this->pImpl->input.scenarioChanged) {
tilesValid = false;
} else {
// Set Origin
QGeoCoordinate origin;
{
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.ENUOrigin = this->pImpl->input.mArea.front();
origin = this->pImpl->output.ENUOrigin;
}
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->input.mArea;
mAreaEnu.clear();
for (auto geoVertex : mArea) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
mAreaEnu.outer().push_back(p);
origin = this->pImpl->input.mArea.front();
// Update Scenario if necessary
if (this->pImpl->input.scenarioChanged) {
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->input.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->input.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->input.corridor;
corridorEnu.clear();
for (auto geoVertex : corridor) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
}
// Update parametes.
this->pImpl->scenario.setTileHeight(this->pImpl->input.tileHeight);
this->pImpl->scenario.setTileWidth(this->pImpl->input.tileWidth);
this->pImpl->scenario.setMinTileArea(this->pImpl->input.minTileArea);
if (!this->pImpl->scenario.update()) {
this->pImpl->output.errorMessage =
this->pImpl->scenario.errorString.c_str();
tilesValid = false;
}
}
// Update service area.
auto &sAreaEnu = this->pImpl->scenario.serviceArea();
auto &sArea = this->pImpl->input.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->input.corridor;
corridor.clear();
for (auto geoVertex : corridor) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
}
// Update parametes.
this->pImpl->scenario.setTileHeight(this->pImpl->input.tileHeight);
this->pImpl->scenario.setTileWidth(this->pImpl->input.tileWidth);
this->pImpl->scenario.setMinTileArea(this->pImpl->input.minTileArea);
this->pImpl->input.scenarioChanged = false;
if (!this->pImpl->scenario.update()) {
this->pImpl->output.errorMessage =
this->pImpl->scenario.errorString.c_str();
return;
}
this->pImpl->output.tilesUpdated = true;
}
}
bool waypointsValid = tilesValid;
bool progressValid = tilesValid;
snake::flight_plan::Transects transects;
snake::flight_plan::Transects transectsRouted;
snake::flight_plan::Route route;
std::vector<int> progress;
if (this->pImpl->input.scenarioChanged ||
this->pImpl->input.routeParametersChanged) {
// Sample data
SharedLock inputLock(this->pImpl->input.mutex);
SharedLock outputLock(this->pImpl->output.mutex);
// Verify progress.
size_t nTiles = this->pImpl->scenario.tiles().size();
if (this->pImpl->output.progress.progress().size() != nTiles) {
outputLock.unlock();
for (size_t i = 0; i < nTiles; ++i) {
progress.push_back(0);
bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
this->pImpl->input.routeParametersChanged ||
this->pImpl->input.progressChanged;
if (tilesValid) {
if (needWaypointUpdate) {
// Sample data
SharedLock inputLock(this->pImpl->input.mutex);
// Verify progress.
size_t nTiles = this->pImpl->scenario.tiles().size();
if (size_t(this->pImpl->input.progress.progress().size()) != nTiles) {
for (size_t i = 0; i < nTiles; ++i) {
progress.push_back(0);
}
} else {
for (size_t i = 0; i < nTiles; ++i) {
progress.push_back(this->pImpl->input.progress.progress()[i]);
}
}
this->pImpl->output.progressUpdated = true;
}
const auto &scenario = this->pImpl->scenario;
const auto lineDistance = this->pImpl->input.lineDistance;
const auto minTransectLength = this->pImpl->input.minTransectLength;
inputLock.unlock();
// Create transects.
std::string errorString;
snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree);
std::cout << "Transects angle: " << alpha << std::endl;
snake::flight_plan::Transects transects;
transects.push_back(
bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});
bool value = snake::flight_plan::transectsFromScenario(
lineDistance, minTransectLength, alpha, scenario, progress, transects,
errorString);
if (!value) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
} else if (transects.size() > 1) {
value = snake::flight_plan::route(scenario.joinedArea(), transects,
transectsRouted, route, errorString);
if (!value) {
// Copy remaining parameters and release lock.
const auto &scenario = this->pImpl->scenario;
const auto lineDistance = this->pImpl->input.lineDistance;
const auto minTransectLength = this->pImpl->input.minTransectLength;
inputLock.unlock();
// Create transects.
std::string errorString;
snake::Angle alpha(scenario.mAreaBoundingBox().angle * si::radian);
std::cout << "Transects angle: " << alpha << std::endl;
transects.push_back(
bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});
bool success = snake::flight_plan::transectsFromScenario(
lineDistance, minTransectLength, alpha, scenario, progress, transects,
errorString);
if (!success) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
waypointsValid = false;
progressValid = true;
} else if (transects.size() > 1) {
success =
snake::flight_plan::route(scenario.joinedArea(), transects,
transectsRouted, route, errorString);
if (!success) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
waypointsValid = false;
progressValid = true;
} else {
waypointsValid = true;
progressValid = true;
}
} else {
this->pImpl->output.waypointsUpdated = true;
// No transects
waypointsValid = false;
progressValid = true;
}
} else {
// No update necessary
waypointsValid = true;
progressValid = true;
}
}
UniqueLock lk(this->pImpl->output.mutex);
// Store tiles.
if (this->pImpl->output.tilesUpdated) {
this->pImpl->output.tilesUpdated = false;
if (!tilesValid) {
// Clear some output data.
this->pImpl->output.tiles.polygons().clear();
this->pImpl->output.tileCenterPoints.clear();
this->pImpl->output.tilesENU.polygons().clear();
this->pImpl->output.tileCenterPointsENU.clear();
this->pImpl->output.ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
this->pImpl->output.tilesUpdated = true;
} else if (this->pImpl->input.scenarioChanged) {
this->pImpl->input.scenarioChanged = false;
// Clear some output data.
this->pImpl->output.tiles.polygons().clear();
this->pImpl->output.tileCenterPoints.clear();
this->pImpl->output.tilesENU.polygons().clear();
this->pImpl->output.tileCenterPointsENU.clear();
this->pImpl->output.ENUOrigin = origin;
// Convert and store scenario data.
const auto &origin = this->pImpl->output.ENUOrigin;
const auto &tiles = this->pImpl->scenario.tiles();
const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
for (unsigned int i = 0; i < tiles.size(); ++i) {
......@@ -407,57 +455,89 @@ void SnakeDataManager::run() {
this->pImpl->output.tilesENU.polygons().push_back(enuTile);
this->pImpl->output.tileCenterPointsENU.push_back(enuVertex);
}
this->pImpl->output.tilesUpdated = true;
}
// Store progress.
if (this->pImpl->output.progressUpdated) {
size_t nTiles = progress.size();
this->pImpl->output.progress.progress().clear();
this->pImpl->output.progress.progress().reserve(nTiles);
for (const auto &p : progress) {
this->pImpl->output.progress.progress().push_back(p);
this->pImpl->output.progressUpdated = false;
if (!progressValid) {
this->pImpl->input.progress.progress().clear();
this->pImpl->output.progressUpdated = true;
} else if (this->pImpl->input.progressChanged) {
if (progress.size() == this->pImpl->scenario.tiles().size()) {
auto &qProgress = this->pImpl->input.progress;
qProgress.progress().clear();
for (const auto &p : progress) {
qProgress.progress().push_back(p);
}
}
this->pImpl->output.progressUpdated = true;
}
// Store waypoints.
if (this->pImpl->output.waypointsUpdated) {
// Store arrival path.
const auto &origin = this->pImpl->output.ENUOrigin;
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->output.arrivalPathENU.push_back(enuVertex);
this->pImpl->output.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;
if (!waypointsValid) {
// Clear waypoints.
this->pImpl->output.arrivalPath.clear();
this->pImpl->output.returnPath.clear();
this->pImpl->output.arrivalPathENU.clear();
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
this->pImpl->output.waypointsUpdated = true;
} else if (needWaypointUpdate) {
// Clear waypoints.
this->pImpl->output.arrivalPath.clear();
this->pImpl->output.returnPath.clear();
this->pImpl->output.arrivalPathENU.clear();
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
// // 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->output.arrivalPathENU.push_back(enuVertex);
// this->pImpl->output.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->output.returnPathENU.push_back(enuVertex);
// this->pImpl->output.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->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
for (const auto &t : transects) {
for (const auto &v : t) {
QPointF enuVertex{v.get<0>(), v.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, v, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.returnPathENU.push_back(enuVertex);
this->pImpl->output.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->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
}
this->pImpl->output.waypointsUpdated = true;
}
}
src/Wima/Snake/SnakeDataManager.h → src/Wima/Snake/SnakeThread.h
View file @ 50929c20
......@@ -15,29 +15,34 @@ using namespace boost::units;
using Length = quantity<si::length>;
using Area = quantity<si::area>;
class SnakeDataManager : public QThread {
class SnakeThread : public QThread {
Q_OBJECT
class Impl;
using PImpl = std::unique_ptr<Impl>;
public:
SnakeDataManager(QObject *parent = nullptr);
~SnakeDataManager() override;
SnakeThread(QObject *parent = nullptr);
~SnakeThread() override;
void setMeasurementArea(const QList<QGeoCoordinate> &measurementArea);
void setServiceArea(const QList<QGeoCoordinate> &serviceArea);
void setCorridor(const QList<QGeoCoordinate> &corridor);
void setProgress(const QVector<int> &progress);
SnakeTiles tiles() const;
SnakeTilesLocal tilesENU() const;
QGeoCoordinate ENUOrigin() const;
QVariantList tileCenterPoints() const;
QNemoProgress progress() const;
QVector<int> progress() const;
bool calcInProgress() const;
QString errorMessage() const;
bool success() const;
bool tilesUpdated();
bool waypointsUpdated();
bool progressUpdated();
QVector<QGeoCoordinate> waypoints() const;
QVector<QGeoCoordinate> arrivalPath() const;
QVector<QGeoCoordinate> returnPath() const;
......
src/Wima/Snake/SnakeTile.cpp
View file @ 50929c20
#include "SnakeTile.h"
SnakeTile::SnakeTile() : WimaAreaData()
{
SnakeTile::SnakeTile() : WimaAreaData() {}
SnakeTile::SnakeTile(const SnakeTile &other, QObject *parent)
: WimaAreaData(parent) {
*this = other;
}
SnakeTile::SnakeTile(const SnakeTile &other) : WimaAreaData()
{
*this = other;
SnakeTile &SnakeTile::operator=(const SnakeTile &other) {
this->assign(other);
return *this;
}
SnakeTile &SnakeTile::operator=(const SnakeTile &other)
{
this->assign(other);
return *this;
}
void SnakeTile::assign(const SnakeTile &other)
{
WimaAreaData::assign(other);
}
void SnakeTile::assign(const SnakeTile &other) { WimaAreaData::assign(other); }
src/Wima/Snake/SnakeTile.h
View file @ 50929c20
......@@ -2,18 +2,16 @@
#include "Wima/Geometry/WimaAreaData.h"
class SnakeTile : public WimaAreaData
{
class SnakeTile : public WimaAreaData {
public:
SnakeTile();
SnakeTile(const SnakeTile&other);
SnakeTile();
SnakeTile(const SnakeTile &other, QObject *parent = nullptr);
QString type() const {return "Tile";}
SnakeTile* Clone() const {return new SnakeTile(*this);}
QString type() const { return "Tile"; }
SnakeTile *Clone() const { return new SnakeTile(*this); }
SnakeTile& operator=(const SnakeTile &other);
SnakeTile &operator=(const SnakeTile &other);
protected:
void assign(const SnakeTile &other);
void assign(const SnakeTile &other);
};
src/Wima/WimaController.cc
View file @ 50929c20
......@@ -11,6 +11,11 @@
#include <memory>
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
......@@ -73,10 +78,11 @@ WimaController::WimaController(QObject *parent)
_snakeMinTransectLength(settingsGroup,
_metaDataMap[snakeMinTransectLengthName]),
_lowBatteryHandlingTriggered(false), _measurementPathLength(-1),
_currentDM(&_emptyDM),
_snakeThread(this), _emptyThread(this), _currentThread(&_emptyThread),
_nemoInterface(this),
_batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
// Set up facts.
// Set up facts for waypoint manager.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
......@@ -112,21 +118,23 @@ WimaController::WimaController(QObject *parent)
&WimaController::_eventTimerHandler);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// Snake Data Manager.
connect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
// SnakeThread.
connect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
connect(this, &QObject::destroyed, &this->_snakeThread, &SnakeThread::quit);
connect(this, &QObject::destroyed, &this->_emptyThread, &SnakeThread::quit);
// NemoInterface.
connect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
connect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
connect(this, &QObject::destroyed, &this->_snakeDM, &SnakeDataManager::quit);
connect(this, &QObject::destroyed, &this->_emptyDM, &SnakeDataManager::quit);
// Enable/disable snake.
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_enableSnakeChangedHandler);
_enableSnakeChangedHandler();
......@@ -185,16 +193,14 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() {
return const_cast<QmlObjectListModel *>(this->_currentDM->tiles());
}
QmlObjectListModel *WimaController::snakeTiles() { return &this->tiles; }
QVariantList WimaController::snakeTileCenterPoints() {
return _currentDM->tileCenterPoints();
return _currentThread->tileCenterPoints();
}
QVector<int> WimaController::nemoProgress() {
return _currentDM->progress().progress();
return _currentThread->progress();
}
double WimaController::phaseDistance() const {
......@@ -207,14 +213,16 @@ double WimaController::phaseDuration() const {
return 0.0;
}
int WimaController::nemoStatus() const { return _currentDM->nemoStatus(); }
int WimaController::nemoStatus() const {
return integral(_nemoInterface.status());
}
QString WimaController::nemoStatusString() const {
return _nemoStatusMap.at(nemoStatus());
}
bool WimaController::snakeCalcInProgress() const {
return _currentDM->calcInProgress();
return _currentThread->calcInProgress();
}
void WimaController::setMasterController(PlanMasterController *masterC) {
......@@ -432,10 +440,10 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
emit currentWaypointPathChanged();
// Update Snake Data Manager
_snakeDM.setMeasurementArea(_measurementArea.coordinateList());
_snakeDM.setServiceArea(_serviceArea.coordinateList());
_snakeDM.setCorridor(_corridor.coordinateList());
_currentDM->start();
_snakeThread.setMeasurementArea(_measurementArea.coordinateList());
_snakeThread.setServiceArea(_serviceArea.coordinateList());
_snakeThread.setCorridor(_corridor.coordinateList());
_currentThread->start();
_localPlanDataValid = true;
return true;
......@@ -710,33 +718,47 @@ void WimaController::_initSmartRTL() {
}
}
void WimaController::_DMFinishedHandler() {
if (!_snakeDM.success()) {
qDebug() << _snakeDM.errorMessage();
// qgcApp()->showMessage(_snakeDM.errorMessage());
return;
void WimaController::_threadFinishedHandler() {
if (!_snakeThread.errorMessage().isEmpty()) {
qDebug() << _snakeThread.errorMessage();
}
// Copy waypoints to waypoint manager.
_snakeWM.clear();
auto waypoints = _snakeDM.waypoints();
if (waypoints.size() < 1) {
return;
if (_snakeThread.tilesUpdated()) {
this->tiles.clearAndDeleteContents();
auto tls = _snakeThread.tiles().polygons();
for (const auto &t : tls) {
this->tiles.append(new SnakeTile(t, &tiles));
}
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
_nemoInterface.setTilesENU(_snakeThread.tilesENU());
_nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
}
for (auto &vertex : waypoints) {
_snakeWM.push_back(vertex);
if (_snakeThread.progressUpdated()) {
emit nemoProgressChanged();
}
// Do update.
this->_snakeWM.update(); // this can take a while (ca. 200ms)
if (_snakeThread.waypointsUpdated()) {
// Copy waypoints to waypoint manager.
_snakeWM.clear();
auto waypoints = _snakeThread.waypoints();
if (waypoints.size() < 1) {
return;
}
for (auto &vertex : waypoints) {
_snakeWM.push_back(vertex);
}
// Do update.
this->_snakeWM.update(); // this can take a while (ca. 200ms)
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
emit nemoProgressChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
}
void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
......@@ -747,32 +769,35 @@ void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
}
}
void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
if (_currentDM != &dataManager) {
disconnect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
disconnect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
void WimaController::_switchThreadObject(SnakeThread &thread) {
if (_currentThread != &thread) {
// SnakeThread.
disconnect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
disconnect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
// NemoInterface.
disconnect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
disconnect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
_currentDM = &dataManager;
_currentThread = &thread;
connect(_currentDM, &SnakeDataManager::finished, this,
&WimaController::_DMFinishedHandler);
connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
// SnakeThread.
connect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
// NemoInterface.
connect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
connect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
emit snakeCalcInProgressChanged();
emit snakeTilesChanged();
......@@ -783,17 +808,21 @@ void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
}
}
void WimaController::_progressChangedHandler() { _snakeDM.start(); }
void WimaController::_progressChangedHandler() {
this->_snakeThread.setProgress(this->_nemoInterface.progress());
this->_snakeThread.start();
}
void WimaController::_enableSnakeChangedHandler() {
if (this->_enableSnake.rawValue().toBool()) {
qDebug() << "WimaController: enabling snake.";
_switchDataManager(this->_snakeDM);
this->_snakeDM.enableRosBridge();
_currentDM->start();
_switchThreadObject(this->_snakeThread);
this->_nemoInterface.start();
this->_snakeThread.start();
} else {
qDebug() << "WimaController: disabling snake.";
this->_snakeDM.disableRosBride();
_switchDataManager(_emptyDM);
this->_nemoInterface.stop();
this->_snakeThread.quit();
_switchThreadObject(_emptyThread);
}
}
src/Wima/WimaController.h
View file @ 50929c20
......@@ -27,7 +27,8 @@
#include "SurveyComplexItem.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/NemoInterface.h"
#include "Snake/SnakeThread.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
......@@ -237,19 +238,16 @@ private slots:
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake.
void _DMFinishedHandler();
void _threadFinishedHandler();
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
void _switchToSnakeWaypointManager(QVariant variant);
void _switchDataManager(SnakeDataManager &dataManager);
void _switchThreadObject(SnakeThread &thread);
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;
......@@ -310,9 +308,12 @@ private:
double _measurementPathLength; // the lenght of the phase in meters
// Snake
SnakeDataManager _snakeDM; // Snake Data Manager
SnakeDataManager _emptyDM;
SnakeDataManager *_currentDM;
QmlObjectListModel tiles;
SnakeThread _snakeThread; // Snake Data Manager
SnakeThread _emptyThread;
SnakeThread *_currentThread;
NemoInterface _nemoInterface;
using StatusMap = std::map<int, QString>;
static StatusMap _nemoStatusMap;
// Periodic tasks.
......
src/comm/ros_bridge/include/com_private.h
View file @ 50929c20
......@@ -2,8 +2,8 @@
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <memory>
#include <deque>
#include <memory>
#include <unordered_map>
namespace ros_bridge {
......@@ -16,11 +16,6 @@ typedef std::size_t HashType;
using ClientMap = std::unordered_map<HashType, std::string>;
static const char* _topicAdvertiserKey = "topic_advertiser";
static const char* _topicPublisherKey = "topic_publisher";
static const char* _serviceAdvertiserKey = "service_advertiser";
static const char* _topicSubscriberKey = "topic_subscriber";
std::size_t getHash(const std::string &str);
std::size_t getHash(const char *str);
......@@ -30,5 +25,5 @@ bool getType(const JsonDoc &doc, std::string &type);
bool removeTopic(JsonDoc &doc);
bool removeType(JsonDoc &doc);
}
}
} // namespace com_private
} // namespace ros_bridge
src/comm/ros_bridge/include/server.cpp
View file @ 50929c20
......@@ -2,98 +2,87 @@
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) :
_rbc(rbc)
, _stopped(std::make_shared<std::atomic_bool>(true))
{
static const char *serviceAdvertiserKey = "service_advertiser";
}
ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc)
: _rbc(rbc), _stopped(std::make_shared<std::atomic_bool>(true)) {}
void ros_bridge::com_private::Server::advertiseService(const std::string &service,
const std::string &type,
const Server::CallbackType &userCallback)
{
std::string clientName = _serviceAdvertiserKey + service;
auto it = _serviceMap.find(clientName);
if (it != _serviceMap.end())
return; // Service allready advertised.
_serviceMap.insert(std::make_pair(clientName, service));
_rbc.addClient(clientName);
void ros_bridge::com_private::Server::advertiseService(
const std::string &service, const std::string &type,
const Server::CallbackType &userCallback) {
std::string clientName = serviceAdvertiserKey + service;
auto it = _serviceMap.find(clientName);
if (it != _serviceMap.end())
return; // Service allready advertised.
_serviceMap.insert(std::make_pair(clientName, service));
_rbc.addClient(clientName);
auto rbc = &_rbc;
_rbc.advertiseService(clientName, service, type,
[userCallback, rbc](
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message){
auto rbc = &_rbc;
_rbc.advertiseService(
clientName, service, type,
[userCallback, rbc](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
// message->string() is destructive, so we have to buffer it first
std::string messagebuf = in_message->string();
//std::cout << "advertiseServiceCallback(): Message Received: "
// << messagebuf << std::endl;
// std::cout << "advertiseServiceCallback(): Message Received: "
// << messagebuf << std::endl;
rapidjson::Document document;
if (document.Parse(messagebuf.c_str()).HasParseError())
{
std::cerr << "advertiseServiceCallback(): Error in parsing service request message: "
if (document.Parse(messagebuf.c_str()).HasParseError()) {
std::cerr << "advertiseServiceCallback(): Error in parsing service "
"request message: "
<< messagebuf << std::endl;
return;
}
if ( !document.HasMember("args") || !document["args"].IsObject()){
std::cerr << "advertiseServiceCallback(): message has no args member: "
<< messagebuf << std::endl;
return;
if (!document.HasMember("args") || !document["args"].IsObject()) {
std::cerr
<< "advertiseServiceCallback(): message has no args member: "
<< messagebuf << std::endl;
return;
}
if ( !document.HasMember("service") || !document["service"].IsString()){
std::cerr << "advertiseServiceCallback(): message has no service member: "
<< messagebuf << std::endl;
return;
if (!document.HasMember("service") || !document["service"].IsString()) {
std::cerr
<< "advertiseServiceCallback(): message has no service member: "
<< messagebuf << std::endl;
return;
}
if ( !document.HasMember("id") || !document["id"].IsString()){
std::cerr << "advertiseServiceCallback(): message has no id member: "
<< messagebuf << std::endl;
return;
if (!document.HasMember("id") || !document["id"].IsString()) {
std::cerr << "advertiseServiceCallback(): message has no id member: "
<< messagebuf << std::endl;
return;
}
JsonDocUPtr pDoc(new JsonDoc());
std::cout << "args member count: " << document["args"].MemberCount();
if ( document["args"].MemberCount() > 0)
pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
if (document["args"].MemberCount() > 0)
pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
JsonDocUPtr pDocResponse = userCallback(std::move(pDoc));
rbc->serviceResponse(
document["service"].GetString(),
document["id"].GetString(),
pDocResponse->MemberCount() > 0 ? true : false,
*pDocResponse);
document["service"].GetString(), document["id"].GetString(),
pDocResponse->MemberCount() > 0 ? true : false, *pDocResponse);
return;
});
});
}
ros_bridge::com_private::Server::~Server()
{
this->reset();
}
ros_bridge::com_private::Server::~Server() { this->reset(); }
void ros_bridge::com_private::Server::start()
{
_stopped->store(false);
}
void ros_bridge::com_private::Server::start() { _stopped->store(false); }
void ros_bridge::com_private::Server::reset()
{
if ( _stopped->load() )
return;
std::cout << "Server: _stopped->store(true)" << std::endl;
_stopped->store(true);
for (const auto& item : _serviceMap){
std::cout << "Server: unadvertiseService " << item.second << std::endl;
_rbc.unadvertiseService(item.second);
std::cout << "Server: removeClient " << item.first << std::endl;
_rbc.removeClient(item.first);
}
std::cout << "Server: _serviceMap cleared " << std::endl;
_serviceMap.clear();
void ros_bridge::com_private::Server::reset() {
if (_stopped->load())
return;
std::cout << "Server: _stopped->store(true)" << std::endl;
_stopped->store(true);
for (const auto &item : _serviceMap) {
std::cout << "Server: unadvertiseService " << item.second << std::endl;
_rbc.unadvertiseService(item.second);
std::cout << "Server: removeClient " << item.first << std::endl;
_rbc.removeClient(item.first);
}
std::cout << "Server: _serviceMap cleared " << std::endl;
_serviceMap.clear();
}
src/comm/ros_bridge/include/topic_publisher.cpp
View file @ 50929c20
#include "topic_publisher.h"
#include <unordered_map>
ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) :
_stopped(std::make_shared<std::atomic_bool>(true))
, _rbc(rbc)
{
}
static const char *topicAdvertiserKey = "topic_advertiser";
ros_bridge::com_private::TopicPublisher::~TopicPublisher()
{
this->reset();
}
#include <unordered_map>
void ros_bridge::com_private::TopicPublisher::start()
{
if ( !_stopped->load() ) // start called while thread running.
return;
_stopped->store(false);
_pThread = std::make_unique<std::thread>([this]{
// Main Loop.
while( !this->_stopped->load() ){
std::unique_lock<std::mutex> lk(this->_mutex);
// Check if new data available, wait if not.
if (this->_queue.empty()){
if ( _stopped->load() ) // Check condition again while holding the lock.
break;
this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
continue;
}
// Pop message from queue.
JsonDocUPtr pJsonDoc = std::move(this->_queue.front());
this->_queue.pop_front();
lk.unlock();
// Get topic and type from Json message and remove it.
std::string topic;
bool ret = com_private::getTopic(*pJsonDoc, topic);
assert(ret);
(void)ret;
// Debug
rapidjson::StringBuffer sb;
rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
pJsonDoc->Accept(writer);
std::cout << "message: " << sb.GetString() << std::endl;
std::cout << "topic: " << topic << std::endl;
ret = com_private::removeTopic(*pJsonDoc);
assert(ret);
(void)ret;
// Wait for topic (Rosbridge needs some time to process a advertise() request).
this->_rbc.waitForTopic(topic, [this]{
return this->_stopped->load();
});
// Publish Json message.
if ( !this->_stopped->load() )
this->_rbc.publish(topic, *pJsonDoc);
} // while loop
ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc)
: _stopped(std::make_shared<std::atomic_bool>(true)), _rbc(rbc) {}
ros_bridge::com_private::TopicPublisher::~TopicPublisher() { this->reset(); }
void ros_bridge::com_private::TopicPublisher::start() {
if (!_stopped->load()) // start called while thread running.
return;
_stopped->store(false);
_pThread = std::make_unique<std::thread>([this] {
// Main Loop.
while (!this->_stopped->load()) {
std::unique_lock<std::mutex> lk(this->_mutex);
// Check if new data available, wait if not.
if (this->_queue.empty()) {
if (_stopped->load()) // Check condition again while holding the lock.
break;
this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter
// in this case.
continue;
}
// Pop message from queue.
JsonDocUPtr pJsonDoc = std::move(this->_queue.front());
this->_queue.pop_front();
lk.unlock();
// Get topic and type from Json message and remove it.
std::string topic;
bool ret = com_private::getTopic(*pJsonDoc, topic);
assert(ret);
(void)ret;
// Debug
rapidjson::StringBuffer sb;
rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
pJsonDoc->Accept(writer);
std::cout << "message: " << sb.GetString() << std::endl;
std::cout << "topic: " << topic << std::endl;
ret = com_private::removeTopic(*pJsonDoc);
assert(ret);
(void)ret;
// Wait for topic (Rosbridge needs some time to process a advertise()
// request).
this->_rbc.waitForTopic(topic, [this] { return this->_stopped->load(); });
// Publish Json message.
if (!this->_stopped->load())
this->_rbc.publish(topic, *pJsonDoc);
} // while loop
#ifdef ROS_BRIDGE_DEBUG
std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
#endif
});
});
}
void ros_bridge::com_private::TopicPublisher::reset()
{
if ( _stopped->load() ) // stop called while thread not running.
return;
std::unique_lock<std::mutex> lk(_mutex);
_stopped->store(true);
_cv.notify_one(); // Wake publisher thread.
lk.unlock();
if ( !_pThread )
return;
_pThread->join();
lk.lock();
// Tidy up.
for (auto& it : this->_topicMap){
this->_rbc.unadvertise(it.first);
this->_rbc.removeClient(it.second);
}
this->_topicMap.clear();
_queue.clear();
void ros_bridge::com_private::TopicPublisher::reset() {
if (_stopped->load()) // stop called while thread not running.
return;
std::unique_lock<std::mutex> lk(_mutex);
_stopped->store(true);
_cv.notify_one(); // Wake publisher thread.
lk.unlock();
if (!_pThread)
return;
_pThread->join();
lk.lock();
// Tidy up.
for (auto &it : this->_topicMap) {
this->_rbc.unadvertise(it.first);
this->_rbc.removeClient(it.second);
}
this->_topicMap.clear();
_queue.clear();
}
void ros_bridge::com_private::TopicPublisher::publish(
ros_bridge::com_private::JsonDocUPtr docPtr,
const char *topic)
{
// Check if topic was advertised.
std::string t(topic);
std::unique_lock<std::mutex> lk(this->_mutex);
auto it = this->_topicMap.find(t);
if ( it == this->_topicMap.end()) { // Not yet advertised?
lk.unlock();
ros_bridge::com_private::JsonDocUPtr docPtr, const char *topic) {
// Check if topic was advertised.
std::string t(topic);
std::unique_lock<std::mutex> lk(this->_mutex);
auto it = this->_topicMap.find(t);
if (it == this->_topicMap.end()) { // Not yet advertised?
lk.unlock();
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "TopicPublisher: topic "
<< t << " not yet advertised" << std::endl;
std::cerr << "TopicPublisher: topic " << t << " not yet advertised"
<< std::endl;
#endif
return;
}
lk.unlock();
// Add topic information to json doc.
auto &allocator = docPtr->GetAllocator();
rapidjson::Value key("topic", allocator);
rapidjson::Value value(topic, allocator);
docPtr->AddMember(key, value, allocator);
lk.lock();
_queue.push_back(std::move(docPtr));
lk.unlock();
_cv.notify_one(); // Wake publisher thread.
return;
}
lk.unlock();
// Add topic information to json doc.
auto &allocator = docPtr->GetAllocator();
rapidjson::Value key("topic", allocator);
rapidjson::Value value(topic, allocator);
docPtr->AddMember(key, value, allocator);
lk.lock();
_queue.push_back(std::move(docPtr));
lk.unlock();
_cv.notify_one(); // Wake publisher thread.
}
bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
{
std::unique_lock<std::mutex> lk(this->_mutex);
std::string t(topic);
auto it = this->_topicMap.find(t);
if ( it == this->_topicMap.end()) { // Need to advertise topic?
std::string clientName =
std::string(ros_bridge::com_private::_topicAdvertiserKey)
+ t;
this->_topicMap.insert(std::make_pair(t, clientName));
lk.unlock();
this->_rbc.addClient(clientName);
this->_rbc.advertise(clientName, t, type);
return true;
} else {
lk.unlock();
bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic,
const char *type) {
std::unique_lock<std::mutex> lk(this->_mutex);
std::string t(topic);
auto it = this->_topicMap.find(t);
if (it == this->_topicMap.end()) { // Need to advertise topic?
std::string clientName = std::string(topicAdvertiserKey) + t;
this->_topicMap.insert(std::make_pair(t, clientName));
lk.unlock();
this->_rbc.addClient(clientName);
this->_rbc.advertise(clientName, t, type);
return true;
} else {
lk.unlock();
#if ROS_BRIDGE_DEBUG
std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
std::cerr << "TopicPublisher: topic " << topic << " already advertised"
<< std::endl;
#endif
return false;
}
return false;
}
}
src/comm/ros_bridge/include/topic_subscriber.cpp
View file @ 50929c20
......@@ -2,98 +2,86 @@
#include <thread>
ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) :
_rbc(rbc)
, _stopped(std::make_shared<std::atomic_bool>(true))
{
static const char *topicSubscriberKey = "topic_subscriber";
}
ros_bridge::com_private::TopicSubscriber::TopicSubscriber(
RosbridgeWsClient &rbc)
: _rbc(rbc), _stopped(std::make_shared<std::atomic_bool>(true)) {}
ros_bridge::com_private::TopicSubscriber::~TopicSubscriber()
{
this->reset();
}
ros_bridge::com_private::TopicSubscriber::~TopicSubscriber() { this->reset(); }
void ros_bridge::com_private::TopicSubscriber::start()
{
_stopped->store(false);
void ros_bridge::com_private::TopicSubscriber::start() {
_stopped->store(false);
}
void ros_bridge::com_private::TopicSubscriber::reset()
{
if ( !_stopped->load() ){
_stopped->store(true);
{
for (auto &item : _topicMap){
_rbc.unsubscribe(item.second);
_rbc.removeClient(item.first);
}
}
_topicMap.clear();
void ros_bridge::com_private::TopicSubscriber::reset() {
if (!_stopped->load()) {
_stopped->store(true);
{
for (auto &item : _topicMap) {
_rbc.unsubscribe(item.second);
_rbc.removeClient(item.first);
}
}
_topicMap.clear();
}
}
void ros_bridge::com_private::TopicSubscriber::subscribe(
const char *topic,
const std::function<void(JsonDocUPtr)> &callback)
{
if ( _stopped->load() )
return;
const char *topic, const std::function<void(JsonDocUPtr)> &callback) {
if (_stopped->load())
return;
std::string clientName = ros_bridge::com_private::_topicSubscriberKey
+ std::string(topic);
auto it = _topicMap.find(clientName);
if ( it != _topicMap.end() ){ // Topic already subscribed?
return;
}
_topicMap.insert(std::make_pair(clientName, std::string(topic)));
std::string clientName = topicSubscriberKey + std::string(topic);
auto it = _topicMap.find(clientName);
if (it != _topicMap.end()) { // Topic already subscribed?
return;
}
_topicMap.insert(std::make_pair(clientName, std::string(topic)));
// Wrap callback.
auto callbackWrapper = [callback](
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
// Wrap callback.
auto callbackWrapper =
[callback](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
// Parse document.
JsonDoc docFull;
docFull.Parse(in_message->string().c_str());
if ( docFull.HasParseError() ) {
std::cout << "Json document has parse error: "
<< in_message->string()
<< std::endl;
return;
if (docFull.HasParseError()) {
std::cout << "Json document has parse error: " << in_message->string()
<< std::endl;
return;
} else if (!docFull.HasMember("msg")) {
std::cout << "Json document does not contain a message (\"msg\"): "
<< in_message->string()
<< std::endl;
return;
std::cout << "Json document does not contain a message (\"msg\"): "
<< in_message->string() << std::endl;
return;
}
// Extract message and call callback.
JsonDocUPtr pDoc(new JsonDoc());
pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
callback(std::move(pDoc));
};
};
if ( !_rbc.topicAvailable(topic) ){
// Wait until topic is available.
std::cout << "TopicSubscriber: Starting wait thread, " << clientName << std::endl;
std::thread t([this, clientName, topic, callbackWrapper]{
this->_rbc.waitForTopic(topic, [this]{
return this->_stopped->load();
});
if ( !this->_stopped->load() ){
this->_rbc.addClient(clientName);
this->_rbc.subscribe(clientName, topic, callbackWrapper);
std::cout << "TopicSubscriber: wait thread subscription successfull: " << clientName << std::endl;
}
std::cout << "TopicSubscriber: wait thread end, " << clientName << std::endl;
});
t.detach();
} else {
_rbc.addClient(clientName);
_rbc.subscribe(clientName, topic, callbackWrapper);
std::cout << "TopicSubscriber: subscription successfull: " << clientName << std::endl;
}
if (!_rbc.topicAvailable(topic)) {
// Wait until topic is available.
std::cout << "TopicSubscriber: Starting wait thread, " << clientName
<< std::endl;
std::thread t([this, clientName, topic, callbackWrapper] {
this->_rbc.waitForTopic(topic, [this] { return this->_stopped->load(); });
if (!this->_stopped->load()) {
this->_rbc.addClient(clientName);
this->_rbc.subscribe(clientName, topic, callbackWrapper);
std::cout << "TopicSubscriber: wait thread subscription successfull: "
<< clientName << std::endl;
}
std::cout << "TopicSubscriber: wait thread end, " << clientName
<< std::endl;
});
t.detach();
} else {
_rbc.addClient(clientName);
_rbc.subscribe(clientName, topic, callbackWrapper);
std::cout << "TopicSubscriber: subscription successfull: " << clientName
<< std::endl;
}
}
src/ui/MainWindowInner.qml
View file @ 50929c20
......@@ -29,6 +29,7 @@ Item {
QGCPalette { id: qgcPal; colorGroupEnabled: true }
property real fps: 0.0
property var currentPopUp: null
property real currentCenterX: 0
property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
......
src/ui/MainWindowNative.qml
View file @ 50929c20
......@@ -39,6 +39,7 @@ Window {
Loader {
id: mainWindowInner
anchors.fill: parent
anchors.margins: 10
source: "MainWindowInner.qml"
Connections {
......
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