Commit b098b99a authored by Valentin Platzgummer's avatar Valentin Platzgummer

tile id int64_t > QString, for robustness

parent 9e620065
...@@ -517,8 +517,6 @@ HEADERS += \ ...@@ -517,8 +517,6 @@ HEADERS += \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ 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/progress_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.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 src/comm/utilities.h
contains (DEFINES, QGC_ENABLE_PAIRING) { contains (DEFINES, QGC_ENABLE_PAIRING) {
...@@ -560,8 +558,6 @@ SOURCES += \ ...@@ -560,8 +558,6 @@ SOURCES += \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \ 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/progress_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.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 \ src/Settings/WimaSettings.cc \
contains (DEFINES, QGC_ENABLE_PAIRING) { contains (DEFINES, QGC_ENABLE_PAIRING) {
......
...@@ -3,6 +3,6 @@ ...@@ -3,6 +3,6 @@
#include <QVector> #include <QVector>
typedef QVector<std::int64_t> IDArray; typedef QVector<QString> IDArray;
#endif // IDARRAY_H #endif // IDARRAY_H
...@@ -10,7 +10,7 @@ ...@@ -10,7 +10,7 @@
#include "SettingsFact.h" #include "SettingsFact.h"
#include "AreaData.h" #include "AreaData.h"
#include "ProgressArray.h" #include "geometry/ProgressArray.h"
class RoutingThread; class RoutingThread;
class RoutingResult; class RoutingResult;
......
...@@ -9,6 +9,8 @@ ...@@ -9,6 +9,8 @@
#include <shared_mutex> #include <shared_mutex>
#include <QJsonArray>
#include <QJsonObject>
#include <QTimer> #include <QTimer>
#include <QUrl> #include <QUrl>
...@@ -24,9 +26,6 @@ ...@@ -24,9 +26,6 @@
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" #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/progress_array.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.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" #include "rosbridge/rosbridge.h"
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
...@@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr<Rosbridge>; ...@@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr<Rosbridge>;
typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate, typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
QList> QList>
Tile; Tile;
typedef std::map<std::int64_t, std::shared_ptr<Tile>> TileMap; typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
typedef std::map<std::int64_t, std::shared_ptr<const Tile>> TileMapConst; typedef std::map<QString, std::shared_ptr<const Tile>> TileMapConst;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::TaskDispatcher Dispatcher; typedef nemo_interface::TaskDispatcher Dispatcher;
typedef nemo_interface::FutureWatcher<QVariant, std::shared_future> typedef nemo_interface::FutureWatcher<QVariant, std::shared_future>
...@@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() { ...@@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
using namespace ros_bridge::messages; using namespace ros_bridge::messages;
// Subscribe nemo progress. // Subscribe nemo progress.
this->_pRosbridge->subscribeTopic(progressTopic, [this]( this->_pRosbridge->subscribeTopic(
const QJsonObject &o) { 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
nemo_msgs::progress_array::ProgressArray progressArray; 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 // correct range errors of progress
for (auto &lp : progressArray.progress_array()) { for (auto &lp : progressArray.progress_array()) {
...@@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() { ...@@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
} else { } else {
qCWarning(NemoInterfaceLog) << "/nemo/progress not able to " qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
"create ProgressArray form json: " "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. // Subscribe heartbeat msg.
this->_pRosbridge->subscribeTopic(heartbeatTopic, [this]( this->_pRosbridge->subscribeTopic(
const QJsonObject &o) { 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
nemo_msgs::heartbeat::Heartbeat heartbeat; nemo_msgs::heartbeat::Heartbeat heartbeat;
if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) { if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) {
std::promise<bool> promise; std::promise<bool> promise;
auto future = promise.get_future(); auto future = promise.get_future();
bool value = QMetaObject::invokeMethod( bool value = QMetaObject::invokeMethod(
...@@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() { ...@@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
} else { } else {
qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to " qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
"create Heartbeat form json: " "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() { void NemoInterface::Impl::_trySynchronize() {
...@@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() { ...@@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() {
this->_state == STATE::USER_SYNC) && this->_state == STATE::USER_SYNC) &&
!_isSynchronized()) { !_isSynchronized()) {
if (!_dispatcher.idle()) {
QTimer::singleShot(5000, [this] { this->_trySynchronize(); });
return;
}
qCWarning(NemoInterfaceLog) << "trying to synchronize"; qCWarning(NemoInterfaceLog) << "trying to synchronize";
this->_setState(STATE::SYS_SYNC); this->_setState(STATE::SYS_SYNC);
...@@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() { ...@@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() {
std::bind(&Impl::_callClearTiles, this)); std::bind(&Impl::_callClearTiles, this));
// dispatch command. // dispatch command.
_dispatcher.clear();
_dispatcher.stop();
Q_ASSERT(_dispatcher.pendingTasks() == 0); Q_ASSERT(_dispatcher.pendingTasks() == 0);
auto ret = _dispatcher.dispatch(std::move(pTask)); auto ret = _dispatcher.dispatch(std::move(pTask));
auto clearFuture = ret.share(); auto clearFuture = ret.share();
...@@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() { ...@@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() {
case STATE::START_BRIDGE: case STATE::START_BRIDGE:
this->_pRosbridge->start(); this->_pRosbridge->start();
break; break;
break;
case STATE::WEBSOCKET_DETECTED: case STATE::WEBSOCKET_DETECTED:
resetDone = false; resetDone = false;
this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP); this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
...@@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles( ...@@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles(
this->_lastCall = CALL_NAME::ADD_TILES; this->_lastCall = CALL_NAME::ADD_TILES;
// create json object // create json object
rapidjson::Document request(rapidjson::kObjectType); QJsonArray jsonTileArray;
auto &allocator = request.GetAllocator();
rapidjson::Value jsonTileArray(rapidjson::kArrayType);
for (const auto &tile : *pTileArray) { for (const auto &tile : *pTileArray) {
using namespace ros_bridge::messages; using namespace ros_bridge::messages;
rapidjson::Value jsonTile(rapidjson::kObjectType); QJsonObject jsonTile;
if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) { if (!nemo_msgs::tile::toJson(*tile, jsonTile)) {
qCDebug(NemoInterfaceLog) qCDebug(NemoInterfaceLog)
<< "addTiles(): not able to create json object: tile id: " << "addTiles(): not able to create json object: tile id: "
<< tile->id() << " progress: " << tile->progress() << tile->id() << " progress: " << tile->progress()
<< " points: " << tile->tile(); << " points: " << tile->tile();
} }
jsonTileArray.PushBack(jsonTile, allocator); jsonTileArray.append(std::move(jsonTile));
} // for } // for
rapidjson::Value tileKey("in_tile_array"); QJsonObject req;
request.AddMember(tileKey, jsonTileArray, allocator); req["in_tile_array"] = std::move(jsonTileArray);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler. // create response handler.
auto promise_response = std::make_shared<std::promise<bool>>(); auto promise_response = std::make_shared<std::promise<bool>>();
auto future_response = promise_response->get_future(); auto future_response = promise_response->get_future();
auto responseHandler = [promise_response](const QJsonObject &o) mutable { auto responseHandler = [promise_response](const QJsonObject &o) mutable {
// check if transaction was successfull // check if transaction was successfull
QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); if (o.contains("success") && o["success"].isBool()) {
rapidjson::Document d; promise_response->set_value(o["success"].toBool());
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);
}
} else { } else {
qCWarning(NemoInterfaceLog) << "/nemo/add_tiles message parse error (" qCWarning(NemoInterfaceLog)
<< d.GetParseError() << "): " << msg; << "/nemo/add_tiles no \"success\" key or wrong type: " << o;
promise_response->set_value(false); promise_response->set_value(false);
} }
}; };
// call service. // call service.
this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, req);
req.object());
// wait for response. // wait for response.
auto tStart = hrc::now(); auto tStart = hrc::now();
...@@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) { ...@@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
this->_lastCall = CALL_NAME::REMOVE_TILES; this->_lastCall = CALL_NAME::REMOVE_TILES;
// create json object // create json object
rapidjson::Document request(rapidjson::kObjectType); QJsonArray jsonIdArray;
auto &allocator = request.GetAllocator();
rapidjson::Value jsonIdArray(rapidjson::kArrayType);
for (const auto id : *pIdArray) { for (const auto id : *pIdArray) {
using namespace ros_bridge::messages; using namespace ros_bridge::messages;
jsonIdArray.PushBack(rapidjson::Value(id), allocator); jsonIdArray.append(id);
} // for } // for
rapidjson::Value tileKey("ids"); QJsonObject req;
request.AddMember(tileKey, jsonIdArray, allocator); req["ids"] = std::move(jsonIdArray);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler. // create response handler.
auto promise_response = std::make_shared<std::promise<bool>>(); auto promise_response = std::make_shared<std::promise<bool>>();
...@@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) { ...@@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
auto responseHandler = [promise_response](const QJsonObject &o) mutable { auto responseHandler = [promise_response](const QJsonObject &o) mutable {
// check if transaction was successfull // check if transaction was successfull
QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
rapidjson::Document d; if (o.contains("success") && o["success"].isBool()) {
d.Parse(msg.toUtf8()); promise_response->set_value(o["success"].toBool());
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);
}
} else { } else {
qCWarning(NemoInterfaceLog) << "/nemo/remove_tiles message parse error (" qCWarning(NemoInterfaceLog)
<< d.GetParseError() << "): " << msg; << "/nemo/remove_tiles no \"success\" key or wrong type: " << msg;
promise_response->set_value(false); promise_response->set_value(false);
} }
}; };
// call service. // call service.
this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, req);
req.object());
// wait for response. // wait for response.
auto tStart = hrc::now(); auto tStart = hrc::now();
...@@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() { ...@@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
auto future_response = promise_response->get_future(); auto future_response = promise_response->get_future();
auto responseHandler = [promise_response](const QJsonObject &o) mutable { auto responseHandler = [promise_response](const QJsonObject &o) mutable {
// check if transaction was successfull // check if transaction was successfull
QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); promise_response->set_value(true);
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);
}
}; };
// call service. // call service.
...@@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) { ...@@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
this->_lastCall = CALL_NAME::GET_PROGRESS; this->_lastCall = CALL_NAME::GET_PROGRESS;
// create json object // create json object
rapidjson::Document request(rapidjson::kObjectType); QJsonArray jsonIdArray;
auto &allocator = request.GetAllocator();
rapidjson::Value jsonIdArray(rapidjson::kArrayType);
for (const auto id : *pIdArray) { for (const auto id : *pIdArray) {
using namespace ros_bridge::messages; using namespace ros_bridge::messages;
jsonIdArray.PushBack(rapidjson::Value(id), allocator); jsonIdArray.append(id);
} // for } // for
rapidjson::Value tileKey("ids"); QJsonObject req;
request.AddMember(tileKey, jsonIdArray, allocator); req["ids"] = std::move(jsonIdArray);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler. // create response handler.
typedef std::shared_ptr<ProgressArray> ResponseType; typedef std::shared_ptr<ProgressArray> ResponseType;
...@@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) { ...@@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
auto future_response = promise_response->get_future(); auto future_response = promise_response->get_future();
auto responseHandler = [promise_response](const QJsonObject &o) mutable { auto responseHandler = [promise_response](const QJsonObject &o) mutable {
// check if transaction was successfull // check if transaction was successfull
QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
rapidjson::Document d; progressArrayMsg;
d.Parse(msg.toUtf8()); if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
if (!d.HasParseError()) { o, progressArrayMsg)) {
if (d.HasMember("values") && d["values"].IsObject()) { auto pArray = std::make_shared<ProgressArray>();
auto values = d["values"].GetObject(); *pArray = std::move(progressArrayMsg.progress_array());
ros_bridge::messages::nemo_msgs::progress_array::ProgressArray promise_response->set_value(pArray);
progressArrayMsg;
if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
d["values"], progressArrayMsg)) {
auto pArray = std::make_shared<ProgressArray>();
*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);
}
} else { } else {
qCWarning(NemoInterfaceLog) << "/nemo/get_progress message parse error (" qCWarning(NemoInterfaceLog)
<< d.GetParseError() << "): " << msg; << "/nemo/get_progress error while creating ProgressArray "
"from json.";
promise_response->set_value(nullptr); promise_response->set_value(nullptr);
} }
}; };
// call service. // call service.
this->_pRosbridge->callService("/nemo/get_progress", responseHandler, this->_pRosbridge->callService("/nemo/get_progress", responseHandler, req);
req.object());
// wait for response. // wait for response.
auto tStart = hrc::now(); auto tStart = hrc::now();
...@@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() { ...@@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
auto future_response = promise_response->get_future(); auto future_response = promise_response->get_future();
auto responseHandler = [promise_response](const QJsonObject &o) mutable { auto responseHandler = [promise_response](const QJsonObject &o) mutable {
// check if transaction was successfull // check if transaction was successfull
QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
rapidjson::Document d; progressArrayMsg;
d.Parse(msg.toUtf8()); if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
if (!d.HasParseError()) { o, progressArrayMsg)) {
if (d.HasMember("values") && d["values"].IsObject()) { auto pArray = std::make_shared<ProgressArray>();
ros_bridge::messages::nemo_msgs::progress_array::ProgressArray *pArray = std::move(progressArrayMsg.progress_array());
progressArrayMsg; promise_response->set_value(pArray);
if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
d["values"], progressArrayMsg)) {
auto pArray = std::make_shared<ProgressArray>();
*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);
}
} else { } else {
qCWarning(NemoInterfaceLog) qCWarning(NemoInterfaceLog)
<< "/nemo/all_get_progress message parse error (" << d.GetParseError() << "/nemo/get_all_progress error while creating ProgressArray "
<< "): " << msg; "from json. msg: "
<< o;
promise_response->set_value(nullptr); promise_response->set_value(nullptr);
} }
}; };
......
...@@ -19,6 +19,8 @@ ...@@ -19,6 +19,8 @@
#define MAX_TILES 1000 #define MAX_TILES 1000
#endif #endif
QString randomId();
using namespace geometry; using namespace geometry;
namespace trans = bg::strategy::transform; namespace trans = bg::strategy::transform;
...@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json, ...@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
// find unique id // find unique id
if (it != _indexMap.end()) { if (it != _indexMap.end()) {
auto newId = tile->id() + 1; auto newId = MeasurementTile::randomId();
constexpr long counterMax = 1e6; constexpr long counterMax = 1e6;
unsigned long counter = 0; unsigned long counter = 0;
for (; counter <= counterMax; ++counter) { for (; counter <= counterMax; ++counter) {
...@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json, ...@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
if (it == _indexMap.end()) { if (it == _indexMap.end()) {
break; break;
} else { } else {
++newId; newId = MeasurementTile::randomId();
} }
} }
...@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() { ...@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
// Convert to geo system. // Convert to geo system.
for (const auto &t : tilesENU) { for (const auto &t : tilesENU) {
auto geoTile = new MeasurementTile(pData.get()); auto geoTile = new MeasurementTile(pData.get());
std::size_t hashValue = 0;
std::hash<QGeoCoordinate> hashFun;
for (const auto &v : t.outer()) { for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex; QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex); fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex); geoTile->push_back(geoVertex);
hashValue ^= hashFun(geoVertex);
} }
geoTile->setId(std::int64_t(hashValue));
pData->append(geoTile); pData->append(geoTile);
} }
} }
...@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() { ...@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
// find unique id // find unique id
if (it != _indexMap.end()) { if (it != _indexMap.end()) {
auto newId = tile->id() + 1; auto newId = MeasurementTile::randomId();
constexpr long counterMax = 1e6; constexpr long counterMax = 1e6;
unsigned long counter = 0; unsigned long counter = 0;
for (; counter <= counterMax; ++counter) { for (; counter <= counterMax; ++counter) {
...@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() { ...@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
if (it == _indexMap.end()) { if (it == _indexMap.end()) {
break; break;
} else { } else {
++newId; newId = MeasurementTile::randomId();
} }
} }
...@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth, ...@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
return true; 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);
}
...@@ -107,7 +107,7 @@ private: ...@@ -107,7 +107,7 @@ private:
// Tile stuff. // Tile stuff.
TilePtr _tiles; TilePtr _tiles;
std::map<std::int64_t /*id*/, int> _indexMap; std::map<QString /*id*/, int> _indexMap;
QTimer _timer; QTimer _timer;
STATE _state; STATE _state;
QFutureWatcher<TilePtr> _watcher; QFutureWatcher<TilePtr> _watcher;
......
...@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile"; ...@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile";
const char *MeasurementTile::nameString = "MeasurementTile"; const char *MeasurementTile::nameString = "MeasurementTile";
MeasurementTile::MeasurementTile(QObject *parent) MeasurementTile::MeasurementTile(QObject *parent)
: GeoArea(parent), _progress(0), _id(0) { : GeoArea(parent), _progress(0), _id(MeasurementTile::randomId()) {
init(); init();
} }
...@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) { ...@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
void MeasurementTile::init() { this->setObjectName(nameString); } 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) { if (_id != id) {
_id = id; _id = id;
emit idChanged(); emit idChanged();
...@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) { ...@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) {
QList<QGeoCoordinate> MeasurementTile::tile() const { return coordinateList(); } QList<QGeoCoordinate> 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; } double MeasurementTile::progress() const { return _progress; }
void MeasurementTile::setProgress(double progress) { void MeasurementTile::setProgress(double progress) {
......
...@@ -24,11 +24,13 @@ public: ...@@ -24,11 +24,13 @@ public:
double progress() const; double progress() const;
void setProgress(double progress); void setProgress(double progress);
int64_t id() const; QString id() const;
void setId(const int64_t &id); void setId(const QString &id);
QList<QGeoCoordinate> tile() const; QList<QGeoCoordinate> tile() const;
static QString randomId(int length = 10);
// Static Variables // Static Variables
static const char *settingsGroup; static const char *settingsGroup;
static const char *nameString; static const char *nameString;
...@@ -40,5 +42,5 @@ signals: ...@@ -40,5 +42,5 @@ signals:
private: private:
void init(); void init();
double _progress; double _progress;
int64_t _id; QString _id;
}; };
...@@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() { ...@@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() {
return this->_taskQueue.size() + (_running ? 1 : 0); return this->_taskQueue.size() + (_running ? 1 : 0);
} }
bool TaskDispatcher::idle() { return this->pendingTasks() == 0; }
void TaskDispatcher::run() { void TaskDispatcher::run() {
while (true) { while (true) {
ULock lk1(this->_mutex); ULock lk1(this->_mutex);
......
...@@ -53,6 +53,7 @@ public: ...@@ -53,6 +53,7 @@ public:
bool isRunning(); bool isRunning();
std::size_t pendingTasks(); std::size_t pendingTasks();
bool idle();
protected: protected:
void run(); void run();
......
...@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics"; ...@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics";
static constexpr auto pollIntervall = std::chrono::milliseconds(200); static constexpr auto pollIntervall = std::chrono::milliseconds(200);
Rosbridge::STATE translate(RosbridgeImpl::STATE s);
Rosbridge::Rosbridge(const QUrl url, QObject *parent) Rosbridge::Rosbridge(const QUrl url, QObject *parent)
: QObject(parent), _url(url), _running(false) { : QObject(parent), _url(url), _running(false) {
static std::once_flag flag; static std::once_flag flag;
...@@ -34,6 +36,9 @@ void Rosbridge::start() { ...@@ -34,6 +36,9 @@ void Rosbridge::start() {
_impl = new RosbridgeImpl(_url); _impl = new RosbridgeImpl(_url);
_impl->moveToThread(_t); _impl->moveToThread(_t);
connect(_impl, &RosbridgeImpl::stateChanged, this,
&Rosbridge::_onImplStateChanged);
connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start); connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start);
connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop); connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop);
...@@ -77,20 +82,7 @@ void Rosbridge::stop() { ...@@ -77,20 +82,7 @@ void Rosbridge::stop() {
Rosbridge::STATE Rosbridge::state() { Rosbridge::STATE Rosbridge::state() {
if (_running) { if (_running) {
switch (_impl->state()) { return translate(_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;
} else { } else {
return STATE::STOPPED; return STATE::STOPPED;
} }
...@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) { ...@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) {
qDebug() << "waitForService: Rosbridge not connected."; 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;
}
...@@ -101,6 +101,9 @@ signals: ...@@ -101,6 +101,9 @@ signals:
void _unadvertiseAllServices(); void _unadvertiseAllServices();
private: private:
void _onImplStateChanged();
std::atomic<STATE> _state;
RosbridgeImpl *_impl; RosbridgeImpl *_impl;
QThread *_t; QThread *_t;
QUrl _url; QUrl _url;
......
...@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) { ...@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _advertisedTopics.find(topic); auto it = _advertisedTopics.find(topic);
if (Q_LIKELY(it != _advertisedTopics.end())) { if (Q_LIKELY(it != _advertisedTopics.end())) {
_advertisedTopics.erase(it);
QJsonObject o; QJsonObject o;
o[opKey] = unadvertiseOpKey; o[opKey] = unadvertiseOpKey;
...@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) { ...@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
QString payload = QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload); _webSocket.sendTextMessage(payload);
_advertisedTopics.erase(it);
} else { } else {
qDebug() << "Topic " << topic << " not advertised."; qDebug() << "Topic " << topic << " not advertised.";
} }
...@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) { ...@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _subscribedTopics.find(topic); auto it = _subscribedTopics.find(topic);
if (Q_LIKELY(it != _subscribedTopics.end())) { if (Q_LIKELY(it != _subscribedTopics.end())) {
_subscribedTopics.erase(it);
QJsonObject o; QJsonObject o;
o[opKey] = unsubscribeOpKey; o[opKey] = unsubscribeOpKey;
...@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) { ...@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
QString payload = QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload); _webSocket.sendTextMessage(payload);
_subscribedTopics.erase(it);
} else { } else {
qDebug() << "unsubscribeTopic: topic " << topic << " already subscribed!"; qDebug() << "unsubscribeTopic: topic " << topic << " already subscribed!";
} }
...@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) { ...@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) { if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _advertisedServices.find(service); auto it = _advertisedServices.find(service);
if (it != _advertisedServices.end()) { if (it != _advertisedServices.end()) {
it = _advertisedServices.erase(it);
QJsonObject o; QJsonObject o;
o[opKey] = unadvertiseServiceOpKey; o[opKey] = unadvertiseServiceOpKey;
...@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) { ...@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
QString payload = QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact); QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload); _webSocket.sendTextMessage(payload);
it = _advertisedServices.erase(it);
} else { } else {
qDebug() << "unadvertiseService: Service " << service qDebug() << "unadvertiseService: Service " << service
<< " not advertised."; << " not advertised.";
...@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() { ...@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() {
} }
void RosbridgeImpl::_onTextMessageReceived(const QString &message) { void RosbridgeImpl::_onTextMessageReceived(const QString &message) {
qDebug() << "_onTextMessageReceived: " << message;
QJsonParseError e; QJsonParseError e;
auto d = QJsonDocument::fromJson(message.toUtf8(), &e); auto d = QJsonDocument::fromJson(message.toUtf8(), &e);
if (!d.isNull()) { if (!d.isNull()) {
......
...@@ -3,147 +3,140 @@ ...@@ -3,147 +3,140 @@
#include <string> #include <string>
#include "ros_bridge/include/message_traits.h" #include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonObject>
#include <QJsonValue>
namespace ros_bridge { namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation. //! @brief Namespace containing classes and methodes ros message generation.
namespace messages { 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 { 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 { namespace geo_point {
std::string messageType(); std::string messageType();
namespace {
const char *lonKey = "longitude";
const char *latKey = "latitude";
const char *altKey = "altitude";
} // namespace
//! @brief C++ representation of geographic_msgs/GeoPoint. //! @brief C++ representation of geographic_msgs/GeoPoint.
template<class FloatType = _Float64, class OStream = std::ostream> template <class FloatType = _Float64, class OStream = std::ostream>
class GenericGeoPoint{ class GenericGeoPoint {
public: public:
GenericGeoPoint() GenericGeoPoint() : _latitude(0), _longitude(0), _altitude(0) {}
: _latitude(0) GenericGeoPoint(const GenericGeoPoint &other)
, _longitude(0) : _latitude(other.latitude()), _longitude(other.longitude()),
, _altitude(0) _altitude(other.altitude()) {}
{} GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
GenericGeoPoint(const GenericGeoPoint &other) : _latitude(latitude), _longitude(longitude), _altitude(altitude) {}
: _latitude(other.latitude())
, _longitude(other.longitude()) FloatType latitude() const { return _latitude; }
, _altitude(other.altitude()) FloatType longitude() const { return _longitude; }
{} FloatType altitude() const { return _altitude; }
GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
: _latitude(latitude) void setLatitude(FloatType latitude) { _latitude = latitude; }
, _longitude(longitude) void setLongitude(FloatType longitude) { _longitude = longitude; }
, _altitude(altitude) void setAltitude(FloatType altitude) { _altitude = altitude; }
{}
bool operator==(const GenericGeoPoint &p1) {
FloatType latitude() const {return _latitude;} return (p1._latitude == this->_latitude &&
FloatType longitude() const {return _longitude;} p1._longitude == this->_longitude &&
FloatType altitude() const {return _altitude;} p1._altitude == this->_altitude);
}
void setLatitude (FloatType latitude) {_latitude = latitude;}
void setLongitude (FloatType longitude) {_longitude = longitude;} bool operator!=(const GenericGeoPoint &p1) { return !this->operator==(p1); }
void setAltitude (FloatType altitude) {_altitude = altitude;}
friend OStream &operator<<(OStream &os, const GenericGeoPoint &p) {
bool operator==(const GenericGeoPoint &p1) { os << "lat: " << p._latitude << " lon: " << p._longitude
return ( p1._latitude == this->_latitude << " alt: " << p._altitude;
&& p1._longitude == this->_longitude return os;
&& 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: private:
FloatType _latitude; FloatType _latitude;
FloatType _longitude; FloatType _longitude;
FloatType _altitude; FloatType _altitude;
}; };
typedef GenericGeoPoint<> GeoPoint; typedef GenericGeoPoint<> GeoPoint;
namespace detail { namespace detail {
template <class T> template <class T>
auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>) auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>) {
{ return p.altitude();
return p.altitude(); }
}
template <class T>
template <class T> auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>) {
auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>) (void)p;
{ return 0.0;
(void)p; }
return 0.0;
} template <class T>
void setAltitude(const QJsonValue &o, T &p,
template <class T> traits::Type2Type<traits::Has3Components>) {
void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has3Components>) p.setAltitude(o.toDouble());
{ }
p.setAltitude(doc.GetFloat());
}
template <class T>
void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has2Components>)
{
(void)doc;
(void)p;
}
} // namespace detail
template <class T> template <class T>
bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) void setAltitude(const QJsonValue &o, T &p,
{ traits::Type2Type<traits::Has2Components>) {
value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator); (void)o;
value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator); (void)p;
typedef typename traits::Select<
traits::HasMemberAltitude<T>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
return true;
} }
} // namespace detail
template <class T> bool toJson(const T &p, QJsonObject &value) {
value[latKey] = p.latitude();
value[lonKey] = p.longitude();
typedef typename traits::Select<traits::HasMemberAltitude<T>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
auto altitude = detail::getAltitude(
p, traits::Type2Type<Components>()); // If T has no member altitude()
// replace it by 0.0;
value[altKey] = altitude;
return true;
}
template <class PointType> template <class PointType>
bool fromJson(const rapidjson::Value &value, bool fromJson(const QJsonObject &value, PointType &p) {
PointType &p) { if (!value.contains("latitude") || !value["latitude"].isDouble()) {
if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){ return false;
assert(false); }
return false; if (!value.contains("longitude") || !value["longitude"].isDouble()) {
} return false;
if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){ }
assert(false); if (!value.contains("altitude") || !value["altitude"].isDouble()) {
return false; return false;
} }
if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
assert(false); p.setLatitude(value["latitude"].toDouble());
return false; p.setLongitude(value["longitude"].toDouble());
} typedef
typename traits::Select<traits::HasMemberSetAltitude<PointType>::value,
p.setLatitude(value["latitude"].GetFloat()); traits::Has3Components,
p.setLongitude(value["longitude"].GetFloat()); traits::Has2Components>::Result
typedef typename traits::Select< Components; // Check if PointType has 2 or 3 dimensions.
traits::HasMemberSetAltitude<PointType>::value, detail::setAltitude(
traits::Has3Components, value["altitude"], p,
traits::Has2Components>::Result traits::Type2Type<Components>()); // If T has no member altitude() discard
Components; // Check if PointType has 2 or 3 dimensions. // doc["altitude"];
detail::setAltitude(value["altitude"], p, traits::Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
return true;
return true;
} // namespace detail } // namespace detail
} // namespace geo_point
} // namespace geopoint } // namespace geographic_msgs
} // namespace geometry_msgs
} // namespace messages } // namespace messages
} // namespace ros_bridge } // namespace ros_bridge
#include "point32.h"
std::string ros_bridge::messages::geometry_msgs::point32::messageType(){
return "geometry_msgs/Point32";
}
#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 <class T>
auto getZ(const T &p, Type2Type<Has3Components>)
{
return p.z();
}
template <class T>
auto getZ(const T &p, Type2Type<Has2Components>)
{
(void)p;
return 0.0; // p has no member z() -> add 0.
}
template <class T, typename V>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<V>)
{
p.setZ(doc["z"].GetFloat());
return true;
}
template <class T>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
{
(void)doc;
(void)p;
return true;
}
} // namespace detail
//! @brief C++ representation of a geometry_msgs/Point/Point32
template<typename FloatType = _Float64, class OStream = std::ostream>
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 <class T>
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<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
return true;
}
template <class PointType>
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<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
(void)detail::setZ(value["z"], p, Type2Type<Components>()); // If PointType has no member z() discard doc["z"].
return true;
}
} // namespace point32
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge
#include "polygon.h"
std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){
return "geometry_msgs/Polygon";
}
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/point32.h"
#include <type_traits>
#include <vector>
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 PointTypeCVR,
template <class, class...> class ContainerType = std::vector>
class GenericPolygon {
typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
public:
GenericPolygon() {}
GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}
const ContainerType<PointType> &points() const {return _points;}
ContainerType<PointType> &points() {return _points;}
GenericPolygon &operator=(const GenericPolygon &other) {
this->_points = other._points;
return *this;
}
private:
ContainerType<PointType> _points;
};
typedef GenericPolygon<geometry_msgs::point32::Point> Polygon;
template <class PolygonType>
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 <class PolygonType>
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<typename std::remove_reference_t<PointTypeCVR>> 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
#include "polygon_stamped.h"
std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){
return "geometry_msgs/PolygonStamped";
}
#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 <type_traits>
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 PolygonType = geometry_msgs::polygon::Polygon,
class HeaderType = std_msgs::header::Header>
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 <class PolygonStampedType>
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<typename std::remove_reference_t<HeaderTypeCVR>> HeaderType;
HeaderType header;
bool ret = header::fromJson(doc, header);
polyStamped.header() = header;
return ret;
}
template <class PolygonStampedType>
bool setHeader(const rapidjson::Value &doc,
PolygonStampedType &polyStamped,
traits::Int2Type<0>) { // polyStamped.header() does not exists
(void)doc;
(void)polyStamped;
return true;
}
template <class PolygonType, class HeaderType>
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<class PolyStamped, int k>
bool _toJson(const PolyStamped &polyStamped,
rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator,
traits::Int2Type<k>){ // PolyStamped has member header(), use integraded header.
return _toJson(polyStamped, polyStamped.header(), value, allocator);
}
template<class PolyStamped>
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 <class PolygonType, class HeaderType>
bool toJson(const PolygonType &poly,
const HeaderType &h,
rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator)
{
return detail::_toJson(poly, h, value, allocator);
}
template <class PolyStamped>
bool toJson(const PolyStamped &polyStamped,
rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator)
{
typedef traits::HasMemberHeader<PolyStamped> HasHeader;
return detail::_toJson(polyStamped, value, allocator, traits::Int2Type<HasHeader::value>());
}
template <class PolygonType, class HeaderType>
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<PolygonType> HasHeader;
if ( !detail::setHeader(value["header"], polyStamped, traits::Int2Type<HasHeader::value>())){
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
#pragma once #pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h" #include <QJsonObject>
namespace ros_bridge { namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation. //! @brief Namespace containing classes and methodes ros message generation.
...@@ -32,19 +32,18 @@ protected: ...@@ -32,19 +32,18 @@ protected:
}; };
template <class HeartbeatType> template <class HeartbeatType>
bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, bool toJson(const HeartbeatType &heartbeat, QJsonObject &value) {
rapidjson::Document::AllocatorType &allocator) { value["status"] = heartbeat.status();
value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
return true; return true;
} }
template <class HeartbeatType> template <class HeartbeatType>
bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) { bool fromJson(const QJsonObject &value, HeartbeatType &heartbeat) {
if (!value.HasMember("status") || !value["status"].IsInt()) { if (!value.contains("status") || !value["status"].isDouble()) {
return false; return false;
} }
heartbeat.setStatus(value["status"].GetInt()); heartbeat.setStatus(value["status"].toInt());
return true; return true;
} }
......
#include "labeled_progress.h" #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"; return "nemo_msgs/LabeledProgress";
} }
#pragma once #pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h" #include <QJsonObject>
#include <vector> #include <QDebug>
#include <QString>
namespace ros_bridge { namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation. //! @brief Namespace containing classes and methodes ros message generation.
...@@ -14,7 +15,7 @@ namespace nemo_msgs { ...@@ -14,7 +15,7 @@ namespace nemo_msgs {
//! generation. //! generation.
namespace labeled_progress { namespace labeled_progress {
std::string messageType(); QString messageType();
namespace { namespace {
const char *progressKey = "progress"; const char *progressKey = "progress";
...@@ -24,40 +25,39 @@ const char *idKey = "id"; ...@@ -24,40 +25,39 @@ const char *idKey = "id";
//! @brief C++ representation of nemo_msgs/labeled_progress message //! @brief C++ representation of nemo_msgs/labeled_progress message
class LabeledProgress { class LabeledProgress {
public: public:
LabeledProgress() : _id(0), _progress(0) {} LabeledProgress() : _id(""), _progress(0) {}
LabeledProgress(double progress, long id) : _id(id), _progress(progress) {} LabeledProgress(double progress, QString id) : _id(id), _progress(progress) {}
long id() const { return _id; } QString id() const { return _id; }
void setId(long id) { _id = id; } void setId(QString id) { _id = id; }
double progress() const { return _progress; } double progress() const { return _progress; }
void setProgress(double progress) { _progress = progress; } void setProgress(double progress) { _progress = progress; }
protected: protected:
long _id; QString _id;
double _progress; double _progress;
}; };
template <class LabeledProgressType> template <class LabeledProgressType>
bool toJson(const LabeledProgressType &lp, rapidjson::Value &value, bool toJson(const LabeledProgressType &lp, QJsonObject &value) {
rapidjson::Document::AllocatorType &allocator) { value[idKey] = lp.id();
value.AddMember(idKey, lp.id(), allocator); value[progressKey] = lp.progress();
value.AddMember(progressKey, lp.progress(), allocator);
return true; return true;
} }
template <class ProgressType> template <class ProgressType>
bool fromJson(const rapidjson::Value &value, ProgressType &p) { bool fromJson(const QJsonObject &value, ProgressType &p) {
if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) { if (!value.contains(progressKey) || !value[progressKey].isDouble()) {
return false; return false;
} }
if (!value.HasMember(idKey) || !value[idKey].IsInt64()) { if (!value.contains(idKey) || !value[idKey].isString()) {
return false; return false;
} }
p.setId(value[idKey].GetInt64()); p.setId(value[idKey].toString());
p.setProgress(value[progressKey].GetDouble()); p.setProgress(value[progressKey].toDouble());
return true; return true;
} }
......
...@@ -4,8 +4,9 @@ ...@@ -4,8 +4,9 @@
#pragma once #pragma once
#include "labeled_progress.h" #include "labeled_progress.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonArray>
#include <QJsonObject>
#include <QVector> #include <QVector>
namespace ros_bridge { namespace ros_bridge {
...@@ -43,34 +44,33 @@ protected: ...@@ -43,34 +44,33 @@ protected:
typedef GenericProgressArray<> ProgressArray; typedef GenericProgressArray<> ProgressArray;
template <class Array> template <class Array> bool toJson(const Array &array, QJsonObject &value) {
bool toJson(const Array &array, rapidjson::Value &value, QJsonArray jsonArray;
rapidjson::Document::AllocatorType &allocator) {
rapidjson::Value jsonArray(rapidjson::kArrayType);
for (const auto &lp : array.progress_array()) { for (const auto &lp : array.progress_array()) {
rapidjson::Value jsonLp; QJsonObject jsonLp;
if (labeled_progress::toJson(lp, jsonLp, allocator)) { if (labeled_progress::toJson(lp, jsonLp)) {
jsonArray.PushBack(jsonLp, allocator); jsonArray.append(std::move(jsonLp));
} else { } else {
return false; return false;
} }
} }
value.AddMember(rapidjson::Value(progressArrayKey), jsonArray, allocator); value[progressArrayKey] = std::move(jsonArray);
return true; return true;
} }
template <class Array> bool fromJson(const rapidjson::Value &value, Array &p) { template <class Array> bool fromJson(const QJsonObject &value, Array &p) {
if (!value.HasMember(progressArrayKey) || if (!value.contains(progressArrayKey) || !value[progressArrayKey].isArray()) {
!value[progressArrayKey].IsArray()) {
return false; return false;
} }
const auto &jsonArray = value[progressArrayKey].GetArray(); const auto jsonArray = value[progressArrayKey].toArray();
p.progress_array().clear(); p.progress_array().clear();
p.progress_array().reserve(jsonArray.Size()); p.progress_array().reserve(jsonArray.size());
for (long i = 0; i < jsonArray.Size(); ++i) { for (long i = 0; i < jsonArray.size(); ++i) {
labeled_progress::LabeledProgress lp; labeled_progress::LabeledProgress lp;
if (!labeled_progress::fromJson(jsonArray[i], lp)) { if (!jsonArray[i].isObject() ||
!labeled_progress::fromJson(jsonArray[i].toObject(), lp)) {
return false; return false;
} else { } else {
p.progress_array().push_back(lp); p.progress_array().push_back(lp);
......
#include "tile.h" #include "tile.h"
std::string ros_bridge::messages::nemo_msgs::tile::messageType() { QString ros_bridge::messages::nemo_msgs::tile::messageType() {
return "nemo_msgs/Tile"; return "nemo_msgs/Tile";
} }
#pragma once #pragma once
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h" #include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <vector> #include <vector>
#include <QJsonArray>
#include <QString>
namespace ros_bridge { namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation. //! @brief Namespace containing classes and methodes ros message generation.
namespace messages { namespace messages {
...@@ -15,7 +17,7 @@ namespace nemo_msgs { ...@@ -15,7 +17,7 @@ namespace nemo_msgs {
//! generation. //! generation.
namespace tile { namespace tile {
std::string messageType(); QString messageType();
namespace { namespace {
const char *progressKey = "progress"; const char *progressKey = "progress";
...@@ -30,11 +32,11 @@ class GenericTile { ...@@ -30,11 +32,11 @@ class GenericTile {
public: public:
typedef Container<GeoPoint> GeoContainer; typedef Container<GeoPoint> GeoContainer;
GenericTile() {} GenericTile() {}
GenericTile(const GeoContainer &tile, double progress, long id) GenericTile(const GeoContainer &tile, double progress, const QString &id)
: _tile(tile), _id(id), _progress(progress) {} : _tile(tile), _id(id), _progress(progress) {}
long id() const { return _id; } QString id() const { return _id; }
void setId(long id) { _id = id; } void setId(const QString &id) { _id = id; }
double progress() const { return _progress; } double progress() const { return _progress; }
void setProgress(double progress) { _progress = progress; } void setProgress(double progress) { _progress = progress; }
...@@ -45,63 +47,58 @@ public: ...@@ -45,63 +47,58 @@ public:
protected: protected:
GeoContainer _tile; GeoContainer _tile;
long _id; QString _id;
double _progress; double _progress;
}; };
typedef GenericTile<> Tile; typedef GenericTile<> Tile;
template <class TileType> template <class TileType>
bool toJson(const TileType &tile, rapidjson::Value &value, bool toJson(const TileType &tile, QJsonObject &value) {
rapidjson::Document::AllocatorType &allocator) {
using namespace rapidjson; value[idKey] = tile.id();
value.AddMember(Value(idKey, allocator), Value(tile.id()), allocator); value[progressKey] = tile.progress();
value.AddMember(Value(progressKey, allocator), Value(tile.progress()),
allocator);
using namespace messages::geographic_msgs; 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) { for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) {
rapidjson::Value geoPoint(rapidjson::kObjectType); QJsonObject geoPoint;
if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) { if (!geo_point::toJson(tile.tile()[i], geoPoint)) {
return false; return false;
} }
geoPoints.PushBack(geoPoint, allocator); geoPoints.append(std::move(geoPoint));
} }
rapidjson::Value key(tileKey, allocator); value[tileKey] = std::move(geoPoints);
value.AddMember(key, geoPoints, allocator);
return true; return true;
} }
template <class TileType> template <class TileType> bool fromJson(const QJsonObject &value, TileType &p) {
bool fromJson(const rapidjson::Value &value, TileType &p) { if (!value.contains(progressKey) || !value[progressKey].isDouble()) {
if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
return false; return false;
} }
if (!value.HasMember(idKey) || !value[idKey].IsInt()) { if (!value.contains(idKey) || !value[idKey].isString()) {
return false; return false;
} }
if (!value.HasMember(tileKey) || !value[tileKey].IsArray()) { if (!value.contains(tileKey) || !value[tileKey].isArray()) {
return false; return false;
} }
p.setId(value[idKey].GetInt()); p.setId(value[idKey].toString());
p.setProgress(value[progressKey].GetDouble()); p.setProgress(value[progressKey].toDouble());
using namespace geographic_msgs; using namespace geographic_msgs;
const auto &jsonArray = value[tileKey].GetArray(); const auto jsonArray = value[tileKey].toArray();
p.tile().clear(); p.tile().clear();
p.tile().reserve(jsonArray.Size()); p.tile().reserve(jsonArray.size());
typedef decltype(p.tile()[0]) PointTypeCVR; typedef decltype(p.tile()[0]) PointTypeCVR;
typedef typedef
typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>>
PointType; PointType;
for (long i = 0; i < jsonArray.Size(); ++i) { for (long i = 0; i < jsonArray.size(); ++i) {
PointType pt; PointType pt;
if (!geo_point::fromJson(jsonArray[i], pt)) { if (!geo_point::fromJson(jsonArray[i], pt)) {
return false; return false;
......
#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";
}
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/std_msgs/time.h"
#include <string>
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 <class HeaderType>
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 <class HeaderType>
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
#include "time.h"
std::string ros_bridge::messages::std_msgs::time::messageType(){
return "std_msgs/Time";
}
#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<class TimeType>
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<class TimeType>
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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment