From 40da114dd953f2a1e86308c71c62766db2971db4 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 31 Dec 2020 18:45:28 +0100 Subject: [PATCH] started to implement new nemo interface --- qgroundcontrol.pro | 4 + src/MeasurementComplexItem/IDArray.h | 5 ++ src/MeasurementComplexItem/LogicalArray.h | 5 ++ src/MeasurementComplexItem/NemoInterface.cpp | 81 +++++++++++++------- src/MeasurementComplexItem/NemoInterface.h | 25 +++--- src/MeasurementComplexItem/TileArray.h | 5 ++ src/MeasurementComplexItem/TilePtrArray.h | 5 ++ 7 files changed, 93 insertions(+), 37 deletions(-) create mode 100644 src/MeasurementComplexItem/IDArray.h create mode 100644 src/MeasurementComplexItem/LogicalArray.h create mode 100644 src/MeasurementComplexItem/TileArray.h create mode 100644 src/MeasurementComplexItem/TilePtrArray.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3677488dd..76a017f54 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -445,6 +445,10 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { # HEADERS += \ + src/MeasurementComplexItem/IDArray.h \ + src/MeasurementComplexItem/LogicalArray.h \ + src/MeasurementComplexItem/TileArray.h \ + src/MeasurementComplexItem/TilePtrArray.h \ src/MeasurementComplexItem/geometry/ProgressArray.h \ src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/geometry.h \ diff --git a/src/MeasurementComplexItem/IDArray.h b/src/MeasurementComplexItem/IDArray.h new file mode 100644 index 000000000..6052cec19 --- /dev/null +++ b/src/MeasurementComplexItem/IDArray.h @@ -0,0 +1,5 @@ +#ifndef IDARRAY_H +#define IDARRAY_H +typedef QVector IDArray; + +#endif // IDARRAY_H diff --git a/src/MeasurementComplexItem/LogicalArray.h b/src/MeasurementComplexItem/LogicalArray.h new file mode 100644 index 000000000..359eb67a3 --- /dev/null +++ b/src/MeasurementComplexItem/LogicalArray.h @@ -0,0 +1,5 @@ +#ifndef LOGICALARRAY_H +#define LOGICALARRAY_H +typedef QVector LogicalArray; + +#endif // LOGICALARRAY_H diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index 4cfd1f104..264dc5811 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -15,9 +15,9 @@ #include "GenericSingelton.h" #include "geometry/MeasurementArea.h" #include "geometry/geometry.h" +#include "nemo_interface/MeasurementTile.h" #include "nemo_interface/QNemoHeartbeat.h" #include "nemo_interface/QNemoProgress.h" -#include "nemo_interface/MeasurementTile.h" #include "ros_bridge/include/messages/geographic_msgs/geopoint.h" #include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h" @@ -31,7 +31,7 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") #define EVENT_TIMER_INTERVAL 100 // ms -auto static timeoutInterval = std::chrono::milliseconds(3000); +auto constexpr static timeoutInterval = std::chrono::milliseconds(3000); using ROSBridgePtr = std::unique_ptr; using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; @@ -47,15 +47,20 @@ public: void start(); void stop(); - void setTileData(const TileData &tileData); - bool hasTileData(const TileData &tileData) const; - void setAutoPublish(bool ap); - void setHoldProgress(bool hp); + void addTiles(const TilePtrArray &tileArray); + void addTiles(const TileArray &tileArray); + void removeTiles(const IDArray &idArray); + void clearTiles(); + TileArray getTiles(const IDArray &idArray); + TileArray getAllTiles(); + LogicalArray containsTiles(const IDArray &idArray); + TileArray extractTiles(const IDArray &idArray); + std::size_t size(); + bool empty(); void publishTileData(); NemoInterface::STATUS status(); - QVector progress(); bool running(); private: @@ -63,32 +68,13 @@ private: void loop(); static STATUS heartbeatToStatus( const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb); - //! - //! \brief Publishes tilesENU - //! \pre this->tilesENUMutex must be locked - //! - void publishTilesENU(); - //! - //! \brief Publishes ENUOrigin - //! \pre this->ENUOriginMutex must be locked - //! - void publishENUOrigin(); + bool setStatus(NemoInterface::STATUS s); - // Data. - SnakeTilesLocal tilesENU; - mutable std::shared_timed_mutex tilesENUMutex; - QGeoCoordinate ENUOrigin; - mutable std::shared_timed_mutex ENUOriginMutex; - QNemoProgress qProgress; - mutable std::shared_timed_mutex progressMutex; TimePoint nextTimeout; mutable std::shared_timed_mutex timeoutMutex; std::atomic status_; - // Not protected data. - TileData tileData; - // Internals std::atomic_bool running_; std::atomic_bool topicServiceSetupDone; @@ -170,7 +156,8 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { tile = qobject_cast(obj); if (tile != nullptr) { SnakeTileLocal tileENU; - geometry::areaToEnu(origin, tile->coordinateList(), tileENU.path()); + geometry::areaToEnu(origin, tile->coordinateList(), + tileENU.path()); this->tilesENU.polygons().push_back(std::move(tileENU)); } else { qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; @@ -443,6 +430,44 @@ void NemoInterface::start() { this->pImpl->start(); } void NemoInterface::stop() { this->pImpl->stop(); } +void NemoInterface::addTiles(const NemoInterface::TilePtrArray &tileArray) { + this->pImpl - +} + +void NemoInterface::addTiles(const TileArray &tileArray) { + TilePtrArray ptrArray; + for (const auto &tile : tileArray) { + ptrArray.append(&tile); + } + addTiles(ptrArray); +} + +void NemoInterface::removeTiles(const IDArray &idArray) { + this->pImpl->removeTiles(idArray); +} + +void NemoInterface::clearTiles() { this->pImpl->clearTiles(); } + +TileArray NemoInterface::getTiles(const IDArray &idArray) { + return this->pImpl->getTiles(idArray); +} + +TileArray NemoInterface::getAllTiles() { + return this->pImpl->getTiles(idArray); +} + +LogicalArray NemoInterface::containsTiles(const IDArray &idArray) { + return this->pImpl->containsTiles(idArray); +} + +TileArray NemoInterface::extractTiles(const IDArray &idArray) { + return this->pImpl->extractTiles(idArray); +} + +std::size_t NemoInterface::size() { return this->pImpl->size(); } + +bool NemoInterface::empty() { return this->pImpl->empty(); } + void NemoInterface::publishTileData() { this->pImpl->publishTileData(); } void NemoInterface::requestProgress() { diff --git a/src/MeasurementComplexItem/NemoInterface.h b/src/MeasurementComplexItem/NemoInterface.h index 681d3251c..745a907d8 100644 --- a/src/MeasurementComplexItem/NemoInterface.h +++ b/src/MeasurementComplexItem/NemoInterface.h @@ -5,8 +5,13 @@ #include -class TileData; +#include "IDArray.h" +#include "LogicalArray.h" +#include "TileArray.h" +#include "TilePtrArray.h" +// Singelton class used to interface measurement devices implementing the nemo +// interface. class NemoInterface : public QObject { Q_OBJECT class Impl; @@ -31,23 +36,25 @@ public: Q_PROPERTY(STATUS status READ status NOTIFY statusChanged) Q_PROPERTY(QString statusString READ statusString NOTIFY statusChanged) - Q_PROPERTY(QVector progress READ progress NOTIFY progressChanged) Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(bool running READ running NOTIFY runningChanged) Q_INVOKABLE void start(); Q_INVOKABLE void stop(); - Q_INVOKABLE void publishTileData(); - Q_INVOKABLE void requestProgress(); - void setTileData(const TileData &tileData); - bool hasTileData(const TileData &tileData) const; - void setAutoPublish(bool ap); - void setHoldProgress(bool hp); + void addTiles(const TilePtrArray &tileArray); + void addTiles(const TileArray &tileArray); + void removeTiles(const IDArray &idArray); + void clearTiles(); + TileArray getTiles(const IDArray &idArray); + TileArray getAllTiles(); + LogicalArray containsTiles(const IDArray &idArray); + TileArray extractTiles(const IDArray &idArray); + std::size_t size(); + bool empty(); STATUS status() const; QString statusString() const; - QVector progress() const; QString editorQml(); bool running(); diff --git a/src/MeasurementComplexItem/TileArray.h b/src/MeasurementComplexItem/TileArray.h new file mode 100644 index 000000000..c611f0275 --- /dev/null +++ b/src/MeasurementComplexItem/TileArray.h @@ -0,0 +1,5 @@ +#ifndef TILEARRAY_H +#define TILEARRAY_H +typedef QVector TileArray; + +#endif // TILEARRAY_H diff --git a/src/MeasurementComplexItem/TilePtrArray.h b/src/MeasurementComplexItem/TilePtrArray.h new file mode 100644 index 000000000..e40d27d85 --- /dev/null +++ b/src/MeasurementComplexItem/TilePtrArray.h @@ -0,0 +1,5 @@ +#ifndef TILEPTRARRAY_H +#define TILEPTRARRAY_H +typedef QVector TilePtrArray; + +#endif // TILEPTRARRAY_H -- 2.22.0