From 67247b3b2fded6e3a590ebc1dac0f73e65b1dae4 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 6 Jan 2021 10:48:29 +0100 Subject: [PATCH] temp --- qgroundcontrol.pro | 6 +- src/MeasurementComplexItem/NemoInterface.cpp | 146 +++++++++--------- .../messages/nemo_msgs/labeled_progress.cpp | 5 + .../messages/nemo_msgs/labeled_progress.h | 68 ++++++++ .../include/messages/nemo_msgs/progress.cpp | 6 - .../include/messages/nemo_msgs/progress.h | 78 ---------- .../include/messages/nemo_msgs/tile.cpp | 6 + .../include/messages/nemo_msgs/tile.h | 91 +++++++++++ 8 files changed, 247 insertions(+), 159 deletions(-) create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h delete mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp delete mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/progress.h create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/tile.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d44117d26..d38da819d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -453,6 +453,7 @@ HEADERS += \ src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/geometry.h \ src/MeasurementComplexItem/HashFunctions.h \ + src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \ src/MeasurementComplexItem/nemo_interface/Command.h \ src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \ @@ -517,7 +518,7 @@ HEADERS += \ src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \ src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ - src/comm/ros_bridge/include/messages/nemo_msgs/progress.h \ + src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \ src/comm/ros_bridge/include/messages/std_msgs/header.h \ src/comm/ros_bridge/include/server.h \ src/comm/ros_bridge/include/messages/std_msgs/time.h \ @@ -564,7 +565,8 @@ SOURCES += \ src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \ src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \ - src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp \ + src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \ + src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \ src/comm/ros_bridge/include/messages/std_msgs/header.cpp \ src/comm/ros_bridge/include/messages/std_msgs/time.cpp \ src/comm/ros_bridge/include/server.cpp \ diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index f70450a25..5b3b5ba62 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -79,15 +79,17 @@ public: NemoInterface::STATUS status(); bool running(); +private slots: + void _addTilesLocal(const TileArray &tileArray); + void _removeTilesLocal(const IDArray &idArray); + void _clearTilesLocal(); + void _updateProgress(ProgressArray progressArray); + 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(); @@ -158,6 +160,8 @@ void NemoInterface::Impl::stop() { emit this->_parent->runningChanged(); } +void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {} + TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) { TileArray tileArray; @@ -292,24 +296,76 @@ QVector NemoInterface::Impl::progress() { bool NemoInterface::Impl::running() { return _running(this->_state); } -void NemoInterface::Impl::doTopicServiceSetup() { - using namespace ros_bridge::messages; +void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) { + bool anyChanges = false; + + for (const auto &tile : tileArray) { + const auto id = tile.id(); + const auto it = _tileMap.find(id); + if (Q_LIKELY(it == _tileMap.end())) { + auto ret = _tileMap.insert(std::make_pair(id, tile)); + anyChanges = true; + Q_ASSERT(ret.second == true); + Q_UNUSED(ret); + } else { + qCDebug(NemoInterfaceLog) + << "_addTilesLocal(): tile with id " << id << " already inserted."; + } + } + + if (anyChanges) { + emit _parent->tilesChanged(); + } +} + +void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) { + bool anyChanges = false; + for (const auto &id : idArray) { + const auto it = _tileMap.find(id); + if (Q_LIKELY(it != _tileMap.end())) { + _tileMap.erase(it); + anyChanges = true; + } else { + qCDebug(NemoInterfaceLog) + << "_removeTilesLocal(): tile with id " << id << " not found."; + } + } + + if (anyChanges) { + emit _parent->tilesChanged(); + } +} - // snake tiles. - { - SharedLock lk(this->tilesENUMutex); - this->_pRosBridge->advertiseTopic( - "/snake/tiles", - jsk_recognition_msgs::polygon_array::messageType().c_str()); +void NemoInterface::Impl::_clearTilesLocal() { + if (!_tileMap.empty()) { + _tileMap.clear(); + emit _parent->tilesChanged(); } +} + +void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) { + for (auto itPair = progressArray.begin(); itPair != progressArray.end();) { + + const auto &id = itPair->first; + auto it = _tileMap.find(id); - // snake origin. - { - SharedLock lk(this->ENUOriginMutex); - this->_pRosBridge->advertiseTopic( - "/snake/origin", geographic_msgs::geo_point::messageType().c_str()); + if (Q_LIKELY(it != _tileMap.end())) { + const auto &progress = itPair->second; + it->second.setProgress(progress); + ++itPair; + } else { + qCDebug(NemoInterfaceLog) + << "_updateProgress(): tile with id " << id << " not found."; + itPair = progressArray.erase(itPair); + } } + emit _parent->progressChanged(progressArray); +} + +void NemoInterface::Impl::doTopicServiceSetup() { + using namespace ros_bridge::messages; + // Subscribe nemo progress. this->_pRosBridge->subscribe( "/nemo/progress", @@ -340,7 +396,6 @@ void NemoInterface::Impl::doTopicServiceSetup() { 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->_setState(STATUS::INVALID_HEARTBEAT); @@ -355,61 +410,6 @@ void NemoInterface::Impl::doTopicServiceSetup() { this->nextTimeout = std::chrono::high_resolution_clock::now() + timeoutInterval; } - - // 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. - this->_pRosBridge->advertiseService( - "/snake/get_origin", "snake_msgs/GetOrigin", - [this](JsonDocUPtr) -> JsonDocUPtr { - using namespace ros_bridge::messages; - SharedLock lk(this->ENUOriginMutex); - - JsonDocUPtr pDoc( - std::make_unique(rapidjson::kObjectType)); - auto &origin = this->ENUOrigin; - rapidjson::Value jOrigin(rapidjson::kObjectType); - lk.unlock(); - if (geographic_msgs::geo_point::toJson(origin, jOrigin, - pDoc->GetAllocator())) { - lk.unlock(); - pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); - } else { - lk.unlock(); - qCWarning(NemoInterfaceLog) - << "/snake/get_origin service: could not create json document."; - } - - return pDoc; - }); - - // Advertise /snake/get_tiles. - this->_pRosBridge->advertiseService( - "/snake/get_tiles", "snake_msgs/GetTiles", - [this](JsonDocUPtr) -> JsonDocUPtr { - SharedLock lk(this->tilesENUMutex); - - JsonDocUPtr pDoc( - std::make_unique(rapidjson::kObjectType)); - rapidjson::Value jSnakeTiles(rapidjson::kObjectType); - - if (jsk_recognition_msgs::polygon_array::toJson( - this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) { - lk.unlock(); - pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); - } else { - lk.unlock(); - qCWarning(NemoInterfaceLog) - << "/snake/get_tiles service: could not create json document."; - } - - return pDoc; }); } diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp new file mode 100644 index 000000000..7718eefea --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp @@ -0,0 +1,5 @@ +#include "labeled_progress.h" + +std::string ros_bridge::messages::nemo_msgs::labeled_progress::messageType() { + return "nemo_msgs/LabeledProgress"; +} diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h new file mode 100644 index 000000000..e6fad66f8 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h @@ -0,0 +1,68 @@ +#pragma once + +#include "ros_bridge/rapidjson/include/rapidjson/document.h" + +#include + +namespace ros_bridge { +//! @brief Namespace containing classes and methodes ros message generation. +namespace messages { +//! @brief Namespace containing classes and methodes for geometry_msgs +//! generation. +namespace nemo_msgs { +//! @brief Namespace containing methodes for geometry_msgs/Point32 message +//! generation. +namespace labeled_progress { + +std::string messageType(); + +namespace { +const char *progressKey = "progress"; +const char *idKey = "id"; +} // namespace + +//! @brief C++ representation of nemo_msgs/labeled_progress message +class LabeledProgress { +public: + LabeledProgress() {} + LabeledProgress(double progress, long id) : _id(id), _progress(progress) {} + + long id() const { return _id; } + void setId(long id) { _id = id; } + + double progress() const { return _progress; } + void setProgress(double progress) { _progress = progress; } + +protected: + long _id; + double _progress; +}; + +template +bool toJson(const LabeledProgressType &lp, rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator) { + value.AddMember(idKey, lb.id(), allocator); + value.AddMember(progressKey, lb.progress(), allocator); + return true; +} + +template +bool fromJson(const rapidjson::Value &value, ProgressType &p) { + if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) { + return false; + } + + if (!value.HasMember(idKey) || !value[idKey].IsInt()) { + return false; + } + + p.setId(value[idKey].GetInt()); + p.setProgress(value[progressKey].GetDouble()); + + return true; +} + +} // namespace labeled_progress +} // namespace nemo_msgs +} // namespace messages +} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp deleted file mode 100644 index 0b408a2df..000000000 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "progress.h" - -std::string ros_bridge::messages::nemo_msgs::progress::messageType(){ - return "nemo_msgs/Progress"; -} - diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h deleted file mode 100644 index 894b8b043..000000000 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include "ros_bridge/rapidjson/include/rapidjson/document.h" - -#include - -namespace ros_bridge { -//! @brief Namespace containing classes and methodes ros message generation. -namespace messages { -//! @brief Namespace containing classes and methodes for geometry_msgs -//! generation. -namespace nemo_msgs { -//! @brief Namespace containing methodes for geometry_msgs/Point32 message -//! generation. -namespace progress { - -std::string messageType(); - -//! @brief C++ representation of nemo_msgs/Progress message -template class ContainterType = std::vector> -class GenericProgress { -public: - GenericProgress() {} - GenericProgress(const ContainterType &progress) - : _progress(progress) {} - GenericProgress(const GenericProgress &p) : _progress(p.progress()) {} - - virtual const ContainterType &progress(void) const { - return _progress; - } - virtual ContainterType &progress(void) { return _progress; } - - bool operator==(const GenericProgress &other) const { - return this->_progress == other._progress; - } - - bool operator!=(const GenericProgress &other) const { - return !this->operator==(other); - } - -protected: - ContainterType _progress; -}; -typedef GenericProgress<> Progress; - -template -bool toJson(const ProgressType &p, rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) { - rapidjson::Value jProgress(rapidjson::kArrayType); - for (unsigned long i = 0; i < std::uint64_t(p.progress().size()); ++i) { - jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), - allocator); - } - value.AddMember("progress", jProgress, allocator); - return true; -} - -template -bool fromJson(const rapidjson::Value &value, ProgressType &p) { - if (!value.HasMember("progress") || !value["progress"].IsArray()) { - assert(false); - return false; - } - - const auto &jsonProgress = value["progress"]; - unsigned long sz = jsonProgress.Size(); - p.progress().clear(); - p.progress().reserve(sz); - for (unsigned long i = 0; i < sz; ++i) - p.progress().push_back(std::int32_t(jsonProgress[i].GetInt())); - return true; -} - -} // namespace progress -} // namespace nemo_msgs -} // namespace messages -} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp new file mode 100644 index 000000000..3b0930410 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp @@ -0,0 +1,6 @@ +#include "tile.h" + +tile::tile() +{ + +} diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h new file mode 100644 index 000000000..a20e25342 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h @@ -0,0 +1,91 @@ +#pragma once + +#include "ros_bridge/include/messages/geographic_msgs/geopoint.h" +#include "ros_bridge/rapidjson/include/rapidjson/document.h" + +#include + +namespace ros_bridge { +//! @brief Namespace containing classes and methodes ros message generation. +namespace messages { +//! @brief Namespace containing classes and methodes for nemo_msgs +//! generation. +namespace nemo_msgs { +//! @brief Namespace containing methodes for nemo_msgs/tile message +//! generation. +namespace labeled_progress { + +std::string messageType(); + +namespace { +const char *progressKey = "progress"; +const char *idKey = "id"; +const char *tileKey = "tile"; +} // namespace + +//! @brief C++ representation of nemo_msgs/tile message +template class Container = std::vector> +class GenericTile { +public: + typedef Container GeoContainer; + GenericTile() {} + GenericTile(const GeoContainer &tile, double progress, long id) + : _tile(tile), _id(id), _progress(progress) {} + + long id() const { return _id; } + void setId(long id) { _id = id; } + + double progress() const { return _progress; } + void setProgress(double progress) { _progress = progress; } + + const GeoContainer &tile() const { return _tile; } + GeoContainer &tile() { return _tile; } + void setTile(const GeoContainer &tile) { _tile = tile; } + +protected: + long _id; + double _progress; + GeoContainer _tile; +}; +typedef GenericTile<> Tile; + +template +bool toJson(const TileType &tile, rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator) { + value.AddMember(idKey, tile.id(), allocator); + value.AddMember(progressKey, tile.progress(), allocator); + + using namespace messages::geographic_msgs; + rapidjson::Value geoPoints(rapidjson::kArrayType); + for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) { + rapidjson::Document geoPoint(rapidjson::kObjectType); + if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) { + return false; + } + geoPoints.PushBack(geoPoint, allocator); + } + value.AddMember(tileKey, geoPoints, allocator); + + return true; +} + +template +bool fromJson(const rapidjson::Value &value, ProgressType &p) { + if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) { + return false; + } + + if (!value.HasMember(idKey) || !value[idKey].IsInt()) { + return false; + } + + p.setId(value[idKey].GetInt()); + p.setProgress(value[progressKey].GetDouble()); + + return true; +} + +} // namespace labeled_progress +} // namespace nemo_msgs +} // namespace messages +} // namespace ros_bridge -- 2.22.0