diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 76a017f54e25e7619aacb6c655bdda2973c1e1a9..d44117d2684f856a11cbcc21939df9176679fbdb 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -453,6 +453,8 @@ HEADERS += \ src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/geometry.h \ src/MeasurementComplexItem/HashFunctions.h \ + src/MeasurementComplexItem/nemo_interface/Command.h \ + src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \ src/QmlControls/QmlUnitsConversion.h \ src/MeasurementComplexItem/geometry/GeoArea.h \ @@ -534,6 +536,8 @@ SOURCES += \ src/MeasurementComplexItem/geometry/SafeArea.cc \ src/MeasurementComplexItem/geometry/geometry.cpp \ src/MeasurementComplexItem/HashFunctions.cpp \ + src/MeasurementComplexItem/nemo_interface/Command.cpp \ + src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \ src/Vehicle/VehicleEscStatusFactGroup.cc \ src/MeasurementComplexItem/AreaData.cc \ diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index 264dc58111b3719aefaca2b88c4b4331d15840bc..f70450a256535158749d8c559b78f2745f7a2ee6 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -40,47 +40,69 @@ using SharedLock = std::shared_lock; using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; class NemoInterface::Impl { - using TimePoint = std::chrono::time_point; - public: + enum class STATE { + STOPPED, + RUNNING, + WEBSOCKET_DETECTED, + HEARTBEAT_DETECTED, + TRY_TOPIC_SERVICE_SETUP, + READY, + SYNCHRONIZING, + TIMEOUT, + INVALID_HEARTBEAT + } + Impl(NemoInterface *p); void start(); void stop(); + // Tile editing. + // Functions that require communication to device. void addTiles(const TilePtrArray &tileArray); void addTiles(const TileArray &tileArray); void removeTiles(const IDArray &idArray); void clearTiles(); + + // Functions that don't require communication to device. 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(); + + // Progress. + ProgressArray getProgress(); + ProgressArray getProgress(const IDArray &idArray); NemoInterface::STATUS status(); bool running(); private: + typedef std::chrono::time_point TimePoint; + typedef std::map TileMap; + typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; + + void _addTiles(const TileArray &tileArray); + void _removeTiles(const IDArray &idArray); + void _clearTiles(); + void doTopicServiceSetup(); void loop(); - static STATUS heartbeatToStatus( - const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb); - bool setStatus(NemoInterface::STATUS s); + bool _setState(STATE s); + static bool _running(STATE s); + static void _translate(STATE state, NemoInterface::STATUS &status); + static void _translate(Heartbeat hb, STATE &state); TimePoint nextTimeout; mutable std::shared_timed_mutex timeoutMutex; - std::atomic status_; - - // Internals - std::atomic_bool running_; - std::atomic_bool topicServiceSetupDone; - ROSBridgePtr pRosBridge; - QTimer loopTimer; - NemoInterface *parent; + + STATE _state; + ROSBridgePtr _pRosBridge; + TileMap _tileMap; + NemoInterface *_parent; }; using StatusMap = std::map; @@ -98,7 +120,7 @@ static StatusMap statusMap{ NemoInterface::Impl::Impl(NemoInterface *p) : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED), - running_(false), topicServiceSetupDone(false), parent(p) { + running_(false), topicServiceSetupDone(false), _parent(p) { // ROS Bridge. WimaSettings *wimaSettings = @@ -114,7 +136,7 @@ NemoInterface::Impl::Impl(NemoInterface *p) "Websocket connection string possibly invalid: " + connectionString + ". Trying to connect anyways."); } - this->pRosBridge.reset( + this->_pRosBridge.reset( new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data())); }; connect(connectionStringFact, &SettingsFact::rawValueChanged, @@ -128,12 +150,73 @@ NemoInterface::Impl::Impl(NemoInterface *p) void NemoInterface::Impl::start() { this->running_ = true; - emit this->parent->runningChanged(); + emit this->_parent->runningChanged(); } void NemoInterface::Impl::stop() { this->running_ = false; - emit this->parent->runningChanged(); + emit this->_parent->runningChanged(); +} + +TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) { + TileArray tileArray; + + for (const auto &id : idArray) { + const auto it = _tileMap.find(id); + if (it != _tileMap.end()) { + tileArray.append(it->second); + } + } + + return tileArray; +} + +TileArray NemoInterface::Impl::getAllTiles() { + TileArray tileArray; + + for (const auto &entry : _tileMap) { + tileArray.append(entry.second); + } + + return tileArray; +} + +LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) { + LogicalArray logicalArray; + + for (const auto &id : idArray) { + const auto &it = _tileMap.find(id); + logicalArray.append(it != _tileMap.end()); + } + + return logicalArray; +} + +std::size_t NemoInterface::Impl::size() { return _tileMap.size(); } + +bool NemoInterface::Impl::empty() { return _tileMap.empty(); } + +ProgressArray NemoInterface::Impl::getProgress() { + ProgressArray progressArray; + + for (const auto &entry : _tileMap) { + progressArray.append(TaggedProgress{entry.first, entry.second.progress()}); + } + + return progressArray; +} + +ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) { + ProgressArray progressArray; + + for (const auto &id : idArray) { + const auto it = _tileMap.find(id); + if (id != _tileMap.end()) { + progressArray.append(TaggedProgress{it->first, it->second.progress()}); + } + } + + return progressArray; } void NemoInterface::Impl::setTileData(const TileData &tileData) { @@ -198,14 +281,16 @@ void NemoInterface::Impl::publishTileData() { } } -NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); } +NemoInterface::STATUS NemoInterface::Impl::status() { + return _translate(this->_state); +} QVector NemoInterface::Impl::progress() { SharedLock lk(this->progressMutex); return this->qProgress.progress(); } -bool NemoInterface::Impl::running() { return this->running_.load(); } +bool NemoInterface::Impl::running() { return _running(this->_state); } void NemoInterface::Impl::doTopicServiceSetup() { using namespace ros_bridge::messages; @@ -213,7 +298,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { // snake tiles. { SharedLock lk(this->tilesENUMutex); - this->pRosBridge->advertiseTopic( + this->_pRosBridge->advertiseTopic( "/snake/tiles", jsk_recognition_msgs::polygon_array::messageType().c_str()); } @@ -221,12 +306,12 @@ void NemoInterface::Impl::doTopicServiceSetup() { // snake origin. { SharedLock lk(this->ENUOriginMutex); - this->pRosBridge->advertiseTopic( + this->_pRosBridge->advertiseTopic( "/snake/origin", geographic_msgs::geo_point::messageType().c_str()); } // Subscribe nemo progress. - this->pRosBridge->subscribe( + this->_pRosBridge->subscribe( "/nemo/progress", /* callback */ [this](JsonDocUPtr pDoc) { std::lock(this->progressMutex, this->tilesENUMutex, @@ -244,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { qgcApp()->informationMessageBoxOnMainThread( "Nemo Interface", "Invalid progress message received."); } - emit this->parent->progressChanged(); + emit this->_parent->progressChanged(); lk1.unlock(); lk2.unlock(); @@ -252,15 +337,15 @@ void NemoInterface::Impl::doTopicServiceSetup() { }); // Subscribe /nemo/heartbeat. - this->pRosBridge->subscribe( + this->_pRosBridge->subscribe( "/nemo/heartbeat", /* callback */ [this](JsonDocUPtr pDoc) { // auto start = std::chrono::high_resolution_clock::now(); nemo_msgs::heartbeat::Heartbeat heartbeatMsg; if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) { - this->setStatus(STATUS::INVALID_HEARTBEAT); + this->_setState(STATUS::INVALID_HEARTBEAT); } else { - this->setStatus(heartbeatToStatus(heartbeatMsg)); + this->_setState(heartbeatToStatus(heartbeatMsg)); } if (this->status_ == STATUS::INVALID_HEARTBEAT) { UniqueLock lk(this->timeoutMutex); @@ -280,7 +365,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { }); // Advertise /snake/get_origin. - this->pRosBridge->advertiseService( + this->_pRosBridge->advertiseService( "/snake/get_origin", "snake_msgs/GetOrigin", [this](JsonDocUPtr) -> JsonDocUPtr { using namespace ros_bridge::messages; @@ -305,7 +390,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { }); // Advertise /snake/get_tiles. - this->pRosBridge->advertiseService( + this->_pRosBridge->advertiseService( "/snake/get_tiles", "snake_msgs/GetTiles", [this](JsonDocUPtr) -> JsonDocUPtr { SharedLock lk(this->tilesENUMutex); @@ -331,25 +416,25 @@ void NemoInterface::Impl::doTopicServiceSetup() { void NemoInterface::Impl::loop() { // Check ROS Bridge status and do setup if necessary. if (this->running_) { - if (!this->pRosBridge->isRunning()) { - this->pRosBridge->start(); + if (!this->_pRosBridge->isRunning()) { + this->_pRosBridge->start(); this->loop(); - } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() && - !this->topicServiceSetupDone) { + } else if (this->_pRosBridge->isRunning() && + this->_pRosBridge->connected() && !this->topicServiceSetupDone) { this->doTopicServiceSetup(); this->topicServiceSetupDone = true; - this->setStatus(STATUS::WEBSOCKET_DETECTED); - } else if (this->pRosBridge->isRunning() && - !this->pRosBridge->connected() && this->topicServiceSetupDone) { - this->pRosBridge->reset(); - this->pRosBridge->start(); + this->_setState(STATUS::WEBSOCKET_DETECTED); + } else if (this->_pRosBridge->isRunning() && + !this->_pRosBridge->connected() && this->topicServiceSetupDone) { + this->_pRosBridge->reset(); + this->_pRosBridge->start(); this->topicServiceSetupDone = false; - this->setStatus(STATUS::TIMEOUT); + this->_setState(STATUS::TIMEOUT); } - } else if (this->pRosBridge->isRunning()) { - this->pRosBridge->reset(); + } else if (this->_pRosBridge->isRunning()) { + this->_pRosBridge->reset(); this->topicServiceSetupDone = false; } @@ -359,10 +444,10 @@ void NemoInterface::Impl::loop() { if (this->nextTimeout != TimePoint::max() && this->nextTimeout < std::chrono::high_resolution_clock::now()) { lk.unlock(); - if (this->pRosBridge->isRunning() && this->pRosBridge->connected()) { - this->setStatus(STATUS::WEBSOCKET_DETECTED); + if (this->_pRosBridge->isRunning() && this->_pRosBridge->connected()) { + this->_setState(STATUS::WEBSOCKET_DETECTED); } else { - this->setStatus(STATUS::TIMEOUT); + this->_setState(STATUS::TIMEOUT); } } } @@ -382,7 +467,7 @@ void NemoInterface::Impl::publishTilesENU() { std::make_unique(rapidjson::kObjectType)); if (jsk_recognition_msgs::polygon_array::toJson( this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) { - this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); } else { qCWarning(NemoInterfaceLog) << "Impl::publishTilesENU: could not create json document."; @@ -395,17 +480,17 @@ void NemoInterface::Impl::publishENUOrigin() { std::make_unique(rapidjson::kObjectType)); if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin, jOrigin->GetAllocator())) { - this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin"); } else { qCWarning(NemoInterfaceLog) << "Impl::publishENUOrigin: could not create json document."; } } -bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) { +bool NemoInterface::Impl::_setState(STATE s) { if (s != this->status_) { this->status_ = s; - emit this->parent->statusChanged(); + emit this->_parent->statusChanged(); return true; } else { return false; @@ -460,14 +545,18 @@ 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(); } +ProgressArray NemoInterface::getProgress() { + return this->pImpl->getProgress(); +} + +ProgressArray NemoInterface::getProgress(const IDArray &idArray) { + return this->pImpl->getProgress(idArray); +} + void NemoInterface::publishTileData() { this->pImpl->publishTileData(); } void NemoInterface::requestProgress() { diff --git a/src/MeasurementComplexItem/NemoInterface.h b/src/MeasurementComplexItem/NemoInterface.h index 745a907d834295580485475ec0a8f010964afa4d..512d528b659ff2787fe1b848fd5d9511119c3419 100644 --- a/src/MeasurementComplexItem/NemoInterface.h +++ b/src/MeasurementComplexItem/NemoInterface.h @@ -9,6 +9,7 @@ #include "LogicalArray.h" #include "TileArray.h" #include "TilePtrArray.h" +#include "geometry/ProgressArray.h" // Singelton class used to interface measurement devices implementing the nemo // interface. @@ -26,11 +27,11 @@ public: static NemoInterface *instance(); enum class STATUS { - NOT_CONNECTED = 0, - HEARTBEAT_DETECTED = 1, - WEBSOCKET_DETECTED = 2, - TIMEOUT = -1, - INVALID_HEARTBEAT = -2 + NOT_CONNECTED, + READY, + WEBSOCKET_DETECTED, + TIMEOUT, + INVALID_HEARTBEAT }; Q_ENUM(STATUS) @@ -39,9 +40,12 @@ public: Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(bool running READ running NOTIFY runningChanged) + QString editorQml(); + Q_INVOKABLE void start(); Q_INVOKABLE void stop(); + // Tile editing. void addTiles(const TilePtrArray &tileArray); void addTiles(const TileArray &tileArray); void removeTiles(const IDArray &idArray); @@ -49,18 +53,22 @@ public: TileArray getTiles(const IDArray &idArray); TileArray getAllTiles(); LogicalArray containsTiles(const IDArray &idArray); - TileArray extractTiles(const IDArray &idArray); std::size_t size(); bool empty(); + // Progress. + ProgressArray getProgress(); + ProgressArray getProgress(const IDArray &idArray); + + // Status. STATUS status() const; QString statusString() const; - QString editorQml(); bool running(); signals: void statusChanged(); - void progressChanged(); + void progressChanged(const ProgressArray &progressArray); + void tilesChanged(); void runningChanged(); private: diff --git a/src/MeasurementComplexItem/nemo_interface/Command.cpp b/src/MeasurementComplexItem/nemo_interface/Command.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c935845d1926de9d466394be4d917bc6c8f23be0 --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/Command.cpp @@ -0,0 +1,9 @@ +#include "Command.h" + +namespace nemo_interface { + +Command::Command(Functor onExec) : _onExec(onExec) {} + +QFuture Command::exec() { return _onExec(); } + +} // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/Command.h b/src/MeasurementComplexItem/nemo_interface/Command.h new file mode 100644 index 0000000000000000000000000000000000000000..b0133990a098f183b2945ce8f1c0abf201a9f116 --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/Command.h @@ -0,0 +1,30 @@ +#ifndef COMMAND_H +#define COMMAND_H + +#include +#include + +namespace nemo_interface { + +class Command { +public: + enum class ERROR { + NO_ERROR, + NETWORK_TIMEOUT, + PARAMETER_ERROR, + UNEXPECTED_SERVER_RESPONSE + }; + + typedef QFuture ReturnType; + typedef std::function Functor; + + Command(Functor onExec); + + QFuture exec(); + +private: + Functor _onExec; +}; +} // namespace nemo_interface + +#endif // COMMAND_H diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2314523d886eff07a6ca3b0c79692f2f8194f5ca --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp @@ -0,0 +1,6 @@ +#include "CommandDispatcher.h" + +CommandDispatcher::CommandDispatcher() +{ + +} diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h new file mode 100644 index 0000000000000000000000000000000000000000..d2f22f738f2b804b595aaa244180c003a1a85169 --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h @@ -0,0 +1,11 @@ +#ifndef COMMANDDISPATCHER_H +#define COMMANDDISPATCHER_H + + +class CommandDispatcher +{ +public: + CommandDispatcher(); +}; + +#endif // COMMANDDISPATCHER_H