From 50929c2040d90db12913bb6cd3f090d245b52b53 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 14 Sep 2020 22:05:35 +0200 Subject: [PATCH] 1243 --- qgroundcontrol.pro | 6 +- src/Snake/snake.cpp | 48 +- src/Snake/snake.h | 2 +- src/Wima/Snake/NemoInterface.cpp | 28 +- src/Wima/Snake/NemoInterface.h | 5 +- src/Wima/Snake/SnakeDataManager.cc | 463 --------------- src/Wima/Snake/SnakeThread.cc | 543 ++++++++++++++++++ .../{SnakeDataManager.h => SnakeThread.h} | 13 +- src/Wima/Snake/SnakeTile.cpp | 24 +- src/Wima/Snake/SnakeTile.h | 16 +- src/Wima/WimaController.cc | 167 +++--- src/Wima/WimaController.h | 21 +- src/comm/ros_bridge/include/com_private.h | 11 +- src/comm/ros_bridge/include/server.cpp | 123 ++-- .../ros_bridge/include/topic_publisher.cpp | 245 ++++---- .../ros_bridge/include/topic_subscriber.cpp | 132 ++--- src/ui/MainWindowInner.qml | 1 + src/ui/MainWindowNative.qml | 1 + 18 files changed, 967 insertions(+), 882 deletions(-) delete mode 100644 src/Wima/Snake/SnakeDataManager.cc create mode 100644 src/Wima/Snake/SnakeThread.cc rename src/Wima/Snake/{SnakeDataManager.h => SnakeThread.h} (84%) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d9f799cc1..004538b50 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 7bab2fb03..37280bac5 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -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 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 rotate(-angle.value() * + trans::rotate_transformer 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 rotate_back( - angle.value() * 180 / M_PI); vector 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 rotate_back( + -angle.value() * 180 / M_PI); bg::transform(transect, temp_transect, rotate_back); - - ClipperLib::IntPoint c1{ - static_cast(transect[0].get<0>() * CLIPPER_SCALE), - static_cast(transect[0].get<1>() * CLIPPER_SCALE)}; - ClipperLib::IntPoint c2{ - static_cast(transect[1].get<0>() * CLIPPER_SCALE), - static_cast(transect[1].get<1>() * CLIPPER_SCALE)}; + // to clipper + ClipperLib::IntPoint c1{static_cast( + temp_transect[0].get<0>() * CLIPPER_SCALE), + static_cast( + temp_transect[0].get<1>() * CLIPPER_SCALE)}; + ClipperLib::IntPoint c2{static_cast( + temp_transect[1].get<0>() * CLIPPER_SCALE), + static_cast( + 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 &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::high_resolution_clock::now() - start); cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; #endif Matrix 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::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( +#ifdef SNAKE_SHOW_TIME + auto delta = std::chrono::duration_cast( 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::high_resolution_clock::now() - start); cout << "Execution time routing.SolveWithParameters(): " << delta.count() diff --git a/src/Snake/snake.h b/src/Snake/snake.h index 145fffdd1..fab52f5f5 100644 --- a/src/Snake/snake.h +++ b/src/Snake/snake.h @@ -173,7 +173,7 @@ public: const BoundingBox &measurementAreaBBox() const; const BoostPoint &homePositon() const; - bool update() const; + bool update(); mutable string errorString; diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index f7f1a3344..2b0a08e47 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/src/Wima/Snake/NemoInterface.cpp @@ -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::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(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 NemoInterface::progress() { return this->pImpl->progress(); } +QVector NemoInterface::progress() const { return this->pImpl->progress(); } diff --git a/src/Wima/Snake/NemoInterface.h b/src/Wima/Snake/NemoInterface.h index bcf44f8ca..b5c59db4d 100644 --- a/src/Wima/Snake/NemoInterface.h +++ b/src/Wima/Snake/NemoInterface.h @@ -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 progress(); + NemoStatus status() const; + QVector progress() const; signals: void statusChanged(); diff --git a/src/Wima/Snake/SnakeDataManager.cc b/src/Wima/Snake/SnakeDataManager.cc deleted file mode 100644 index b01b9a222..000000000 --- a/src/Wima/Snake/SnakeDataManager.cc +++ /dev/null @@ -1,463 +0,0 @@ -#include "SnakeDataManager.h" - -#include -#include - -#include "QGCApplication.h" -#include "QGCToolbox.h" -#include "SettingsFact.h" -#include "SettingsManager.h" -#include "WimaSettings.h" - -#include -#include -#include -#include - -#include "Wima/Snake/SnakeTiles.h" -#include "Wima/Snake/SnakeTilesLocal.h" -#include "snake.h" - -using QVariantList = QList; - -using UniqueLock = std::unique_lock; -using SharedLock = std::shared_lock; - -class SnakeDataManager::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) {} - - QList mArea; - QGeoCoordinate ENUOrigin; - QList sArea; - QList corridor; - Length tileWidth; - Length tileHeight; - Area minTileArea; - std::atomic_bool scenarioChanged; - - Length lineDistance; - Length minTransectLength; - std::atomic_bool routeParametersChanged; - - mutable std::shared_timed_mutex mutex; - }; - - struct Output { - Output() : calcInProgress(false) {} - - SnakeTiles tiles; - QVariantList tileCenterPoints; - SnakeTilesLocal tilesENU; - QVector tileCenterPointsENU; - QGeoCoordinate ENUOrigin; - - QVector waypoints; - QVector arrivalPath; - QVector returnPath; - QVector waypointsENU; - QVector arrivalPathENU; - QVector returnPathENU; - - QString errorMessage; - - std::atomic_bool calcInProgress; - std::atomic_bool tilesUpdated; - std::atomic_bool waypointsUpdated; - std::atomic_bool progressUpdated; - - QNemoProgress progress; - mutable std::shared_timed_mutex mutex; - }; - - Impl(SnakeDataManager *p); - - bool precondition() const; - bool doTopicServiceSetup(); - - // Internal data. - snake::Scenario scenario; - SnakeDataManager *parent; - - // Input - Input input; - // Output - Output output; -}; - -SnakeDataManager::Impl::Impl(SnakeDataManager *p) : parent(p) {} - -bool SnakeDataManager::Impl::precondition() const { return true; } - -template class CommandRAII { -public: - CommandRAII(Callable &fun) : _fun(fun) {} - - ~CommandRAII() { _fun(); } - -private: - Callable _fun; -}; - -SnakeDataManager::SnakeDataManager(QObject *parent) - : QThread(parent), pImpl(std::make_unique(this)) {} - -void SnakeDataManager::setMeasurementArea( - const QList &measurementArea) { - this->pImpl->input.scenarioChanged = true; - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->input.mArea = measurementArea; - for (auto &vertex : this->pImpl->input.mArea) { - vertex.setAltitude(0); - } -} - -void SnakeDataManager::setServiceArea( - const QList &serviceArea) { - this->pImpl->input.scenarioChanged = true; - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->input.sArea = serviceArea; - for (auto &vertex : this->pImpl->input.sArea) { - vertex.setAltitude(0); - } -} - -void SnakeDataManager::setCorridor(const QList &corridor) { - this->pImpl->input.scenarioChanged = true; - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->input.corridor = corridor; - for (auto &vertex : this->pImpl->input.corridor) { - vertex.setAltitude(0); - } -} - -SnakeTiles SnakeDataManager::tiles() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.tiles; -} - -QVariantList SnakeDataManager::tileCenterPoints() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.tileCenterPoints; -} - -QNemoProgress SnakeDataManager::progress() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.progress; -} - -bool SnakeDataManager::calcInProgress() const { - return this->pImpl->output.calcInProgress.load(); -} - -QString SnakeDataManager::errorMessage() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.errorMessage; -} - -bool SnakeDataManager::success() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.errorMessage.isEmpty() ? true : false; -} - -QVector SnakeDataManager::waypoints() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.waypoints; -} - -QVector SnakeDataManager::arrivalPath() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.arrivalPath; -} - -QVector SnakeDataManager::returnPath() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.returnPath; -} - -Length SnakeDataManager::lineDistance() const { - SharedLock lk(this->pImpl->input.mutex); - return this->pImpl->input.lineDistance; -} - -void SnakeDataManager::setLineDistance(Length lineDistance) { - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->input.routeParametersChanged = true; - this->pImpl->input.lineDistance = lineDistance; -} - -Area SnakeDataManager::minTileArea() const { - SharedLock lk(this->pImpl->input.mutex); - return this->pImpl->scenario.minTileArea(); -} - -void SnakeDataManager::setMinTileArea(Area minTileArea) { - this->pImpl->input.scenarioChanged = true; - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->scenario.setMinTileArea(minTileArea); -} - -Length SnakeDataManager::tileHeight() const { - SharedLock lk(this->pImpl->input.mutex); - return this->pImpl->scenario.tileHeight(); -} - -void SnakeDataManager::setTileHeight(Length tileHeight) { - this->pImpl->input.scenarioChanged = true; - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->scenario.setTileHeight(tileHeight); -} - -Length SnakeDataManager::tileWidth() const { - SharedLock lk(this->pImpl->input.mutex); - return this->pImpl->scenario.tileWidth(); -} - -void SnakeDataManager::setTileWidth(Length tileWidth) { - UniqueLock lk(this->pImpl->input.mutex); - this->pImpl->input.scenarioChanged = true; - this->pImpl->scenario.setTileWidth(tileWidth); -} - -void SnakeDataManager::run() { -#ifndef NDEBUG - auto startTime = std::chrono::high_resolution_clock::now(); -#endif - - this->pImpl->output.calcInProgress.store(true); - emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); - auto onExit = [this, &startTime] { -#ifndef NDEBUG - qDebug() << "SnakeDataManager::run() execution time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - startTime) - .count() - << " ms"; -#endif - this->pImpl->output.calcInProgress.store(true); - emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); - }; - CommandRAII onExitRAII(onExit); - - this->pImpl->output.tilesUpdated = false; - this->pImpl->output.waypointsUpdated = false; - this->pImpl->output.progressUpdated = false; - - { - // 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) { - 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) { - // 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); - } - - // 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; - } - } - - snake::flight_plan::Transects transectsRouted; - snake::flight_plan::Route route; - std::vector 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); - } - 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{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) { - UniqueLock lk(this->pImpl->output.mutex); - this->pImpl->output.errorMessage = errorString.c_str(); - } else { - this->pImpl->output.waypointsUpdated = true; - } - } - } - - UniqueLock lk(this->pImpl->output.mutex); - // Store tiles. - if (this->pImpl->output.tilesUpdated) { - // 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(); - // Convert and store scenario data. - const auto &origin = this->pImpl->output.ENUOrigin; - const auto &tiles = this->pImpl->scenario.tiles(); - const auto ¢erPoints = 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 = 0; 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->output.tiles.polygons().push_back(geoTile); - this->pImpl->output.tileCenterPoints.push_back( - QVariant::fromValue(geoVertex)); - this->pImpl->output.tilesENU.polygons().push_back(enuTile); - this->pImpl->output.tileCenterPointsENU.push_back(enuVertex); - } - } - // 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); - } - } - // 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; - } - 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); - } - } -} diff --git a/src/Wima/Snake/SnakeThread.cc b/src/Wima/Snake/SnakeThread.cc new file mode 100644 index 000000000..c156e775e --- /dev/null +++ b/src/Wima/Snake/SnakeThread.cc @@ -0,0 +1,543 @@ +#include "SnakeThread.h" + +#include +#include + +#include "QGCApplication.h" +#include "QGCToolbox.h" +#include "SettingsFact.h" +#include "SettingsManager.h" +#include "WimaSettings.h" + +#include +#include +#include +#include + +#include "Wima/Snake/SnakeTiles.h" +#include "Wima/Snake/SnakeTilesLocal.h" +#include "snake.h" + +template class CommandRAII { +public: + CommandRAII(Callable &fun) : _fun(fun) {} + + ~CommandRAII() { _fun(); } + +private: + Callable _fun; +}; + +using QVariantList = QList; + +using UniqueLock = std::unique_lock; +using SharedLock = std::shared_lock; + +class SnakeThread::Impl { +public: + struct Input { + + Input() + : tileWidth(5 * si::meter), tileHeight(5 * si::meter), + minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter), + minTransectLength(1 * si::meter), scenarioChanged(true), + progressChanged(true), routeParametersChanged(true) {} + + QList mArea; + QGeoCoordinate ENUOrigin; + QList sArea; + QList corridor; + Length tileWidth; + Length tileHeight; + Area minTileArea; + + 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), tilesUpdated(false), waypointsUpdated(false), + progressUpdated(false) {} + + SnakeTiles tiles; + QVariantList tileCenterPoints; + SnakeTilesLocal tilesENU; + QVector tileCenterPointsENU; + QGeoCoordinate ENUOrigin; + + QVector waypoints; + QVector arrivalPath; + QVector returnPath; + QVector waypointsENU; + QVector arrivalPathENU; + QVector returnPathENU; + + QString errorMessage; + + std::atomic_bool calcInProgress; + bool tilesUpdated; + bool waypointsUpdated; + bool progressUpdated; + + mutable std::shared_timed_mutex mutex; + }; + + Impl(SnakeThread *p); + + bool precondition() const; + bool doTopicServiceSetup(); + + // Internal data. + snake::Scenario scenario; + SnakeThread *parent; + + // Input + Input input; + // Output + Output output; +}; + +SnakeThread::Impl::Impl(SnakeThread *p) : parent(p) {} + +SnakeThread::SnakeThread(QObject *parent) + : QThread(parent), pImpl(std::make_unique(this)) {} + +SnakeThread::~SnakeThread() {} + +void SnakeThread::setMeasurementArea( + const QList &measurementArea) { + this->pImpl->input.scenarioChanged = true; + UniqueLock lk(this->pImpl->input.mutex); + this->pImpl->input.mArea = measurementArea; + for (auto &vertex : this->pImpl->input.mArea) { + vertex.setAltitude(0); + } +} + +void SnakeThread::setServiceArea(const QList &serviceArea) { + this->pImpl->input.scenarioChanged = true; + UniqueLock lk(this->pImpl->input.mutex); + this->pImpl->input.sArea = serviceArea; + for (auto &vertex : this->pImpl->input.sArea) { + vertex.setAltitude(0); + } +} + +void SnakeThread::setCorridor(const QList &corridor) { + this->pImpl->input.scenarioChanged = true; + UniqueLock lk(this->pImpl->input.mutex); + this->pImpl->input.corridor = corridor; + for (auto &vertex : this->pImpl->input.corridor) { + vertex.setAltitude(0); + } +} + +void SnakeThread::setProgress(const QVector &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; +} + +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; +} + +QVector SnakeThread::progress() const { + SharedLock lk(this->pImpl->output.mutex); + return this->pImpl->input.progress.progress(); +} + +bool SnakeThread::calcInProgress() const { + return this->pImpl->output.calcInProgress.load(); +} + +QString SnakeThread::errorMessage() const { + SharedLock lk(this->pImpl->output.mutex); + return this->pImpl->output.errorMessage; +} + +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.progressUpdated; +} + +QVector SnakeThread::waypoints() const { + SharedLock lk(this->pImpl->output.mutex); + return this->pImpl->output.waypoints; +} + +QVector SnakeThread::arrivalPath() const { + SharedLock lk(this->pImpl->output.mutex); + return this->pImpl->output.arrivalPath; +} + +QVector SnakeThread::returnPath() const { + SharedLock lk(this->pImpl->output.mutex); + return this->pImpl->output.returnPath; +} + +Length SnakeThread::lineDistance() const { + SharedLock lk(this->pImpl->input.mutex); + return this->pImpl->input.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 SnakeThread::minTileArea() const { + SharedLock lk(this->pImpl->input.mutex); + return this->pImpl->scenario.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 SnakeThread::tileHeight() const { + SharedLock lk(this->pImpl->input.mutex); + return this->pImpl->scenario.tileHeight(); +} + +void SnakeThread::setTileHeight(Length tileHeight) { + this->pImpl->input.scenarioChanged = true; + UniqueLock lk(this->pImpl->input.mutex); + this->pImpl->scenario.setTileHeight(tileHeight); +} + +Length SnakeThread::tileWidth() const { + SharedLock lk(this->pImpl->input.mutex); + return this->pImpl->scenario.tileWidth(); +} + +void SnakeThread::setTileWidth(Length tileWidth) { + this->pImpl->input.scenarioChanged = true; + UniqueLock lk(this->pImpl->input.mutex); + this->pImpl->scenario.setTileWidth(tileWidth); +} + +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() << "SnakeThread::run() execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - startTime) + .count() + << " ms"; +#endif + this->pImpl->output.calcInProgress.store(false); + emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); + }; + CommandRAII onExitRAII(onExit); + + bool tilesValid = true; + QGeoCoordinate origin; + { + // Check preconditions. + SharedLock lk(this->pImpl->input.mutex); + if (this->pImpl->input.mArea.size() < 3) { + UniqueLock uLock(this->pImpl->output.mutex); + this->pImpl->output.errorMessage = "Measurement area invalid: 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."; + tilesValid = false; + } else { + // Set Origin + 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; + } + } + } + } + + bool waypointsValid = tilesValid; + bool progressValid = tilesValid; + snake::flight_plan::Transects transects; + snake::flight_plan::Transects transectsRouted; + snake::flight_plan::Route route; + std::vector progress; + 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]); + } + } + // 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{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 { + // No transects + waypointsValid = false; + progressValid = true; + } + } else { + // No update necessary + waypointsValid = true; + progressValid = true; + } + } + + UniqueLock lk(this->pImpl->output.mutex); + // Store tiles. + 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 &tiles = this->pImpl->scenario.tiles(); + const auto ¢erPoints = 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 = 0; 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->output.tiles.polygons().push_back(geoTile); + this->pImpl->output.tileCenterPoints.push_back( + QVariant::fromValue(geoVertex)); + this->pImpl->output.tilesENU.polygons().push_back(enuTile); + this->pImpl->output.tileCenterPointsENU.push_back(enuVertex); + } + this->pImpl->output.tilesUpdated = true; + } + // Store progress. + 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 (!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); + } + } + this->pImpl->output.waypointsUpdated = true; + } +} diff --git a/src/Wima/Snake/SnakeDataManager.h b/src/Wima/Snake/SnakeThread.h similarity index 84% rename from src/Wima/Snake/SnakeDataManager.h rename to src/Wima/Snake/SnakeThread.h index 543a5e15c..aa3ed1702 100644 --- a/src/Wima/Snake/SnakeDataManager.h +++ b/src/Wima/Snake/SnakeThread.h @@ -15,29 +15,34 @@ using namespace boost::units; using Length = quantity; using Area = quantity; -class SnakeDataManager : public QThread { +class SnakeThread : public QThread { Q_OBJECT class Impl; using PImpl = std::unique_ptr; public: - SnakeDataManager(QObject *parent = nullptr); - ~SnakeDataManager() override; + SnakeThread(QObject *parent = nullptr); + ~SnakeThread() override; void setMeasurementArea(const QList &measurementArea); void setServiceArea(const QList &serviceArea); void setCorridor(const QList &corridor); + void setProgress(const QVector &progress); SnakeTiles tiles() const; SnakeTilesLocal tilesENU() const; QGeoCoordinate ENUOrigin() const; QVariantList tileCenterPoints() const; - QNemoProgress progress() const; + QVector progress() const; bool calcInProgress() const; QString errorMessage() const; bool success() const; + bool tilesUpdated(); + bool waypointsUpdated(); + bool progressUpdated(); + QVector waypoints() const; QVector arrivalPath() const; QVector returnPath() const; diff --git a/src/Wima/Snake/SnakeTile.cpp b/src/Wima/Snake/SnakeTile.cpp index a1c73a2fe..44617ab65 100644 --- a/src/Wima/Snake/SnakeTile.cpp +++ b/src/Wima/Snake/SnakeTile.cpp @@ -1,23 +1,15 @@ #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); } diff --git a/src/Wima/Snake/SnakeTile.h b/src/Wima/Snake/SnakeTile.h index aeb6a7e7c..c527b4a4f 100644 --- a/src/Wima/Snake/SnakeTile.h +++ b/src/Wima/Snake/SnakeTile.h @@ -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); }; - diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 7e6edecf8..b04512578 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -11,6 +11,11 @@ #include +template +constexpr typename std::underlying_type::type integral(T value) { + return static_cast::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(this->_currentDM->tiles()); -} +QmlObjectListModel *WimaController::snakeTiles() { return &this->tiles; } QVariantList WimaController::snakeTileCenterPoints() { - return _currentDM->tileCenterPoints(); + return _currentThread->tileCenterPoints(); } QVector 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); } } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 3684a9c6a..35be27e6a 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -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; - // 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; static StatusMap _nemoStatusMap; // Periodic tasks. diff --git a/src/comm/ros_bridge/include/com_private.h b/src/comm/ros_bridge/include/com_private.h index c5dcffead..421fcbb80 100644 --- a/src/comm/ros_bridge/include/com_private.h +++ b/src/comm/ros_bridge/include/com_private.h @@ -2,8 +2,8 @@ #include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include #include +#include #include namespace ros_bridge { @@ -16,11 +16,6 @@ typedef std::size_t HashType; using ClientMap = std::unordered_map; -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 diff --git a/src/comm/ros_bridge/include/server.cpp b/src/comm/ros_bridge/include/server.cpp index eca731e71..548f2026f 100644 --- a/src/comm/ros_bridge/include/server.cpp +++ b/src/comm/ros_bridge/include/server.cpp @@ -2,98 +2,87 @@ #include "rapidjson/include/rapidjson/ostreamwrapper.h" -ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) : - _rbc(rbc) - , _stopped(std::make_shared(true)) -{ +static const char *serviceAdvertiserKey = "service_advertiser"; -} +ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) + : _rbc(rbc), _stopped(std::make_shared(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, - std::shared_ptr in_message){ + auto rbc = &_rbc; + _rbc.advertiseService( + clientName, service, type, + [userCallback, rbc](std::shared_ptr, + std::shared_ptr 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(); } diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp index dbee9d683..3f5fd4998 100644 --- a/src/comm/ros_bridge/include/topic_publisher.cpp +++ b/src/comm/ros_bridge/include/topic_publisher.cpp @@ -1,147 +1,134 @@ #include "topic_publisher.h" -#include - -ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) : - _stopped(std::make_shared(true)) - , _rbc(rbc) -{ - -} +static const char *topicAdvertiserKey = "topic_advertiser"; -ros_bridge::com_private::TopicPublisher::~TopicPublisher() -{ - this->reset(); -} +#include -void ros_bridge::com_private::TopicPublisher::start() -{ - if ( !_stopped->load() ) // start called while thread running. - return; - _stopped->store(false); - _pThread = std::make_unique([this]{ - // Main Loop. - while( !this->_stopped->load() ){ - std::unique_lock 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 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(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([this] { + // Main Loop. + while (!this->_stopped->load()) { + std::unique_lock 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 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 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 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 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 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 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 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; + } } - diff --git a/src/comm/ros_bridge/include/topic_subscriber.cpp b/src/comm/ros_bridge/include/topic_subscriber.cpp index 966303bbf..23cf5c5f0 100644 --- a/src/comm/ros_bridge/include/topic_subscriber.cpp +++ b/src/comm/ros_bridge/include/topic_subscriber.cpp @@ -2,98 +2,86 @@ #include -ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) : - _rbc(rbc) - , _stopped(std::make_shared(true)) -{ +static const char *topicSubscriberKey = "topic_subscriber"; -} +ros_bridge::com_private::TopicSubscriber::TopicSubscriber( + RosbridgeWsClient &rbc) + : _rbc(rbc), _stopped(std::make_shared(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 &callback) -{ - if ( _stopped->load() ) - return; + const char *topic, const std::function &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, - std::shared_ptr in_message) - { + // Wrap callback. + auto callbackWrapper = + [callback](std::shared_ptr, + std::shared_ptr 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; + } } - - diff --git a/src/ui/MainWindowInner.qml b/src/ui/MainWindowInner.qml index dc78d194f..d07cf0810 100644 --- a/src/ui/MainWindowInner.qml +++ b/src/ui/MainWindowInner.qml @@ -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 diff --git a/src/ui/MainWindowNative.qml b/src/ui/MainWindowNative.qml index 1b7cbe17e..374d9d564 100644 --- a/src/ui/MainWindowNative.qml +++ b/src/ui/MainWindowNative.qml @@ -39,6 +39,7 @@ Window { Loader { id: mainWindowInner anchors.fill: parent + anchors.margins: 10 source: "MainWindowInner.qml" Connections { -- 2.22.0