Skip to content

Q
qgroundcontrol
Commit 9e620065 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

new rosbridge impl added, not tested

parent b94dfebd
Hide whitespace changes
Inline Side-by-side
Showing with 1212 additions and 293 deletions
qgroundcontrol.pro
View file @ 9e620065
......@@ -267,7 +267,8 @@ QT += \
widgets \
xml \
texttospeech \
core-private
core-private \
websockets
# Multimedia only used if QVC is enabled
!contains (DEFINES, QGC_DISABLE_UVC) {
......@@ -420,6 +421,7 @@ INCLUDEPATH += \
src/Audio \
src/comm \
src/MeasurementComplexItem \
src/MeasurementComplexItem/rosbridge \
src/MeasurementComplexItem/geometry \
src/MeasurementComplexItem/nemo_interface \
src/comm/ros_bridge \
......@@ -454,6 +456,8 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \
src/MeasurementComplexItem/rosbridge/rosbridge.h \
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \
src/MeasurementComplexItem/nemo_interface/Task.h \
......@@ -508,18 +512,13 @@ HEADERS += \
src/GPS/Drivers/src/base_station.h \
src/Settings/WimaSettings.h \
src/comm/QmlObjectListHelper.h \
src/comm/ros_bridge/include/RosBridgeClient.h \
src/comm/ros_bridge/include/com_private.h \
src/comm/ros_bridge/include/message_traits.h \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.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/tile.h \
src/comm/ros_bridge/include/messages/std_msgs/header.h \
src/comm/ros_bridge/include/server.h \
src/comm/ros_bridge/include/messages/std_msgs/time.h \
src/comm/ros_bridge/include/topic_publisher.h \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h
contains (DEFINES, QGC_ENABLE_PAIRING) {
......@@ -534,6 +533,8 @@ SOURCES += \
src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/MeasurementComplexItem/HashFunctions.cpp \
src/MeasurementComplexItem/rosbridge/rosbridge.cpp \
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
src/MeasurementComplexItem/nemo_interface/Task.cpp \
......@@ -554,8 +555,6 @@ SOURCES += \
src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
src/MeasurementComplexItem/NemoInterface.cpp \
src/comm/QmlObjectListHelper.cpp \
src/comm/ros_bridge/include/RosBridgeClient.cpp \
src/comm/ros_bridge/include/com_private.cpp \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
......@@ -563,11 +562,7 @@ SOURCES += \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
src/comm/ros_bridge/include/server.cpp \
src/comm/ros_bridge/include/topic_publisher.cpp \
src/comm/ros_bridge/include/topic_subscriber.cpp \
src/Settings/WimaSettings.cc \
src/comm/ros_bridge/src/ros_bridge.cpp
contains (DEFINES, QGC_ENABLE_PAIRING) {
SOURCES += \
......
src/MeasurementComplexItem/NemoInterface.cpp
View file @ 9e620065
......@@ -10,6 +10,7 @@
#include <shared_mutex>
#include <QTimer>
#include <QUrl>
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
......@@ -19,26 +20,26 @@
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
#include "ros_bridge/include/messages/nemo_msgs/progress_array.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.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/tile.h"
#include "ros_bridge/include/ros_bridge.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")
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
#define CONNECTION_TIMER_INTERVAL 1000 // ms
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
static constexpr auto maxResponseTime = std::chrono::milliseconds(10000);
static const char *progressTopic = "/nemo/progress";
static const char *heartbeatTopic = "/nemo/heartbeat";
using hrc = std::chrono::high_resolution_clock;
using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;
using ROSBridgePtr = std::shared_ptr<Rosbridge>;
typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
QList>
......@@ -103,6 +104,7 @@ private:
bool _sysSync() const; // thread safe
void _onFutureWatcherFinished(); // thread safe
void _onHeartbeatTimeout(); // thread safe
void _onRosbridgeStateChanged();
// called from dispatcher thread!
QVariant _callAddTiles(
......@@ -150,7 +152,7 @@ private:
std::atomic<STATE> _state;
std::atomic<CALL_NAME> _lastCall;
ROSBridgePtr _pRosBridge;
ROSBridgePtr _pRosbridge;
TileMap _remoteTiles;
TileMapConst _localTiles;
NemoInterface *const _parent;
......@@ -158,7 +160,6 @@ private:
QString _infoString;
QString _warningString;
QTimer _timeoutTimer;
QTimer _connectionTimer;
QNemoHeartbeat _lastHeartbeat;
FutureWatcher _futureWatcher;
};
......@@ -185,17 +186,11 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this] {
auto connectionString = connectionStringFact->rawValue().toString();
if (!is_valid_port_path(connectionString.toLocal8Bit().data())) {
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
bool wasRunning = this->running();
this->stop();
this->_pRosBridge = std::make_shared<RosbridgeWsClient>(
connectionString.toLocal8Bit().data());
this->_pRosbridge = std::make_shared<Rosbridge>(
QUrl(QString("ws://") + connectionString.toLocal8Bit().data()));
if (wasRunning) {
this->start();
}
......@@ -209,29 +204,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
std::bind(&Impl::_onHeartbeatTimeout, this));
// Connection timer (temporary workaround)
connect(&this->_connectionTimer, &QTimer::timeout, [this] {
if (this->_pRosBridge->connected()) {
if (this->_state == STATE::START_BRIDGE ||
this->_state == STATE::WEBSOCKET_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_DETECTED);
this->_doAction();
}
} else {
if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP ||
this->_state == STATE::READY ||
this->_state == STATE::WEBSOCKET_DETECTED ||
this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_TIMEOUT);
this->_doAction();
}
}
});
connect(this->_pRosbridge.get(), &Rosbridge::stateChanged,
[this] { this->_onRosbridgeStateChanged(); });
connect(&this->_futureWatcher, &FutureWatcher::finished,
[this] { this->_onFutureWatcherFinished(); });
}
NemoInterface::Impl::~Impl() { this->_pRosBridge->reset(); }
NemoInterface::Impl::~Impl() { this->_pRosbridge->stop(); }
void NemoInterface::Impl::start() {
if (!running()) {
......@@ -533,6 +513,25 @@ void NemoInterface::Impl::_onHeartbeatTimeout() {
this->_doAction();
}
void NemoInterface::Impl::_onRosbridgeStateChanged() {
auto state = this->_pRosbridge->state();
if (state == Rosbridge::STATE::CONNECTED) {
if (this->_state == STATE::START_BRIDGE ||
this->_state == STATE::WEBSOCKET_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_DETECTED);
this->_doAction();
}
} else if (state == Rosbridge::STATE::TIMEOUT) {
if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP ||
this->_state == STATE::READY ||
this->_state == STATE::WEBSOCKET_DETECTED ||
this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_TIMEOUT);
this->_doAction();
}
}
}
bool NemoInterface::Impl::_userSync() const { return _userSync(this->_state); }
const QString &NemoInterface::Impl::infoString() const { return _infoString; }
......@@ -600,113 +599,101 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
using namespace ros_bridge::messages;
// Subscribe nemo progress.
const char *progressClient = "client:/nemo/progress";
this->_pRosBridge->addClient(progressClient);
this->_pRosBridge->subscribe(
progressClient, "/nemo/progress",
[this](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
auto msg = in_message->string();
// parse in_message
rapidjson::Document d;
d.Parse(msg.c_str());
if (!d.HasParseError()) {
if (d.HasMember("msg") && d["msg"].IsObject()) {
// create obj from json
nemo_msgs::progress_array::ProgressArray progressArray;
if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) {
// correct range errors of progress
for (auto &lp : progressArray.progress_array()) {
bool rangeError = false;
if (lp.progress() < 0) {
lp.setProgress(0);
rangeError = true;
}
if (lp.progress() > 100) {
lp.setProgress(100);
rangeError = true;
}
if (rangeError) {
qCWarning(NemoInterfaceLog) << "/nemo/progress progress out "
"of range, value was set to: "
<< lp.progress();
}
}
auto p = std::make_shared<ProgressArray>();
*p = std::move(progressArray.progress_array());
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent,
[this, p, promise = std::move(promise)]() mutable {
this->_updateProgress(p, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
} else {
qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
"create ProgressArray form json: "
<< msg.c_str();
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
nemo_msgs::progress_array::ProgressArray progressArray;
if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) {
// correct range errors of progress
for (auto &lp : progressArray.progress_array()) {
bool rangeError = false;
if (lp.progress() < 0) {
lp.setProgress(0);
rangeError = true;
}
if (lp.progress() > 100) {
lp.setProgress(100);
rangeError = true;
}
if (rangeError) {
qCWarning(NemoInterfaceLog) << "/nemo/progress progress out "
"of range, value was set to: "
<< lp.progress();
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/progress no \"msg\" key or wrong type: "
<< msg.c_str();
}
auto p = std::make_shared<ProgressArray>();
*p = std::move(progressArray.progress_array());
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, p, promise = std::move(promise)]() mutable {
this->_updateProgress(p, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/progress message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
"create ProgressArray form json: "
<< msg;
}
});
} 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.
const char *heartbeatClient = "client:/nemo/heartbeat";
this->_pRosBridge->addClient(heartbeatClient);
this->_pRosBridge->subscribe(
heartbeatClient, "/nemo/heartbeat",
[this](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
auto msg = in_message->string();
// parse in_message
rapidjson::Document d;
d.Parse(msg.c_str());
if (!d.HasParseError()) {
if (d.HasMember("msg") && d["msg"].IsObject()) {
// create obj from json
nemo_msgs::heartbeat::Heartbeat heartbeat;
if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) {
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent,
[this, heartbeat, promise = std::move(promise)]() mutable {
this->_onHeartbeatReceived(heartbeat, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
} else {
qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
"create Heartbeat form json: "
<< msg.c_str();
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/heartbeat no \"msg\" key or wrong type: "
<< msg.c_str();
}
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
nemo_msgs::heartbeat::Heartbeat heartbeat;
if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) {
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent,
[this, heartbeat, promise = std::move(promise)]() mutable {
this->_onHeartbeatReceived(heartbeat, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/heartbeat message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
"create Heartbeat form json: "
<< msg;
}
});
} 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() {
......@@ -778,17 +765,15 @@ void NemoInterface::Impl::_doAction() {
static bool resetDone = false;
switch (this->_state) {
case STATE::STOPPED:
this->_connectionTimer.stop();
this->_timeoutTimer.stop();
this->_clearTilesRemote(std::promise<bool>());
if (this->_pRosBridge->running()) {
this->_pRosBridge->reset();
if (this->_pRosbridge->state() != Rosbridge::STATE::STOPPED) {
this->_pRosbridge->stop();
}
break;
case STATE::START_BRIDGE:
this->_pRosBridge->run();
this->_connectionTimer.start(CONNECTION_TIMER_INTERVAL);
this->_pRosbridge->start();
break;
break;
case STATE::WEBSOCKET_DETECTED:
......@@ -812,8 +797,8 @@ void NemoInterface::Impl::_doAction() {
case STATE::WEBSOCKET_TIMEOUT:
if (!resetDone) {
resetDone = true;
this->_pRosBridge->reset();
this->_pRosBridge->run();
this->_pRosbridge->stop();
this->_pRosbridge->start();
}
this->_timeoutTimer.stop();
this->_clearTilesRemote(std::promise<bool>());
......@@ -843,48 +828,48 @@ QVariant NemoInterface::Impl::_callAddTiles(
}
jsonTileArray.PushBack(jsonTile, allocator);
} // for
rapidjson::Value tileKey("in_tile_array");
request.AddMember(tileKey, jsonTileArray, allocator);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler.
auto promise_response = std::make_shared<std::promise<bool>>();
auto future_response = promise_response->get_future();
auto responseHandler =
[promise_response](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) mutable {
// check if transaction was successfull
auto msg = in_message->string();
rapidjson::Document d;
d.Parse(msg.c_str());
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.c_str();
promise_response->set_value(false);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/add_tiles no \"values\" key or wrong type: "
<< msg.c_str();
promise_response->set_value(false);
}
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 message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
<< "/nemo/add_tiles no \"success\" key or wrong type: " << msg;
promise_response->set_value(false);
}
connection->send_close(1000);
};
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/add_tiles no \"values\" key or wrong type: " << msg;
promise_response->set_value(false);
}
} else {
qCWarning(NemoInterfaceLog) << "/nemo/add_tiles message parse error ("
<< d.GetParseError() << "): " << msg;
promise_response->set_value(false);
}
};
// call service.
this->_pRosBridge->callService("/nemo/add_tiles", responseHandler, request);
this->_pRosbridge->callService("/nemo/add_tiles", responseHandler,
req.object());
// wait for response.
auto tStart = hrc::now();
......@@ -940,46 +925,44 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
rapidjson::Value tileKey("ids");
request.AddMember(tileKey, jsonIdArray, allocator);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler.
auto promise_response = std::make_shared<std::promise<bool>>();
auto future_response = promise_response->get_future();
auto responseHandler =
[promise_response](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) mutable {
// check if transaction was successfull
auto msg = in_message->string();
rapidjson::Document d;
d.Parse(msg.c_str());
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.c_str();
promise_response->set_value(false);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/remove_tiles no \"values\" key or wrong type: "
<< msg.c_str();
promise_response->set_value(false);
}
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 message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
<< "/nemo/remove_tiles no \"success\" key or wrong type: " << msg;
promise_response->set_value(false);
}
connection->send_close(1000);
};
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/remove_tiles no \"values\" key or wrong type: " << msg;
promise_response->set_value(false);
}
} else {
qCWarning(NemoInterfaceLog) << "/nemo/remove_tiles message parse error ("
<< d.GetParseError() << "): " << msg;
promise_response->set_value(false);
}
};
// call service.
this->_pRosBridge->callService("/nemo/remove_tiles", responseHandler,
request);
this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler,
req.object());
// wait for response.
auto tStart = hrc::now();
......@@ -1026,35 +1009,29 @@ QVariant NemoInterface::Impl::_callClearTiles() {
// create response handler.
auto promise_response = std::make_shared<std::promise<bool>>();
auto future_response = promise_response->get_future();
auto responseHandler =
[promise_response](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) mutable {
// check if transaction was successfull
auto msg = in_message->string();
rapidjson::Document d;
d.Parse(msg.c_str());
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.c_str();
promise_response->set_value(false);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/clear_tiles message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
promise_response->set_value(false);
}
connection->send_close(1000);
};
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);
}
};
// call service.
this->_pRosBridge->callService("/nemo/clear_tiles", responseHandler,
rapidjson::Document(rapidjson::kObjectType));
this->_pRosbridge->callService("/nemo/clear_tiles", responseHandler,
QJsonObject());
// wait for response.
auto tStart = hrc::now();
......@@ -1108,52 +1085,51 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
rapidjson::Value tileKey("ids");
request.AddMember(tileKey, jsonIdArray, allocator);
rapidjson::StringBuffer buffer;
rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
request.Accept(writer);
QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
// create response handler.
typedef std::shared_ptr<ProgressArray> ResponseType;
auto promise_response = std::make_shared<std::promise<ResponseType>>();
auto future_response = promise_response->get_future();
auto responseHandler =
[promise_response](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) mutable {
// check if transaction was successfull
auto msg = in_message->string();
rapidjson::Document d;
d.Parse(msg.c_str());
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<ProgressArray>();
*pArray = std::move(progressArrayMsg.progress_array());
promise_response->set_value(pArray);
} else {