From b098b99a33a31dccc87f36bf1dac4d118c19067d Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Tue, 26 Jan 2021 10:25:03 +0100 Subject: [PATCH] tile id int64_t > QString, for robustness --- qgroundcontrol.pro | 4 - src/MeasurementComplexItem/IDArray.h | 2 +- .../MeasurementComplexItem.h | 2 +- src/MeasurementComplexItem/NemoInterface.cpp | 265 +++++------------- .../geometry/MeasurementArea.cc | 25 +- .../geometry/MeasurementArea.h | 2 +- .../nemo_interface/MeasurementTile.cpp | 19 +- .../nemo_interface/MeasurementTile.h | 8 +- .../nemo_interface/TaskDispatcher.cpp | 2 + .../nemo_interface/TaskDispatcher.h | 1 + .../rosbridge/rosbridge.cpp | 47 +++- .../rosbridge/rosbridge.h | 3 + .../rosbridge/rosbridgeimpl.cpp | 10 +- .../messages/geographic_msgs/geopoint.h | 223 +++++++-------- .../messages/geometry_msgs/point32.cpp | 6 - .../include/messages/geometry_msgs/point32.h | 139 --------- .../messages/geometry_msgs/polygon.cpp | 6 - .../include/messages/geometry_msgs/polygon.h | 91 ------ .../geometry_msgs/polygon_stamped.cpp | 6 - .../messages/geometry_msgs/polygon_stamped.h | 172 ------------ .../include/messages/nemo_msgs/heartbeat.h | 13 +- .../messages/nemo_msgs/labeled_progress.cpp | 2 +- .../messages/nemo_msgs/labeled_progress.h | 34 +-- .../messages/nemo_msgs/progress_array.h | 32 +-- .../include/messages/nemo_msgs/tile.cpp | 2 +- .../include/messages/nemo_msgs/tile.h | 53 ++-- .../include/messages/std_msgs/header.cpp | 69 ----- .../include/messages/std_msgs/header.h | 102 ------- .../include/messages/std_msgs/time.cpp | 5 - .../include/messages/std_msgs/time.h | 65 ----- 30 files changed, 326 insertions(+), 1084 deletions(-) delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/point32.h delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp delete mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h delete mode 100644 src/comm/ros_bridge/include/messages/std_msgs/header.cpp delete mode 100644 src/comm/ros_bridge/include/messages/std_msgs/header.h delete mode 100644 src/comm/ros_bridge/include/messages/std_msgs/time.cpp delete mode 100644 src/comm/ros_bridge/include/messages/std_msgs/time.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3b6e1c298..dcd246db8 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -517,8 +517,6 @@ HEADERS += \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.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/messages/std_msgs/time.h \ src/comm/utilities.h contains (DEFINES, QGC_ENABLE_PAIRING) { @@ -560,8 +558,6 @@ SOURCES += \ src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.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/Settings/WimaSettings.cc \ contains (DEFINES, QGC_ENABLE_PAIRING) { diff --git a/src/MeasurementComplexItem/IDArray.h b/src/MeasurementComplexItem/IDArray.h index 6ad3d32ea..b4d2e1d3d 100644 --- a/src/MeasurementComplexItem/IDArray.h +++ b/src/MeasurementComplexItem/IDArray.h @@ -3,6 +3,6 @@ #include -typedef QVector IDArray; +typedef QVector IDArray; #endif // IDARRAY_H diff --git a/src/MeasurementComplexItem/MeasurementComplexItem.h b/src/MeasurementComplexItem/MeasurementComplexItem.h index 4812b67d0..f3ec040bc 100644 --- a/src/MeasurementComplexItem/MeasurementComplexItem.h +++ b/src/MeasurementComplexItem/MeasurementComplexItem.h @@ -10,7 +10,7 @@ #include "SettingsFact.h" #include "AreaData.h" -#include "ProgressArray.h" +#include "geometry/ProgressArray.h" class RoutingThread; class RoutingResult; diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index 171c3fc0a..69d176252 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -9,6 +9,8 @@ #include +#include +#include #include #include @@ -24,9 +26,6 @@ #include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" #include "ros_bridge/include/messages/nemo_msgs/progress_array.h" #include "ros_bridge/include/messages/nemo_msgs/tile.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h" -#include "ros_bridge/rapidjson/include/rapidjson/writer.h" #include "rosbridge/rosbridge.h" QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") @@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr; typedef ros_bridge::messages::nemo_msgs::tile::GenericTile Tile; -typedef std::map> TileMap; -typedef std::map> TileMapConst; +typedef std::map> TileMap; +typedef std::map> TileMapConst; typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; typedef nemo_interface::TaskDispatcher Dispatcher; typedef nemo_interface::FutureWatcher @@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() { using namespace ros_bridge::messages; // Subscribe nemo progress. - this->_pRosbridge->subscribeTopic(progressTopic, [this]( - const QJsonObject &o) { - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - - // parse in_message - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("msg") && d["msg"].IsObject()) { - - // create obj from json + this->_pRosbridge->subscribeTopic( + progressTopic, [this](const QJsonObject &o) { nemo_msgs::progress_array::ProgressArray progressArray; - if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) { + if (nemo_msgs::progress_array::fromJson(o, progressArray)) { // correct range errors of progress for (auto &lp : progressArray.progress_array()) { @@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() { } else { qCWarning(NemoInterfaceLog) << "/nemo/progress not able to " "create ProgressArray form json: " - << msg; + << o; } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/progress no \"msg\" key or wrong type: " << msg; - } - } else { - qCWarning(NemoInterfaceLog) << "/nemo/progress message parse error (" - << d.GetParseError() << "): " << msg; - } - }); + }); // Subscribe heartbeat msg. - this->_pRosbridge->subscribeTopic(heartbeatTopic, [this]( - const QJsonObject &o) { - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - - // parse in_message - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("msg") && d["msg"].IsObject()) { - - // create obj from json + this->_pRosbridge->subscribeTopic( + heartbeatTopic, [this](const QJsonObject &o) { nemo_msgs::heartbeat::Heartbeat heartbeat; - if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) { + if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) { std::promise promise; auto future = promise.get_future(); bool value = QMetaObject::invokeMethod( @@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() { } else { qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to " "create Heartbeat form json: " - << msg; + << o; } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/heartbeat no \"msg\" key or wrong type: " << msg; - } - } else { - qCWarning(NemoInterfaceLog) << "/nemo/heartbeat message parse error (" - << d.GetParseError() << "): " << msg; - } - }); + }); } void NemoInterface::Impl::_trySynchronize() { @@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() { this->_state == STATE::USER_SYNC) && !_isSynchronized()) { + if (!_dispatcher.idle()) { + QTimer::singleShot(5000, [this] { this->_trySynchronize(); }); + return; + } + qCWarning(NemoInterfaceLog) << "trying to synchronize"; this->_setState(STATE::SYS_SYNC); @@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() { std::bind(&Impl::_callClearTiles, this)); // dispatch command. - _dispatcher.clear(); - _dispatcher.stop(); Q_ASSERT(_dispatcher.pendingTasks() == 0); auto ret = _dispatcher.dispatch(std::move(pTask)); auto clearFuture = ret.share(); @@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() { case STATE::START_BRIDGE: this->_pRosbridge->start(); break; - break; case STATE::WEBSOCKET_DETECTED: resetDone = false; this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP); @@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles( this->_lastCall = CALL_NAME::ADD_TILES; // create json object - rapidjson::Document request(rapidjson::kObjectType); - auto &allocator = request.GetAllocator(); - rapidjson::Value jsonTileArray(rapidjson::kArrayType); + QJsonArray jsonTileArray; for (const auto &tile : *pTileArray) { using namespace ros_bridge::messages; - rapidjson::Value jsonTile(rapidjson::kObjectType); - if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) { + QJsonObject jsonTile; + if (!nemo_msgs::tile::toJson(*tile, jsonTile)) { qCDebug(NemoInterfaceLog) << "addTiles(): not able to create json object: tile id: " << tile->id() << " progress: " << tile->progress() << " points: " << tile->tile(); } - jsonTileArray.PushBack(jsonTile, allocator); + jsonTileArray.append(std::move(jsonTile)); } // for - rapidjson::Value tileKey("in_tile_array"); - request.AddMember(tileKey, jsonTileArray, allocator); - - rapidjson::StringBuffer buffer; - rapidjson::Writer writer(buffer); - request.Accept(writer); - QJsonDocument req = QJsonDocument::fromJson(buffer.GetString()); + QJsonObject req; + req["in_tile_array"] = std::move(jsonTileArray); // create response handler. auto promise_response = std::make_shared>(); auto future_response = promise_response->get_future(); auto responseHandler = [promise_response](const QJsonObject &o) mutable { // check if transaction was successfull - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("values") && d["values"].IsObject()) { - auto values = d["values"].GetObject(); - if (values.HasMember("success") && values["success"].IsBool()) { - promise_response->set_value(values["success"].GetBool()); - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/add_tiles no \"success\" key or wrong type: " << msg; - promise_response->set_value(false); - } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/add_tiles no \"values\" key or wrong type: " << msg; - promise_response->set_value(false); - } + if (o.contains("success") && o["success"].isBool()) { + promise_response->set_value(o["success"].toBool()); } else { - qCWarning(NemoInterfaceLog) << "/nemo/add_tiles message parse error (" - << d.GetParseError() << "): " << msg; + qCWarning(NemoInterfaceLog) + << "/nemo/add_tiles no \"success\" key or wrong type: " << o; promise_response->set_value(false); } }; // call service. - this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, - req.object()); + this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, req); // wait for response. auto tStart = hrc::now(); @@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr pIdArray) { this->_lastCall = CALL_NAME::REMOVE_TILES; // create json object - rapidjson::Document request(rapidjson::kObjectType); - auto &allocator = request.GetAllocator(); - rapidjson::Value jsonIdArray(rapidjson::kArrayType); + QJsonArray jsonIdArray; for (const auto id : *pIdArray) { using namespace ros_bridge::messages; - jsonIdArray.PushBack(rapidjson::Value(id), allocator); + jsonIdArray.append(id); } // for - rapidjson::Value tileKey("ids"); - request.AddMember(tileKey, jsonIdArray, allocator); - - rapidjson::StringBuffer buffer; - rapidjson::Writer writer(buffer); - request.Accept(writer); - QJsonDocument req = QJsonDocument::fromJson(buffer.GetString()); + QJsonObject req; + req["ids"] = std::move(jsonIdArray); // create response handler. auto promise_response = std::make_shared>(); @@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr pIdArray) { auto responseHandler = [promise_response](const QJsonObject &o) mutable { // check if transaction was successfull QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("values") && d["values"].IsObject()) { - auto values = d["values"].GetObject(); - if (values.HasMember("success") && values["success"].IsBool()) { - promise_response->set_value(values["success"].GetBool()); - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/remove_tiles no \"success\" key or wrong type: " << msg; - promise_response->set_value(false); - } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/remove_tiles no \"values\" key or wrong type: " << msg; - promise_response->set_value(false); - } + if (o.contains("success") && o["success"].isBool()) { + promise_response->set_value(o["success"].toBool()); } else { - qCWarning(NemoInterfaceLog) << "/nemo/remove_tiles message parse error (" - << d.GetParseError() << "): " << msg; + qCWarning(NemoInterfaceLog) + << "/nemo/remove_tiles no \"success\" key or wrong type: " << msg; promise_response->set_value(false); } }; // call service. - this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, - req.object()); + this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, req); // wait for response. auto tStart = hrc::now(); @@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() { auto future_response = promise_response->get_future(); auto responseHandler = [promise_response](const QJsonObject &o) mutable { // check if transaction was successfull - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("result") && d["result"].IsBool()) { - promise_response->set_value(d["result"].GetBool()); - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/clear_tiles no \"result\" key or wrong type: " << msg; - promise_response->set_value(false); - } - } else { - qCWarning(NemoInterfaceLog) << "/nemo/clear_tiles message parse error (" - << d.GetParseError() << "): " << msg; - promise_response->set_value(false); - } + promise_response->set_value(true); }; // call service. @@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr pIdArray) { this->_lastCall = CALL_NAME::GET_PROGRESS; // create json object - rapidjson::Document request(rapidjson::kObjectType); - auto &allocator = request.GetAllocator(); - rapidjson::Value jsonIdArray(rapidjson::kArrayType); + QJsonArray jsonIdArray; for (const auto id : *pIdArray) { using namespace ros_bridge::messages; - jsonIdArray.PushBack(rapidjson::Value(id), allocator); + jsonIdArray.append(id); } // for - rapidjson::Value tileKey("ids"); - request.AddMember(tileKey, jsonIdArray, allocator); - - rapidjson::StringBuffer buffer; - rapidjson::Writer writer(buffer); - request.Accept(writer); - QJsonDocument req = QJsonDocument::fromJson(buffer.GetString()); + QJsonObject req; + req["ids"] = std::move(jsonIdArray); // create response handler. typedef std::shared_ptr ResponseType; @@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr pIdArray) { auto future_response = promise_response->get_future(); auto responseHandler = [promise_response](const QJsonObject &o) mutable { // check if transaction was successfull - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("values") && d["values"].IsObject()) { - auto values = d["values"].GetObject(); - ros_bridge::messages::nemo_msgs::progress_array::ProgressArray - progressArrayMsg; - if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( - d["values"], progressArrayMsg)) { - auto pArray = std::make_shared(); - *pArray = std::move(progressArrayMsg.progress_array()); - promise_response->set_value(pArray); - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/get_progress error while creating ProgressArray " - "from json."; - promise_response->set_value(nullptr); - } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/get_progress no \"values\" key or wrong type: " << msg; - promise_response->set_value(nullptr); - } + ros_bridge::messages::nemo_msgs::progress_array::ProgressArray + progressArrayMsg; + if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( + o, progressArrayMsg)) { + auto pArray = std::make_shared(); + *pArray = std::move(progressArrayMsg.progress_array()); + promise_response->set_value(pArray); } else { - qCWarning(NemoInterfaceLog) << "/nemo/get_progress message parse error (" - << d.GetParseError() << "): " << msg; + qCWarning(NemoInterfaceLog) + << "/nemo/get_progress error while creating ProgressArray " + "from json."; promise_response->set_value(nullptr); } }; // call service. - this->_pRosbridge->callService("/nemo/get_progress", responseHandler, - req.object()); + this->_pRosbridge->callService("/nemo/get_progress", responseHandler, req); // wait for response. auto tStart = hrc::now(); @@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() { auto future_response = promise_response->get_future(); auto responseHandler = [promise_response](const QJsonObject &o) mutable { // check if transaction was successfull - QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); - rapidjson::Document d; - d.Parse(msg.toUtf8()); - if (!d.HasParseError()) { - if (d.HasMember("values") && d["values"].IsObject()) { - ros_bridge::messages::nemo_msgs::progress_array::ProgressArray - progressArrayMsg; - if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( - d["values"], progressArrayMsg)) { - auto pArray = std::make_shared(); - *pArray = std::move(progressArrayMsg.progress_array()); - promise_response->set_value(pArray); - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/all_get_progress error while creating ProgressArray " - "from json."; - promise_response->set_value(nullptr); - } - } else { - qCWarning(NemoInterfaceLog) - << "/nemo/all_get_progress no \"values\" key or wrong type: " - << msg; - promise_response->set_value(nullptr); - } + ros_bridge::messages::nemo_msgs::progress_array::ProgressArray + progressArrayMsg; + if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( + o, progressArrayMsg)) { + auto pArray = std::make_shared(); + *pArray = std::move(progressArrayMsg.progress_array()); + promise_response->set_value(pArray); } else { qCWarning(NemoInterfaceLog) - << "/nemo/all_get_progress message parse error (" << d.GetParseError() - << "): " << msg; + << "/nemo/get_all_progress error while creating ProgressArray " + "from json. msg: " + << o; promise_response->set_value(nullptr); } }; diff --git a/src/MeasurementComplexItem/geometry/MeasurementArea.cc b/src/MeasurementComplexItem/geometry/MeasurementArea.cc index d8f6e84b9..c42592307 100644 --- a/src/MeasurementComplexItem/geometry/MeasurementArea.cc +++ b/src/MeasurementComplexItem/geometry/MeasurementArea.cc @@ -19,6 +19,8 @@ #define MAX_TILES 1000 #endif +QString randomId(); + using namespace geometry; namespace trans = bg::strategy::transform; @@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json, // find unique id if (it != _indexMap.end()) { - auto newId = tile->id() + 1; + auto newId = MeasurementTile::randomId(); constexpr long counterMax = 1e6; unsigned long counter = 0; for (; counter <= counterMax; ++counter) { @@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json, if (it == _indexMap.end()) { break; } else { - ++newId; + newId = MeasurementTile::randomId(); } } @@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() { // Convert to geo system. for (const auto &t : tilesENU) { auto geoTile = new MeasurementTile(pData.get()); - std::size_t hashValue = 0; - std::hash hashFun; for (const auto &v : t.outer()) { QGeoCoordinate geoVertex; fromENU(origin, v, geoVertex); geoTile->push_back(geoVertex); - hashValue ^= hashFun(geoVertex); } - geoTile->setId(std::int64_t(hashValue)); pData->append(geoTile); } } @@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() { // find unique id if (it != _indexMap.end()) { - auto newId = tile->id() + 1; + auto newId = MeasurementTile::randomId(); constexpr long counterMax = 1e6; unsigned long counter = 0; for (; counter <= counterMax; ++counter) { @@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() { if (it == _indexMap.end()) { break; } else { - ++newId; + newId = MeasurementTile::randomId(); } } @@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth, return true; } + +QString randomId() { + std::srand(std::time(nullptr)); + std::int64_t r = 0; + + for (int i = 0; i < 10; ++i) { + r ^= std::rand(); + } + + return QString::number(r); +} diff --git a/src/MeasurementComplexItem/geometry/MeasurementArea.h b/src/MeasurementComplexItem/geometry/MeasurementArea.h index 4f061fd1a..4d392076d 100644 --- a/src/MeasurementComplexItem/geometry/MeasurementArea.h +++ b/src/MeasurementComplexItem/geometry/MeasurementArea.h @@ -107,7 +107,7 @@ private: // Tile stuff. TilePtr _tiles; - std::map _indexMap; + std::map _indexMap; QTimer _timer; STATE _state; QFutureWatcher _watcher; diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp index c63ca002c..1d8735330 100644 --- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp +++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp @@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile"; const char *MeasurementTile::nameString = "MeasurementTile"; MeasurementTile::MeasurementTile(QObject *parent) - : GeoArea(parent), _progress(0), _id(0) { + : GeoArea(parent), _progress(0), _id(MeasurementTile::randomId()) { init(); } @@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) { void MeasurementTile::init() { this->setObjectName(nameString); } -int64_t MeasurementTile::id() const { return _id; } +QString MeasurementTile::id() const { return _id; } -void MeasurementTile::setId(const int64_t &id) { +void MeasurementTile::setId(const QString &id) { if (_id != id) { _id = id; emit idChanged(); @@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) { QList MeasurementTile::tile() const { return coordinateList(); } +QString MeasurementTile::randomId(int length) { + static const QString values( + "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789"); + QString str; + std::srand(std::time(nullptr)); + for (int i = 0; i < length; ++i) { + int index = std::rand() % values.length(); + QChar c = values.at(index); + str.append(c); + } + return str; +} + double MeasurementTile::progress() const { return _progress; } void MeasurementTile::setProgress(double progress) { diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h index 3e08e8206..cb0393cc2 100644 --- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h +++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h @@ -24,11 +24,13 @@ public: double progress() const; void setProgress(double progress); - int64_t id() const; - void setId(const int64_t &id); + QString id() const; + void setId(const QString &id); QList tile() const; + static QString randomId(int length = 10); + // Static Variables static const char *settingsGroup; static const char *nameString; @@ -40,5 +42,5 @@ signals: private: void init(); double _progress; - int64_t _id; + QString _id; }; diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp index c31d71206..e1b5e0820 100644 --- a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp @@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() { return this->_taskQueue.size() + (_running ? 1 : 0); } +bool TaskDispatcher::idle() { return this->pendingTasks() == 0; } + void TaskDispatcher::run() { while (true) { ULock lk1(this->_mutex); diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h index d641136bf..20dee8a33 100644 --- a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h @@ -53,6 +53,7 @@ public: bool isRunning(); std::size_t pendingTasks(); + bool idle(); protected: void run(); diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.cpp b/src/MeasurementComplexItem/rosbridge/rosbridge.cpp index e8aac4b95..9c2efe949 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridge.cpp +++ b/src/MeasurementComplexItem/rosbridge/rosbridge.cpp @@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics"; static constexpr auto pollIntervall = std::chrono::milliseconds(200); +Rosbridge::STATE translate(RosbridgeImpl::STATE s); + Rosbridge::Rosbridge(const QUrl url, QObject *parent) : QObject(parent), _url(url), _running(false) { static std::once_flag flag; @@ -34,6 +36,9 @@ void Rosbridge::start() { _impl = new RosbridgeImpl(_url); _impl->moveToThread(_t); + connect(_impl, &RosbridgeImpl::stateChanged, this, + &Rosbridge::_onImplStateChanged); + connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start); connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop); @@ -77,20 +82,7 @@ void Rosbridge::stop() { Rosbridge::STATE Rosbridge::state() { if (_running) { - switch (_impl->state()) { - case RosbridgeImpl::STATE::STOPPED: - case RosbridgeImpl::STATE::STOPPING: - return STATE::STOPPED; - case RosbridgeImpl::STATE::STARTING: - return STATE::STARTED; - case RosbridgeImpl::STATE::CONNECTED: - return STATE::CONNECTED; - case RosbridgeImpl::STATE::TIMEOUT: - return STATE::TIMEOUT; - break; - } - qCritical() << "unreachable branch!"; - return STATE::STOPPED; + return translate(_impl->state()); } else { return STATE::STOPPED; } @@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) { qDebug() << "waitForService: Rosbridge not connected."; } } + +void Rosbridge::_onImplStateChanged() { + static STATE oldState = STATE::STOPPED; + auto newState = translate(_impl->state()); + + if (oldState != newState) { + emit stateChanged(); + } + oldState = newState; +} + +Rosbridge::STATE translate(RosbridgeImpl::STATE s) { + switch (s) { + case RosbridgeImpl::STATE::STOPPED: + case RosbridgeImpl::STATE::STOPPING: + return Rosbridge::STATE::STOPPED; + case RosbridgeImpl::STATE::STARTING: + return Rosbridge::STATE::STARTED; + case RosbridgeImpl::STATE::CONNECTED: + return Rosbridge::STATE::CONNECTED; + case RosbridgeImpl::STATE::TIMEOUT: + return Rosbridge::STATE::TIMEOUT; + break; + } + qCritical() << "unreachable branch!"; + return Rosbridge::STATE::STOPPED; +} diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.h b/src/MeasurementComplexItem/rosbridge/rosbridge.h index 48e88cc61..2be1b8c4f 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridge.h +++ b/src/MeasurementComplexItem/rosbridge/rosbridge.h @@ -101,6 +101,9 @@ signals: void _unadvertiseAllServices(); private: + void _onImplStateChanged(); + + std::atomic _state; RosbridgeImpl *_impl; QThread *_t; QUrl _url; diff --git a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp index 9a7e756ed..25b591d0c 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp +++ b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp @@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { auto it = _advertisedTopics.find(topic); if (Q_LIKELY(it != _advertisedTopics.end())) { - _advertisedTopics.erase(it); QJsonObject o; o[opKey] = unadvertiseOpKey; @@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) { QString payload = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); _webSocket.sendTextMessage(payload); + + _advertisedTopics.erase(it); } else { qDebug() << "Topic " << topic << " not advertised."; } @@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { auto it = _subscribedTopics.find(topic); if (Q_LIKELY(it != _subscribedTopics.end())) { - _subscribedTopics.erase(it); QJsonObject o; o[opKey] = unsubscribeOpKey; @@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) { QString payload = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); _webSocket.sendTextMessage(payload); + + _subscribedTopics.erase(it); } else { qDebug() << "unsubscribeTopic: topic " << topic << " already subscribed!"; } @@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { auto it = _advertisedServices.find(service); if (it != _advertisedServices.end()) { - it = _advertisedServices.erase(it); QJsonObject o; o[opKey] = unadvertiseServiceOpKey; @@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) { QString payload = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); _webSocket.sendTextMessage(payload); + + it = _advertisedServices.erase(it); } else { qDebug() << "unadvertiseService: Service " << service << " not advertised."; @@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() { } void RosbridgeImpl::_onTextMessageReceived(const QString &message) { + qDebug() << "_onTextMessageReceived: " << message; QJsonParseError e; auto d = QJsonDocument::fromJson(message.toUtf8(), &e); if (!d.isNull()) { diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h index 6a5e90e2a..7458cda8e 100644 --- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h @@ -3,147 +3,140 @@ #include #include "ros_bridge/include/message_traits.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" + +#include +#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. +//! @brief Namespace containing classes and methodes for geometry_msgs +//! generation. namespace geographic_msgs { -//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation. +//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message +//! generation. namespace geo_point { std::string messageType(); +namespace { +const char *lonKey = "longitude"; +const char *latKey = "latitude"; +const char *altKey = "altitude"; + +} // namespace + //! @brief C++ representation of geographic_msgs/GeoPoint. -template -class GenericGeoPoint{ +template +class GenericGeoPoint { public: - GenericGeoPoint() - : _latitude(0) - , _longitude(0) - , _altitude(0) - {} - GenericGeoPoint(const GenericGeoPoint &other) - : _latitude(other.latitude()) - , _longitude(other.longitude()) - , _altitude(other.altitude()) - {} - GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude) - : _latitude(latitude) - , _longitude(longitude) - , _altitude(altitude) - {} - - FloatType latitude() const {return _latitude;} - FloatType longitude() const {return _longitude;} - FloatType altitude() const {return _altitude;} - - void setLatitude (FloatType latitude) {_latitude = latitude;} - void setLongitude (FloatType longitude) {_longitude = longitude;} - void setAltitude (FloatType altitude) {_altitude = altitude;} - - bool operator==(const GenericGeoPoint &p1) { - return ( p1._latitude == this->_latitude - && p1._longitude == this->_longitude - && p1._altitude == this->_altitude); - } - - bool operator!=(const GenericGeoPoint &p1) { - return !this->operator==(p1); - } - - friend OStream& operator<<(OStream& os, const GenericGeoPoint& p) - { - os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude; - return os; - } + GenericGeoPoint() : _latitude(0), _longitude(0), _altitude(0) {} + GenericGeoPoint(const GenericGeoPoint &other) + : _latitude(other.latitude()), _longitude(other.longitude()), + _altitude(other.altitude()) {} + GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude) + : _latitude(latitude), _longitude(longitude), _altitude(altitude) {} + + FloatType latitude() const { return _latitude; } + FloatType longitude() const { return _longitude; } + FloatType altitude() const { return _altitude; } + + void setLatitude(FloatType latitude) { _latitude = latitude; } + void setLongitude(FloatType longitude) { _longitude = longitude; } + void setAltitude(FloatType altitude) { _altitude = altitude; } + + bool operator==(const GenericGeoPoint &p1) { + return (p1._latitude == this->_latitude && + p1._longitude == this->_longitude && + p1._altitude == this->_altitude); + } + + bool operator!=(const GenericGeoPoint &p1) { return !this->operator==(p1); } + + friend OStream &operator<<(OStream &os, const GenericGeoPoint &p) { + os << "lat: " << p._latitude << " lon: " << p._longitude + << " alt: " << p._altitude; + return os; + } private: - FloatType _latitude; - FloatType _longitude; - FloatType _altitude; + FloatType _latitude; + FloatType _longitude; + FloatType _altitude; }; typedef GenericGeoPoint<> GeoPoint; namespace detail { - template - auto getAltitude(const T &p, traits::Type2Type) - { - return p.altitude(); - } - - template - auto getAltitude(const T &p, traits::Type2Type) - { - (void)p; - return 0.0; - } - - template - void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type) - { - p.setAltitude(doc.GetFloat()); - } - - template - void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type) - { - (void)doc; - (void)p; - } -} // namespace detail +template +auto getAltitude(const T &p, traits::Type2Type) { + return p.altitude(); +} + +template +auto getAltitude(const T &p, traits::Type2Type) { + (void)p; + return 0.0; +} + +template +void setAltitude(const QJsonValue &o, T &p, + traits::Type2Type) { + p.setAltitude(o.toDouble()); +} template -bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) -{ - value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator); - value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator); - - typedef typename traits::Select< - traits::HasMemberAltitude::value, - traits::Has3Components, - traits::Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - auto altitude = detail::getAltitude(p, traits::Type2Type()); // If T has no member altitude() replace it by 0.0; - - value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); - return true; +void setAltitude(const QJsonValue &o, T &p, + traits::Type2Type) { + (void)o; + (void)p; } +} // namespace detail + +template bool toJson(const T &p, QJsonObject &value) { + value[latKey] = p.latitude(); + value[lonKey] = p.longitude(); + typedef typename traits::Select::value, + traits::Has3Components, + traits::Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + auto altitude = detail::getAltitude( + p, traits::Type2Type()); // If T has no member altitude() + // replace it by 0.0; + value[altKey] = altitude; + return true; +} template -bool fromJson(const rapidjson::Value &value, - PointType &p) { - if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){ - assert(false); - return false; - } - if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){ - assert(false); - return false; - } - if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){ - assert(false); - return false; - } - - p.setLatitude(value["latitude"].GetFloat()); - p.setLongitude(value["longitude"].GetFloat()); - typedef typename traits::Select< - traits::HasMemberSetAltitude::value, - traits::Has3Components, - traits::Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - detail::setAltitude(value["altitude"], p, traits::Type2Type()); // If T has no member altitude() discard doc["altitude"]; - - return true; +bool fromJson(const QJsonObject &value, PointType &p) { + if (!value.contains("latitude") || !value["latitude"].isDouble()) { + return false; + } + if (!value.contains("longitude") || !value["longitude"].isDouble()) { + return false; + } + if (!value.contains("altitude") || !value["altitude"].isDouble()) { + return false; + } + + p.setLatitude(value["latitude"].toDouble()); + p.setLongitude(value["longitude"].toDouble()); + typedef + typename traits::Select::value, + traits::Has3Components, + traits::Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + detail::setAltitude( + value["altitude"], p, + traits::Type2Type()); // If T has no member altitude() discard + // doc["altitude"]; + + return true; } // namespace detail - -} // namespace geopoint -} // namespace geometry_msgs +} // namespace geo_point +} // namespace geographic_msgs } // namespace messages } // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp deleted file mode 100644 index 2736636ed..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "point32.h" - -std::string ros_bridge::messages::geometry_msgs::point32::messageType(){ - return "geometry_msgs/Point32"; -} - diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h deleted file mode 100644 index c6e6865fd..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h +++ /dev/null @@ -1,139 +0,0 @@ -#pragma once - -#include "ros_bridge/include/message_traits.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" - -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 geometry_msgs { -//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation. -namespace point32 { - - -std::string messageType(); - -namespace detail { -using namespace ros_bridge::traits; - -template -auto getZ(const T &p, Type2Type) -{ - return p.z(); -} - -template -auto getZ(const T &p, Type2Type) -{ - (void)p; - return 0.0; // p has no member z() -> add 0. -} - -template -bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) -{ - p.setZ(doc["z"].GetFloat()); - return true; -} - -template -bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) -{ - (void)doc; - (void)p; - return true; -} - -} // namespace detail - -//! @brief C++ representation of a geometry_msgs/Point/Point32 -template -class GenericPoint { -public: - GenericPoint(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){} - - FloatType x() const {return _x;} - FloatType y() const {return _y;} - FloatType z() const {return _z;} - - void setX(FloatType x) {_x = x;} - void setY(FloatType y) {_y = y;} - void setZ(FloatType z) {_z = z;} - - - - bool operator==(const GenericPoint &p1) { - return (p1._x == this->_x - && p1._y == this->_y - && p1._z == this->_z); - } - - bool operator!=(const GenericPoint &p1) { - return !this->operator==(p1); - } - - friend OStream& operator<<(OStream& os, const GenericPoint& p) - { - os << "x: " << p._x << " y: " << p._y<< " z: " << p._z; - return os; - } - -private: - FloatType _x; - FloatType _y; - FloatType _z; -}; - -typedef GenericPoint<> Point; -typedef GenericPoint<_Float32> Point32; - - - -template -bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) -{ - using namespace ros_bridge::traits; - - value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator); - value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator); - - typedef typename Select::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - auto z = detail::getZ(p, Type2Type()); // If T has no member z() replace it by 0. - - value.AddMember("z", rapidjson::Value().SetFloat(z), allocator); - return true; -} - -template -bool fromJson(const rapidjson::Value &value, - PointType &p) { - using namespace ros_bridge::traits; - - if (!value.HasMember("x") || !value["x"].IsFloat()){ - assert(false); - return false; - } - if (!value.HasMember("y") || !value["y"].IsFloat()){ - assert(false); - return false; - } - if (!value.HasMember("z") || !value["z"].IsFloat()){ - assert(false); - return false; - } - - p.setX(value["x"].GetFloat()); - p.setY(value["y"].GetFloat()); - typedef typename Select::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - (void)detail::setZ(value["z"], p, Type2Type()); // If PointType has no member z() discard doc["z"]. - - return true; -} - -} // namespace point32 -} // namespace geometry_msgs -} // namespace messages -} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp deleted file mode 100644 index 9c67ca377..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "polygon.h" - -std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){ - return "geometry_msgs/Polygon"; -} - diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h deleted file mode 100644 index e5d815c10..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/include/messages/geometry_msgs/point32.h" - -#include -#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 geometry_msgs { -//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation. -namespace polygon { - - -std::string messageType(); - -//! @brief C++ representation of geometry_msgs/Polygon -template class ContainerType = std::vector> -class GenericPolygon { - typedef typename std::remove_cv_t> PointType; -public: - GenericPolygon() {} - GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){} - - - const ContainerType &points() const {return _points;} - ContainerType &points() {return _points;} - - GenericPolygon &operator=(const GenericPolygon &other) { - this->_points = other._points; - return *this; - } - -private: - ContainerType _points; -}; -typedef GenericPolygon Polygon; - - -template -bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) -{ - using namespace geometry_msgs::point32; - - rapidjson::Value points(rapidjson::kArrayType); - - for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) { - rapidjson::Document point(rapidjson::kObjectType); - if ( !point32::toJson(poly.points()[i], point, allocator) ){ - assert(false); - return false; - } - points.PushBack(point, allocator); - } - - value.AddMember("points", points, allocator); - return true; -} - -template -bool fromJson(const rapidjson::Value &value, PolygonType &poly) -{ - using namespace geometry_msgs::point32; - - if (!value.HasMember("points") || !value["points"].IsArray()){ - assert(false); - return false; - } - const auto &jsonArray = value["points"].GetArray(); - poly.points().clear(); - poly.points().reserve(jsonArray.Size()); - typedef decltype (poly.points()[0]) PointTypeCVR; - typedef typename std::remove_cv_t> PointType; - for (long i=0; i < jsonArray.Size(); ++i){ - PointType pt; - if ( !point32::fromJson(jsonArray[i], pt) ){ - assert(false); - return false; - } - poly.points().push_back(std::move(pt)); - } - return true; -} - -} // namespace polygon -} // namespace geometry_msgs -} // namespace messages -} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp deleted file mode 100644 index 0e974bca7..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "polygon_stamped.h" - -std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){ - return "geometry_msgs/PolygonStamped"; -} - diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h deleted file mode 100644 index 49dfbff57..000000000 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h +++ /dev/null @@ -1,172 +0,0 @@ -#pragma once - -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/include/messages/geometry_msgs/polygon.h" -#include "ros_bridge/include/messages/std_msgs/header.h" -#include "ros_bridge/include/message_traits.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 geometry_msgs { -//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation. -namespace polygon_stamped { - - -std::string messageType(); - -//! @brief C++ representation of geometry_msgs/PolygonStamped -template -class GenericPolygonStamped { - typedef PolygonType Polygon; -public: - GenericPolygonStamped(){} - GenericPolygonStamped(const GenericPolygonStamped &other) : - _header(other.header()) - , _polygon(other.polygon()) - {} - GenericPolygonStamped(const HeaderType &header, Polygon &polygon) : - _header(header) - , _polygon(polygon) - {} - - - const HeaderType &header() const {return _header;} - const Polygon &polygon() const {return _polygon;} - - HeaderType &header() {return _header;} - Polygon &polygon() {return _polygon;} - -private: - HeaderType _header; - Polygon _polygon; -}; - -typedef GenericPolygonStamped<> PolygonStamped; - -// =================================================================================== -namespace detail { - -template -bool setHeader(const rapidjson::Value &doc, - PolygonStampedType &polyStamped, - traits::Int2Type<1>) { // polyStamped.header() exists - using namespace std_msgs; - - typedef decltype (polyStamped.header()) HeaderTypeCVR; - typedef typename std::remove_cv_t> HeaderType; - HeaderType header; - bool ret = header::fromJson(doc, header); - polyStamped.header() = header; - return ret; -} -template -bool setHeader(const rapidjson::Value &doc, - PolygonStampedType &polyStamped, - traits::Int2Type<0>) { // polyStamped.header() does not exists - (void)doc; - (void)polyStamped; - return true; -} - - -template -bool _toJson(const PolygonType &poly, - const HeaderType &h, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) -{ - using namespace std_msgs; - using namespace geometry_msgs; - - rapidjson::Document header(rapidjson::kObjectType); - if (!header::toJson(h, header, allocator)){ - assert(false); - return false; - } - rapidjson::Document polygon(rapidjson::kObjectType); - if (!polygon::toJson(poly, polygon, allocator)){ - assert(false); - return false; - } - value.AddMember("header", header, allocator); - value.AddMember("polygon", polygon, allocator); - - return true; -} - -template -bool _toJson(const PolyStamped &polyStamped, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator, - traits::Int2Type){ // PolyStamped has member header(), use integraded header. -return _toJson(polyStamped, polyStamped.header(), value, allocator); -} - -template -bool _toJson(const PolyStamped &polyStamped, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator, - traits::Int2Type<0>){ // PolyStamped has no member header(), generate one on the fly. -using namespace std_msgs::header; -return _toJson(polyStamped, Header(), value, allocator); -} - -} // namespace detail -// =================================================================================== - - -template -bool toJson(const PolygonType &poly, - const HeaderType &h, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) -{ - return detail::_toJson(poly, h, value, allocator); -} - -template -bool toJson(const PolyStamped &polyStamped, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) -{ - typedef traits::HasMemberHeader HasHeader; - return detail::_toJson(polyStamped, value, allocator, traits::Int2Type()); -} - -template -bool fromJson(const rapidjson::Value &value, PolygonType &polyStamped) -{ - using namespace geometry_msgs::polygon; - - if ( !value.HasMember("header") ){ - assert(false); - return false; - } - if ( !value.HasMember("polygon") ){ - assert(false); - return false; - } - - typedef traits::HasMemberSetHeader HasHeader; - if ( !detail::setHeader(value["header"], polyStamped, traits::Int2Type())){ - assert(false); - return false; - } - - if ( !polygon::fromJson(value["polygon"], polyStamped.polygon()) ){ - assert(false); - return false; - } - - return true; -} - -} // namespace polygon_stamped -} // namespace geometry_msgs -} // namespace messages -} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h index 6c9065060..01de4edb0 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -1,6 +1,6 @@ #pragma once -#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include namespace ros_bridge { //! @brief Namespace containing classes and methodes ros message generation. @@ -32,19 +32,18 @@ protected: }; template -bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) { - value.AddMember("status", std::int32_t(heartbeat.status()), allocator); +bool toJson(const HeartbeatType &heartbeat, QJsonObject &value) { + value["status"] = heartbeat.status(); return true; } template -bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) { - if (!value.HasMember("status") || !value["status"].IsInt()) { +bool fromJson(const QJsonObject &value, HeartbeatType &heartbeat) { + if (!value.contains("status") || !value["status"].isDouble()) { return false; } - heartbeat.setStatus(value["status"].GetInt()); + heartbeat.setStatus(value["status"].toInt()); return true; } 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 index 7718eefea..7c8532348 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp @@ -1,5 +1,5 @@ #include "labeled_progress.h" -std::string ros_bridge::messages::nemo_msgs::labeled_progress::messageType() { +QString 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 index caff7f369..1fa4ccddc 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h @@ -1,8 +1,9 @@ #pragma once -#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include -#include +#include +#include namespace ros_bridge { //! @brief Namespace containing classes and methodes ros message generation. @@ -14,7 +15,7 @@ namespace nemo_msgs { //! generation. namespace labeled_progress { -std::string messageType(); +QString messageType(); namespace { const char *progressKey = "progress"; @@ -24,40 +25,39 @@ const char *idKey = "id"; //! @brief C++ representation of nemo_msgs/labeled_progress message class LabeledProgress { public: - LabeledProgress() : _id(0), _progress(0) {} - LabeledProgress(double progress, long id) : _id(id), _progress(progress) {} + LabeledProgress() : _id(""), _progress(0) {} + LabeledProgress(double progress, QString id) : _id(id), _progress(progress) {} - long id() const { return _id; } - void setId(long id) { _id = id; } + QString id() const { return _id; } + void setId(QString id) { _id = id; } double progress() const { return _progress; } void setProgress(double progress) { _progress = progress; } protected: - long _id; + QString _id; double _progress; }; template -bool toJson(const LabeledProgressType &lp, rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) { - value.AddMember(idKey, lp.id(), allocator); - value.AddMember(progressKey, lp.progress(), allocator); +bool toJson(const LabeledProgressType &lp, QJsonObject &value) { + value[idKey] = lp.id(); + value[progressKey] = lp.progress(); return true; } template -bool fromJson(const rapidjson::Value &value, ProgressType &p) { - if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) { +bool fromJson(const QJsonObject &value, ProgressType &p) { + if (!value.contains(progressKey) || !value[progressKey].isDouble()) { return false; } - if (!value.HasMember(idKey) || !value[idKey].IsInt64()) { + if (!value.contains(idKey) || !value[idKey].isString()) { return false; } - p.setId(value[idKey].GetInt64()); - p.setProgress(value[progressKey].GetDouble()); + p.setId(value[idKey].toString()); + p.setProgress(value[progressKey].toDouble()); return true; } diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h index e3d3299ab..25938b4b8 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h @@ -4,8 +4,9 @@ #pragma once #include "labeled_progress.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include +#include #include namespace ros_bridge { @@ -43,34 +44,33 @@ protected: typedef GenericProgressArray<> ProgressArray; -template -bool toJson(const Array &array, rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) { - rapidjson::Value jsonArray(rapidjson::kArrayType); +template bool toJson(const Array &array, QJsonObject &value) { + QJsonArray jsonArray; + for (const auto &lp : array.progress_array()) { - rapidjson::Value jsonLp; - if (labeled_progress::toJson(lp, jsonLp, allocator)) { - jsonArray.PushBack(jsonLp, allocator); + QJsonObject jsonLp; + if (labeled_progress::toJson(lp, jsonLp)) { + jsonArray.append(std::move(jsonLp)); } else { return false; } } - value.AddMember(rapidjson::Value(progressArrayKey), jsonArray, allocator); + value[progressArrayKey] = std::move(jsonArray); return true; } -template bool fromJson(const rapidjson::Value &value, Array &p) { - if (!value.HasMember(progressArrayKey) || - !value[progressArrayKey].IsArray()) { +template bool fromJson(const QJsonObject &value, Array &p) { + if (!value.contains(progressArrayKey) || !value[progressArrayKey].isArray()) { return false; } - const auto &jsonArray = value[progressArrayKey].GetArray(); + const auto jsonArray = value[progressArrayKey].toArray(); p.progress_array().clear(); - p.progress_array().reserve(jsonArray.Size()); - for (long i = 0; i < jsonArray.Size(); ++i) { + p.progress_array().reserve(jsonArray.size()); + for (long i = 0; i < jsonArray.size(); ++i) { labeled_progress::LabeledProgress lp; - if (!labeled_progress::fromJson(jsonArray[i], lp)) { + if (!jsonArray[i].isObject() || + !labeled_progress::fromJson(jsonArray[i].toObject(), lp)) { return false; } else { p.progress_array().push_back(lp); diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp index b50910138..6fc3f935f 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp @@ -1,5 +1,5 @@ #include "tile.h" -std::string ros_bridge::messages::nemo_msgs::tile::messageType() { +QString ros_bridge::messages::nemo_msgs::tile::messageType() { return "nemo_msgs/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 index 246a85e43..7c7c59383 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h @@ -1,10 +1,12 @@ #pragma once #include "ros_bridge/include/messages/geographic_msgs/geopoint.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" #include +#include +#include + namespace ros_bridge { //! @brief Namespace containing classes and methodes ros message generation. namespace messages { @@ -15,7 +17,7 @@ namespace nemo_msgs { //! generation. namespace tile { -std::string messageType(); +QString messageType(); namespace { const char *progressKey = "progress"; @@ -30,11 +32,11 @@ class GenericTile { public: typedef Container GeoContainer; GenericTile() {} - GenericTile(const GeoContainer &tile, double progress, long id) + GenericTile(const GeoContainer &tile, double progress, const QString &id) : _tile(tile), _id(id), _progress(progress) {} - long id() const { return _id; } - void setId(long id) { _id = id; } + QString id() const { return _id; } + void setId(const QString &id) { _id = id; } double progress() const { return _progress; } void setProgress(double progress) { _progress = progress; } @@ -45,63 +47,58 @@ public: protected: GeoContainer _tile; - long _id; + QString _id; double _progress; }; typedef GenericTile<> Tile; template -bool toJson(const TileType &tile, rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) { +bool toJson(const TileType &tile, QJsonObject &value) { - using namespace rapidjson; - value.AddMember(Value(idKey, allocator), Value(tile.id()), allocator); - value.AddMember(Value(progressKey, allocator), Value(tile.progress()), - allocator); + value[idKey] = tile.id(); + value[progressKey] = tile.progress(); using namespace messages::geographic_msgs; - rapidjson::Value geoPoints(rapidjson::kArrayType); + QJsonArray geoPoints; for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) { - rapidjson::Value geoPoint(rapidjson::kObjectType); - if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) { + QJsonObject geoPoint; + if (!geo_point::toJson(tile.tile()[i], geoPoint)) { return false; } - geoPoints.PushBack(geoPoint, allocator); + geoPoints.append(std::move(geoPoint)); } - rapidjson::Value key(tileKey, allocator); - value.AddMember(key, geoPoints, allocator); + value[tileKey] = std::move(geoPoints); return true; } -template -bool fromJson(const rapidjson::Value &value, TileType &p) { - if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) { +template bool fromJson(const QJsonObject &value, TileType &p) { + if (!value.contains(progressKey) || !value[progressKey].isDouble()) { return false; } - if (!value.HasMember(idKey) || !value[idKey].IsInt()) { + if (!value.contains(idKey) || !value[idKey].isString()) { return false; } - if (!value.HasMember(tileKey) || !value[tileKey].IsArray()) { + if (!value.contains(tileKey) || !value[tileKey].isArray()) { return false; } - p.setId(value[idKey].GetInt()); - p.setProgress(value[progressKey].GetDouble()); + p.setId(value[idKey].toString()); + p.setProgress(value[progressKey].toDouble()); using namespace geographic_msgs; - const auto &jsonArray = value[tileKey].GetArray(); + const auto jsonArray = value[tileKey].toArray(); p.tile().clear(); - p.tile().reserve(jsonArray.Size()); + p.tile().reserve(jsonArray.size()); typedef decltype(p.tile()[0]) PointTypeCVR; typedef typename std::remove_cv_t> PointType; - for (long i = 0; i < jsonArray.Size(); ++i) { + for (long i = 0; i < jsonArray.size(); ++i) { PointType pt; if (!geo_point::fromJson(jsonArray[i], pt)) { return false; diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp deleted file mode 100644 index bd9cc9467..000000000 --- a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "header.h" - -std::uint32_t ros_bridge::messages::std_msgs::header::Header::_defaultSeq = 0; - -ros_bridge::messages::std_msgs::header::Header::Header() - : _seq(_defaultSeq++), - _stamp(Time()), - _frameId("") -{} - -ros_bridge::messages::std_msgs::header::Header::Header( - uint32_t seq, - const ros_bridge::messages::std_msgs::header::Header::Time &stamp, - const std::string &frame_id) - : _seq(seq) - , _stamp(stamp) - , _frameId(frame_id) {} - -ros_bridge::messages::std_msgs::header::Header::Header( - const ros_bridge::messages::std_msgs::header::Header &header) - : _seq(header.seq()) - , _stamp(header.stamp()) - , _frameId(header.frameId()) {} - -uint32_t ros_bridge::messages::std_msgs::header::Header::seq() const -{ - return _seq; -} - -const ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp() const -{ - return _stamp; -} - -const std::string &ros_bridge::messages::std_msgs::header::Header::frameId() const -{ - return _frameId; -} - -ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp() -{ - return _stamp; -} - -std::string &ros_bridge::messages::std_msgs::header::Header::frameId() -{ - return _frameId; -} - -void ros_bridge::messages::std_msgs::header::Header::setSeq(uint32_t seq) -{ - _seq = seq; -} - -void ros_bridge::messages::std_msgs::header::Header::setStamp( - const ros_bridge::messages::std_msgs::header::Header::Time &stamp) -{ - _stamp = stamp; -} - -void ros_bridge::messages::std_msgs::header::Header::setFrameId( - const std::string &frameId) -{ - _frameId = frameId; -} - -std::string ros_bridge::messages::std_msgs::header::messageType(){ - return "std_msgs/Header"; -} diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h deleted file mode 100644 index e3a6546a0..000000000 --- a/src/comm/ros_bridge/include/messages/std_msgs/header.h +++ /dev/null @@ -1,102 +0,0 @@ -#pragma once - -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/include/messages/std_msgs/time.h" - -#include - -namespace ros_bridge { -//! @brief Namespace containing classes and methodes ros message generation. -namespace messages { -//! @brief Namespace containing classes and methodes for std_msgs generation. -namespace std_msgs { -//! @brief Namespace containing classes and methodes for std_msgs/Header message generation. -namespace header { - - -std::string messageType(); - -//! @brief C++ representation of std_msgs/Header -class Header{ -public: - using Time = std_msgs::time::Time; - Header(); - Header(uint32_t seq, const Time &stamp, const std::string &frame_id); - Header(const Header &header); - - uint32_t seq() const; - const Time &stamp() const; - const std::string &frameId() const; - - Time &stamp(); - std::string &frameId(); - - void setSeq (uint32_t seq); - void setStamp (const Time &stamp); - void setFrameId (const std::string &frameId); -private: - static uint32_t _defaultSeq; - uint32_t _seq; - Time _stamp; - std::string _frameId; -}; - -template -bool toJson(const HeaderType &header, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) -{ - using namespace messages::std_msgs; - - value.AddMember("seq", rapidjson::Value().SetUint(uint32_t(header.seq())), allocator); - - rapidjson::Value stamp(rapidjson::kObjectType); - if (!time::toJson(header.stamp(), stamp, allocator)){ - assert(false); - return false; - } - value.AddMember("stamp", stamp, allocator); - - value.AddMember("frame_id", - rapidjson::Value().SetString(header.frameId().data(), - header.frameId().length(), - allocator), - allocator); - - return true; -} - -template -bool fromJson(const rapidjson::Value &value, - HeaderType &header) -{ - using namespace messages::std_msgs; - - if (!value.HasMember("seq")|| !value["seq"].IsUint()){ - assert(false); - return false; - } - if (!value.HasMember("stamp")){ - assert(false); - return false; - } - if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){ - assert(false); - return false; - } - header.setSeq(value["seq"].GetUint()); - decltype(header.stamp()) time; - if (!time::fromJson(value["stamp"], time)){ - assert(false); - return false; - } - header.setStamp(time); - header.setFrameId(value["frame_id"].GetString()); - - return true; -} - -} // namespace time -} // namespace std_msgs -} // namespace messages -} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.cpp b/src/comm/ros_bridge/include/messages/std_msgs/time.cpp deleted file mode 100644 index 7d41fe8ff..000000000 --- a/src/comm/ros_bridge/include/messages/std_msgs/time.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "time.h" - -std::string ros_bridge::messages::std_msgs::time::messageType(){ - return "std_msgs/Time"; -} diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.h b/src/comm/ros_bridge/include/messages/std_msgs/time.h deleted file mode 100644 index c3f721614..000000000 --- a/src/comm/ros_bridge/include/messages/std_msgs/time.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "ros_bridge/rapidjson/include/rapidjson/document.h" - -namespace ros_bridge { -//! @brief Namespace containing classes and methodes ros message generation. -namespace messages { -//! @brief Namespace containing classes and methodes for std_msgs generation. -namespace std_msgs { -//! @brief Namespace containing classes and methodes for std_msgs/Time message generation. -namespace time { - -std::string messageType(); - -//! @brief C++ representation of std_msgs/Time -class Time{ -public: - Time(): _secs(0), _nsecs(0) {} - Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {} - Time(const Time &time): _secs(time.secs()), _nsecs(time.nSecs()) {} - - uint32_t secs() const {return _secs;} - uint32_t nSecs() const {return _nsecs;} - - void setSecs(uint32_t secs) {_secs = secs;} - void setNSecs(uint32_t nsecs) {_nsecs = nsecs;} - -private: - uint32_t _secs; - uint32_t _nsecs; -}; - -template -bool toJson(const TimeType &time, - rapidjson::Value &value, - rapidjson::Document::AllocatorType &allocator) -{ - value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator); - value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator); - - return true; -} - -template -bool fromJson(const rapidjson::Value &value, - TimeType &time) -{ - if (!value.HasMember("secs") || !value["secs"].IsUint()){ - assert(false); - return false; - } - if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){ - assert(false); - return false; - } - time.setSecs(value["secs"].GetUint()); - time.setNSecs(value["nsecs"].GetUint()); - - return true; -} - -} // namespace time -} // namespace std_msgs -} // namespace messages -} // namespace ros_bridge -- 2.22.0