Skip to content

Q
qgroundcontrol
Commit 22c1b55e authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

temp

parent 5beee98a
Hide whitespace changes
Inline Side-by-side
Showing with 1534 additions and 337 deletions
qgroundcontrol.pro
View file @ 22c1b55e
......@@ -35,7 +35,7 @@ DebugBuild {
#DEFINES += SNAKE_SHOW_TIME
#DEFINES += SNAKE_DEBUG
#DEFINES += SNAKE_SHOW_TIME
DEFINES += ROS_BRIDGE_DEBUG
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
......@@ -453,10 +453,12 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \
src/MeasurementComplexItem/nemo_interface/Task.h \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h \
src/MeasurementComplexItem/nemo_interface/tileHelper.h \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
src/QmlControls/QmlUnitsConversion.h \
src/MeasurementComplexItem/geometry/GeoArea.h \
......@@ -510,6 +512,7 @@ HEADERS += \
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 \
......@@ -529,9 +532,10 @@ SOURCES += \
src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/MeasurementComplexItem/HashFunctions.cpp \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
src/MeasurementComplexItem/nemo_interface/Task.cpp \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
......@@ -553,6 +557,7 @@ SOURCES += \
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 \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
......
src/MeasurementComplexItem/MeasurementComplexItem.cc
View file @ 22c1b55e
......@@ -2,6 +2,7 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "NemoInterface.h"
#include "RoutingThread.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
......@@ -128,6 +129,10 @@ MeasurementComplexItem::MeasurementComplexItem(
resetGenerators();
startEditing();
// connect to nemo interface
connect(pNemoInterface, &NemoInterface::progressChanged, this,
&MeasurementComplexItem::_onNewProgress);
}
MeasurementComplexItem::~MeasurementComplexItem() {}
......@@ -919,6 +924,69 @@ void MeasurementComplexItem::_reverseRoute() {
}
}
void MeasurementComplexItem::_syncTiles() {
auto areaArray = _pAreaData->measurementAreaArray();
bool clear = false;
if (areaArray.size() > 0) {
// create tile ptr array
TilePtrArray tilePtrArray;
auto *pMeasurementArea = areaArray[0];
auto pTiles = pMeasurementArea->tiles();
for (int i = 0; i < pTiles->count(); ++i) {
auto *tile = pTiles->value<MeasurementTile *>(i);
Q_ASSERT(tile != nullptr);
tilePtrArray.push_back(tile);
}
if (tilePtrArray.size() > 0) {
// create id array
IDArray idArray;
for (const auto *pTile : tilePtrArray) {
idArray.push_back(pTile->id());
}
// sync. necessary?
bool doSync = false;
auto contains = pNemoInterface->containsTiles(idArray);
for (auto &&logical : contains) {
if (logical == false) {
doSync = true;
break;
}
}
if (doSync) {
if (!pNemoInterface->empty()) {
(void)pNemoInterface->clearTiles();
}
(void)pNemoInterface->addTiles(tilePtrArray);
return;
}
} else {
clear = true;
}
} else {
clear = true;
}
if (clear) {
if (!pNemoInterface->empty()) {
(void)pNemoInterface->clearTiles();
}
}
}
void MeasurementComplexItem::_onNewProgress(const ProgressArray &array) {
auto areaArray = this->_pAreaData->measurementAreaArray();
if (areaArray.size() > 0) {
for (auto &area : areaArray) {
area->updateProgress(array);
}
}
}
ComplexMissionItem::ReadyForSaveState
MeasurementComplexItem::readyForSaveState() const {
if (idle()) {
......@@ -1145,6 +1213,10 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
_updateRoute();
}
if (correct && isDifferent) {
_syncTiles();
}
return updated;
}
return false;
......@@ -1153,6 +1225,7 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
void MeasurementComplexItem::abortEditing() {
if (editing()) {
_setAreaData(_pAreaData);
_syncTiles();
_setState(STATE::IDLE);
}
}
......
src/MeasurementComplexItem/MeasurementComplexItem.h
View file @ 22c1b55e
......@@ -10,6 +10,7 @@
#include "SettingsFact.h"
#include "AreaData.h"
#include "ProgressArray.h"
class RoutingThread;
class RoutingResult;
......@@ -238,6 +239,8 @@ private slots:
void _updateRoute();
void _changeVariantIndex();
void _reverseRoute();
void _syncTiles();
void _onNewProgress(const ProgressArray &array);
private:
bool _setGenerator(PtrGenerator newG);
......
src/MeasurementComplexItem/NemoInterface.cpp
View file @ 22c1b55e
......@@ -14,9 +14,12 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/CommandDispatcher.h"
#include "nemo_interface/FutureWatcher.h"
#include "nemo_interface/MeasurementTile.h"
#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"
......@@ -29,14 +32,23 @@
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define EVENT_TIMER_INTERVAL 100 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
auto constexpr static maxResponseTime = std::chrono::milliseconds(10000);
using hrc = std::chrono::high_resolution_clock;
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 10000 // ms
#define CONNECTION_TIMER_INTERVAL 1000 // ms
static constexpr auto maxResponseTime = std::chrono::milliseconds(10000);
using hrc = std::chrono::high_resolution_clock;
using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;
using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
using SharedLock = std::shared_lock<std::shared_timed_mutex>;
typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
QList>
Tile;
typedef std::map<std::int64_t, std::shared_ptr<Tile>> TileMap;
typedef std::map<std::int64_t, std::shared_ptr<const Tile>> TileMapConst;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::TaskDispatcher Dispatcher;
typedef nemo_interface::FutureWatcher<QVariant, std::shared_future>
FutureWatcher;
class NemoInterface::Impl {
enum class STATE {
......@@ -44,8 +56,11 @@ class NemoInterface::Impl {
START_BRIDGE,
WEBSOCKET_DETECTED,
TRY_TOPIC_SERVICE_SETUP,
SYNC_USER,
SYNC_SYS,
READY,
TIMEOUT
WEBSOCKET_TIMEOUT,
HEARTBEAT_TIMEOUT
};
public:
......@@ -57,9 +72,9 @@ public:
// Tile editing.
// Functions that require communication to device.
std::future<QVariant> addTiles(const TileArray &tileArray);
void removeTiles(const IDArray &idArray);
void clearTiles();
std::shared_future<QVariant> addTiles(const TilePtrArray &tileArray);
std::shared_future<QVariant> removeTiles(const IDArray &idArray);
std::shared_future<QVariant> clearTiles();
// Functions that don't require communication to device.
TileArray getTiles(const IDArray &idArray);
......@@ -79,51 +94,69 @@ public:
const QString &infoString();
const QString &warningString();
void _addTilesLocal(const TileArray &tileArray);
void _removeTilesLocal(const IDArray &idArray);
void _clearTilesLocal();
void _updateProgress(ProgressArray progressArray);
void _setHeartbeat(const QNemoHeartbeat &hb);
void _setInfoString(const QString &info);
void _setWarningString(const QString &warning);
private:
typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint;
typedef std::map<long, MeasurementTile> TileMap;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::CommandDispatcher Dispatcher;
void _doTopicServiceSetup();
void _doAction(); // does action according to state
void _doAction();
void _trySynchronize();
bool _isSynchronized();
bool _userSync(); // thread safe
bool _sysSync(); // thread safe
void _onFutureWatcherFinished(); // thread safe
// called from dispatcher thread!
QVariant _callAddTiles(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray);
// called from dispatcher thread!
QVariant _callRemoveTiles(std::shared_ptr<IDArray> pIdArray);
// called from dispatcher thread!
QVariant _callClearTiles();
// called from dispatcher thread!
QVariant _callGetProgress(std::shared_ptr<IDArray> pIdArray);
void _addTilesRemote(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray);
void
_addTilesRemote(std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray);
void _removeTilesRemote(std::shared_ptr<IDArray> idArray);
void _clearTilesRemote();
void _updateProgress(std::shared_ptr<ProgressArray> pArray);
void _onHeartbeatReceived(const QNemoHeartbeat &hb);
void _setInfoString(const QString &info);
void _setWarningString(const QString &warning);
bool _setState(STATE s); // not thread safe
bool _setState(STATE newState); // not thread safe
static bool _ready(STATE s);
static bool _userSync(STATE s);
static bool _sysSync(STATE s);
static bool _running(STATE s);
static void _translate(STATE state, NemoInterface::STATUS &status);
static void _translate(Heartbeat hb, STATE &state);
static NemoInterface::STATUS _status(STATE state);
static QString _toString(STATE s);
static QString _toString(NemoInterface::STATUS s);
std::atomic<STATE> _state;
ROSBridgePtr _pRosBridge;
TileMap _tileMap;
NemoInterface *_parent;
TileMap _remoteTiles;
TileMapConst _localTiles;
NemoInterface *const _parent;
Dispatcher _dispatcher;
QString _infoString;
QString _warningString;
QTimer _timeoutTimer;
QTimer _connectionTimer;
QNemoHeartbeat _lastHeartbeat;
FutureWatcher _futureWatcher;
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::SYNC,
"Synchronizing"),
std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::READY,
"Ready"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::TIMEOUT, "Timeout"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
......@@ -150,32 +183,38 @@ NemoInterface::Impl::Impl(NemoInterface *p)
this->_pRosBridge = std::make_shared<RosbridgeWsClient>(
connectionString.toLocal8Bit().data());
this->_pRosBridge->reset();
qCritical() << "NemoInterface: add reset code here";
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
// Heartbeat timeout.
connect(&this->_timeoutTimer, &QTimer::timeout,
[this] { this->_setState(STATE::TIMEOUT); });
connect(&this->_timeoutTimer, &QTimer::timeout, [this] {
this->_setState(STATE::HEARTBEAT_TIMEOUT);
this->_doAction();
});
// Connection timer (temporary workaround)
connect(&this->_connectionTimer, &QTimer::timeout, [this] {
if (this->_pRosBridge->connected()) {
if (this->_state == STATE::START_BRIDGE ||
this->_state == STATE::TIMEOUT) {
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->_setState(STATE::TIMEOUT);
this->_state == STATE::READY ||
this->_state == STATE::WEBSOCKET_DETECTED ||
this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_TIMEOUT);
this->_doAction();
}
}
});
connect(&this->_futureWatcher, &FutureWatcher::finished,
[this] { this->_onFutureWatcherFinished(); });
}
NemoInterface::Impl::~Impl() {}
......@@ -190,91 +229,118 @@ void NemoInterface::Impl::start() {
void NemoInterface::Impl::stop() {
if (running()) {
this->_setState(STATE::STOPPED);
this->_connectionTimer.stop();
this->_doAction();
}
}
std::future<QVariant>
NemoInterface::Impl::addTiles(const TileArray &tileArray) {
std::shared_future<QVariant>
NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
using namespace nemo_interface;
if (this->ready()) {
if (tileArray.size() > 0) {
// copy unknown tiles
auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
for (const auto *pTile : tileArray) {
auto id = pTile->id();
const auto it = this->_localTiles.find(id);
Q_ASSERT(it == _localTiles.end());
if (Q_LIKELY(it == _localTiles.end())) {
auto pTileCopy =
std::make_shared<const Tile>(pTile->coordinateList(), 0.0, id);
_localTiles.insert(std::make_pair(id, pTileCopy));
pTileArray->push_back(pTileCopy);
} else {
qCDebug(NemoInterfaceLog)
<< "addTiles(): tile with id: " << pTile->id() << "already added.";
}
}
// create command.
auto pRosBridge = this->_pRosBridge;
auto pDispatcher = &this->_dispatcher;
Task sendTilesCommand([pRosBridge, tileArray, pDispatcher,
this](std::promise<QVariant> promise) {
// create json object
rapidjson::Document request;
auto &allocator = request.GetAllocator();
rapidjson::Value jsonTileArray(rapidjson::kArrayType);
for (const auto &tile : tileArray) {
const auto it = _tileMap.find(tile.id());
if (Q_LIKELY(it == _tileMap.end())) {
using namespace ros_bridge::messages;
rapidjson::Value jsonTile(rapidjson::kObjectType);
if (!nemo_msgs::tile::toJson(tile, jsonTile, allocator)) {
qCDebug(NemoInterfaceLog)
<< "addTiles(): not able to create json object: tile id: "
<< tile.id() << " progress: " << tile.progress()
<< " points: " << tile.path();
}
jsonTileArray.PushBack(jsonTile, allocator);
}
} // for
rapidjson::Value tileKey("in_tile_array");
request.AddMember(tileKey, jsonTileArray, allocator);
// create response handler.
auto promise_response = std::make_shared<std::promise<void>>();
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 {
qDebug() << "addTiles: in_message" << in_message->string().c_str();
promise_response->set_value();
connection->send_close(1000);
};
// call service.
pRosBridge->callService("/nemo/add_tiles", responseHandler, request);
// wait for response.
auto tStart = hrc::now();
bool abort = true;
do {
auto status = future_response.wait_for(std::chrono::milliseconds(100));
if (status == std::future_status::ready) {
abort = false;
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
pDispatcher->interruptionPoint());
// ready for send?
if (pTileArray->size() > 0 && (this->ready() || this->_userSync())) {
this->_setState(STATE::SYNC_USER);
this->_doAction();
if (abort) {
qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
promise.set_value(QVariant(false));
return;
// create command.
auto sendTilesCommand = std::make_unique<Task>(
std::bind(&Impl::_callAddTiles, this, pTileArray));
// dispatch command and return.
auto ret = _dispatcher.dispatch(std::move(sendTilesCommand));
auto sfut = ret.share();
_futureWatcher.setFuture(sfut);
return sfut;
}
}
std::promise<QVariant> p;
p.set_value(QVariant(false));
return p.get_future();
}
std::shared_future<QVariant>
NemoInterface::Impl::removeTiles(const IDArray &idArray) {
using namespace nemo_interface;
if (idArray.size() > 0) {
// copy known ids
auto pIdArray = std::make_shared<IDArray>();
for (const auto id : idArray) {
const auto it = this->_localTiles.find(id);
Q_ASSERT(it != _localTiles.end());
if (Q_LIKELY(it != _localTiles.end())) {
_localTiles.erase(it);
pIdArray->push_back(id);
} else {
qCDebug(NemoInterfaceLog) << "removeTiles(): unknown id: " << id << ".";
}
}
// ready for send?
if (pIdArray->size() > 0 && (this->ready() || this->_userSync())) {
this->_setState(STATE::SYNC_USER);
this->_doAction();
// create command.
auto cmd = std::make_unique<Task>(
std::bind(&Impl::_callRemoveTiles, this, pIdArray));
// dispatch command and return.
auto ret = _dispatcher.dispatch(std::move(cmd));
auto sfut = ret.share();
_futureWatcher.setFuture(sfut);
return sfut;
}
}
std::promise<QVariant> p;
p.set_value(QVariant(false));
return p.get_future();
}
qCritical() << "addTiles(): ToDo: add return value checking here.";
std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
using namespace nemo_interface;
// update local tiles
QMetaObject::invokeMethod(
this->_parent, std::bind(&Impl::_addTilesLocal, this, tileArray));
// clear local tiles (_localTiles)
this->_localTiles.clear();
if (this->_localTiles.size() > 0 && (this->ready() || this->_userSync())) {
this->_setState(STATE::SYNC_USER);
this->_doAction();
// return success
promise.set_value(QVariant(true));
return;
}); // sendTilesCommand
// create command.
auto pTask =
std::make_unique<Task>(std::bind(&Impl::_callClearTiles, this));
// dispatch command and return.
auto ret = _dispatcher.dispatch(sendTilesCommand);
return ret;
auto ret = _dispatcher.dispatch(std::move(pTask));
auto sfut = ret.share();
_futureWatcher.setFuture(sfut);
return sfut;
} else {
std::promise<QVariant> p;
p.set_value(QVariant(false));
......@@ -286,9 +352,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray tileArray;
for (const auto &id : idArray) {
const auto it = _tileMap.find(id);
if (it != _tileMap.end()) {
tileArray.append(it->second);
const auto it = _localTiles.find(id);
if (it != _localTiles.end()) {
MeasurementTile copy;
copy.setId(it->second->id());
copy.setProgress(it->second->progress());
copy.setPath(it->second->tile());
tileArray.append(std::move(copy));
}
}
......@@ -298,8 +368,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray NemoInterface::Impl::getAllTiles() {
TileArray tileArray;
for (const auto &entry : _tileMap) {
tileArray.append(entry.second);
for (const auto &entry : _localTiles) {
auto pTile = entry.second;
MeasurementTile copy;
copy.setId(pTile->id());
copy.setProgress(pTile->progress());
copy.setPath(pTile->tile());
tileArray.append(std::move(copy));
}
return tileArray;
......@@ -309,22 +384,30 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) {
LogicalArray logicalArray;
for (const auto &id : idArray) {
const auto &it = _tileMap.find(id);
logicalArray.append(it != _tileMap.end());
const auto &it = _localTiles.find(id);
logicalArray.append(it != _localTiles.end());
}
return logicalArray;
}
std::size_t NemoInterface::Impl::size() { return _tileMap.size(); }
std::size_t NemoInterface::Impl::size() { return _localTiles.size(); }
bool NemoInterface::Impl::empty() { return _tileMap.empty(); }
bool NemoInterface::Impl::empty() { return _localTiles.empty(); }
ProgressArray NemoInterface::Impl::getProgress() {
ProgressArray progressArray;
for (const auto &entry : _tileMap) {
progressArray.append(TaggedProgress{entry.first, entry.second.progress()});
if (this->_isSynchronized()) {
for (const auto &entry : _remoteTiles) {
progressArray.append(
LabeledProgress{entry.second->progress(), entry.second->id()});
}
} else {
for (const auto &entry : _localTiles) {
progressArray.append(
LabeledProgress{entry.second->progress(), entry.second->id()});
}
}
return progressArray;
......@@ -333,10 +416,21 @@ ProgressArray NemoInterface::Impl::getProgress() {
ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
ProgressArray progressArray;
for (const auto &id : idArray) {
const auto it = _tileMap.find(id);
if (it != _tileMap.end()) {
progressArray.append(TaggedProgress{it->first, it->second.progress()});
if (this->_isSynchronized()) {
for (const auto &id : idArray) {
const auto it = _remoteTiles.find(id);
if (it != _remoteTiles.end()) {
progressArray.append(
LabeledProgress{it->second->progress(), it->second->id()});
}
}
} else {
for (const auto &id : idArray) {
const auto it = _localTiles.find(id);
if (it != _localTiles.end()) {
progressArray.append(
LabeledProgress{it->second->progress(), it->second->id()});
}
}
}
......@@ -344,93 +438,76 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
}
NemoInterface::STATUS NemoInterface::Impl::status() {
NemoInterface::STATUS status;
_translate(this->_state, status);
return status;
return _status(this->_state);
}
bool NemoInterface::Impl::running() { return _running(this->_state); }
bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); }
const QString &NemoInterface::Impl::infoString() { return _infoString; }
const QString &NemoInterface::Impl::warningString() { return _warningString; }
bool NemoInterface::Impl::_sysSync() { return _sysSync(this->_state); }
void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
bool anyChanges = false;
void NemoInterface::Impl::_onFutureWatcherFinished() {
auto lastTransactionSuccessfull = _futureWatcher.result().toBool();
if (!lastTransactionSuccessfull) {
if (this->_userSync()) {
_trySynchronize();
} else if (this->_sysSync()) {
QTimer::singleShot(1000, [this] { this->_trySynchronize(); });
}
} else {
// fetch progress
for (const auto &tile : tileArray) {
const auto id = tile.id();
const auto it = _tileMap.find(id);
if (Q_LIKELY(it == _tileMap.end())) {
auto ret = _tileMap.insert(std::make_pair(id, tile));
anyChanges = true;
Q_ASSERT(ret.second == true);
Q_UNUSED(ret);
} else {
qCDebug(NemoInterfaceLog)
<< "_addTilesLocal(): tile with id " << id << " already inserted.";
auto pIdArray = std::make_shared<IDArray>();
for (const auto &pair : _remoteTiles) {
pIdArray->push_back(pair.first);
}
}
auto pTask = std::make_unique<nemo_interface::Task>(
std::bind(&Impl::_callGetProgress, this, pIdArray));
if (anyChanges) {
emit _parent->tilesChanged();
// dispatch command.
auto ret = _dispatcher.dispatch(std::move(pTask));
Q_ASSERT(false);
_futureWatcher.setFuture(ret.share());
}
}
void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) {
bool anyChanges = false;
for (const auto &id : idArray) {
const auto it = _tileMap.find(id);
if (Q_LIKELY(it != _tileMap.end())) {
_tileMap.erase(it);
anyChanges = true;
} else {
qCDebug(NemoInterfaceLog)
<< "_removeTilesLocal(): tile with id " << id << " not found.";
}
}
bool NemoInterface::Impl::_userSync() { return _userSync(this->_state); }
if (anyChanges) {
emit _parent->tilesChanged();
}
}
const QString &NemoInterface::Impl::infoString() { return _infoString; }
void NemoInterface::Impl::_clearTilesLocal() {
if (!_tileMap.empty()) {
_tileMap.clear();
emit _parent->tilesChanged();
}
}
const QString &NemoInterface::Impl::warningString() { return _warningString; }
void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
for (auto itPair = progressArray.begin(); itPair != progressArray.end();) {
void NemoInterface::Impl::_updateProgress(
std::shared_ptr<ProgressArray> pArray) {
for (auto itLP = pArray->begin(); itLP != pArray->end();) {
const auto &id = itPair->first;
auto it = _tileMap.find(id);
auto it = _remoteTiles.find(itLP->id());
if (Q_LIKELY(it != _tileMap.end())) {
const auto &progress = itPair->second;
it->second.setProgress(progress);
++itPair;
if (Q_LIKELY(it != _remoteTiles.end())) {
it->second->setProgress(itLP->progress());
++itLP;
} else {
qCDebug(NemoInterfaceLog)
<< "_updateProgress(): tile with id " << id << " not found.";
itPair = progressArray.erase(itPair);
<< "_updateProgress(): tile with id " << itLP->id() << " not found.";
itLP = pArray->erase(itLP);
}
}
emit _parent->progressChanged(progressArray);
if (pArray->size() > 0) {
emit _parent->progressChanged(*pArray);
}
}
void NemoInterface::Impl::_setHeartbeat(const QNemoHeartbeat &hb) {
if (this->_lastHeartbeat != hb) {
_lastHeartbeat = hb;
if (ready()) {
this->_timeoutTimer.stop();
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
}
void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb) {
_lastHeartbeat = hb;
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP) {
this->_setState(STATE::READY);
this->_doAction();
} else if (this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::READY);
this->_doAction();
}
}
......@@ -456,57 +533,611 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
this->_pRosBridge->addClient(progressClient);
this->_pRosBridge->subscribe(
progressClient, "/nemo/progress",
[this](std::shared_ptr<WsClient::Connection> connection,
[this](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
qDebug() << "doTopicServiceSetup(): /nemo/progress: "
<< in_message->string().c_str();
qDebug() << "impl missing";
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());
QMetaObject::invokeMethod(
this->_parent,
std::bind(&NemoInterface::Impl::_updateProgress, this, p));
} else {
qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
"create ProgressArray form json: "
<< msg.c_str();
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/progress no \"msg\" key or wrong type: "
<< msg.c_str();
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/progress message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
}
});
// 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> connection,
[this](std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) {
qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: "
<< in_message->string().c_str();
qDebug() << "impl missing";
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)) {
QMetaObject::invokeMethod(
this->_parent,
std::bind(&NemoInterface::Impl::_onHeartbeatReceived, this,
heartbeat));
} 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();
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/heartbeat message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
}
});
}
void NemoInterface::Impl::_trySynchronize() {
if (!_isSynchronized()) {
this->_setState(STATE::SYNC_SYS);
this->_doAction();
// create clear cmd.
auto pTask = std::make_unique<nemo_interface::Task>(
std::bind(&Impl::_callClearTiles, this));
// dispatch command.
qCritical() << "this assert is triggered sometimes! sdf92894";
Q_ASSERT(_dispatcher.pendingTasks() == 0);
auto ret = _dispatcher.dispatch(std::move(pTask));
// create tile array.
auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
for (auto pair : _localTiles) {
pTileArray->push_back(pair.second);
}
// create addTiles cmd.
auto sendTilesCommand = std::make_unique<nemo_interface::Task>(
std::bind(&Impl::_callAddTiles, this, pTileArray));
// dispatch command.
ret = _dispatcher.dispatch(std::move(sendTilesCommand));
_futureWatcher.setFuture(ret.share());
}
}
bool NemoInterface::Impl::_isSynchronized() {
return _localTiles.size() > 0 && _remoteTiles.size() > 0 &&
std::equal(
_localTiles.begin(), _localTiles.end(), _remoteTiles.begin(),
[](const auto &a, const auto &b) { return a.first == b.first; });
}
void NemoInterface::Impl::_doAction() {
// Check ROS Bridge status and do setup if necessary.
static bool resetDone = false;
switch (this->_state) {
case STATE::STOPPED:
this->_connectionTimer.stop();
this->_timeoutTimer.stop();
this->_clearTilesRemote();
if (this->_pRosBridge->running()) {
this->_pRosBridge->reset();
}
break;
case STATE::START_BRIDGE:
case STATE::TIMEOUT:
this->_pRosBridge->reset();
this->_pRosBridge->run();
this->_connectionTimer.start(EVENT_TIMER_INTERVAL);
this->_connectionTimer.start(CONNECTION_TIMER_INTERVAL);
break;
break;
case STATE::WEBSOCKET_DETECTED:
resetDone = false;
this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
this->_doAction();
break;
case STATE::TRY_TOPIC_SERVICE_SETUP:
this->_doTopicServiceSetup();
this->_setState(STATE::READY);
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
break;
case STATE::READY:
_trySynchronize();
break;
case STATE::SYNC_USER:
case STATE::SYNC_SYS:
break;
case STATE::HEARTBEAT_TIMEOUT:
this->_clearTilesRemote();
break;
case STATE::WEBSOCKET_TIMEOUT:
if (!resetDone) {
resetDone = true;
this->_pRosBridge->reset();
this->_pRosBridge->run();
}
this->_clearTilesRemote();
break;
};
}
bool NemoInterface::Impl::_setState(STATE s) {
if (s != this->_state) {
this->_state = s;
emit this->_parent->statusChanged();
QVariant NemoInterface::Impl::_callAddTiles(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray) {
// create json object
rapidjson::Document request(rapidjson::kObjectType);
auto &allocator = request.GetAllocator();
rapidjson::Value jsonTileArray(rapidjson::kArrayType);
for (const auto &tile : *pTileArray) {
using namespace ros_bridge::messages;
rapidjson::Value jsonTile(rapidjson::kObjectType);
if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) {
qCDebug(NemoInterfaceLog)
<< "addTiles(): not able to create json object: tile id: "
<< tile->id() << " progress: " << tile->progress()
<< " points: " << tile->tile();
}
jsonTileArray.PushBack(jsonTile, allocator);
} // for
rapidjson::Value tileKey("in_tile_array");
request.AddMember(tileKey, jsonTileArray, allocator);
// 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);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/add_tiles message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
promise_response->set_value(false);
}
connection->send_close(1000);
};
// call service.
this->_pRosBridge->callService("/nemo/add_tiles", responseHandler, request);
// wait for response.
auto tStart = hrc::now();
bool abort = true;
do {
auto status = future_response.wait_for(std::chrono::milliseconds(100));
if (status == std::future_status::ready) {
abort = false;
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "addTiles(): Websocket not responding to request.";
return QVariant(false);
}
// transaction error?
if (!future_response.get()) {
return QVariant(false);
}
// add remote tiles (_remoteTiles)
QMetaObject::invokeMethod(this->_parent /* context */, [this, pTileArray] {
this->_addTilesRemote(pTileArray);
});
// return success
return QVariant(true);
}
QVariant
NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
// create json object
rapidjson::Document request(rapidjson::kObjectType);
auto &allocator = request.GetAllocator();
rapidjson::Value jsonIdArray(rapidjson::kArrayType);
for (const auto id : *pIdArray) {
using namespace ros_bridge::messages;
jsonIdArray.PushBack(rapidjson::Value(id), allocator);
} // for
rapidjson::Value tileKey("ids");
request.AddMember(tileKey, jsonIdArray, allocator);
// 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);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/remove_tiles message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
promise_response->set_value(false);
}
connection->send_close(1000);
};
// call service.
this->_pRosBridge->callService("/nemo/remove_tiles", responseHandler,
request);
// wait for response.
auto tStart = hrc::now();
bool abort = true;
do {
auto status = future_response.wait_for(std::chrono::milliseconds(100));
if (status == std::future_status::ready) {
abort = false;
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "remove_tiles(): Websocket not responding to request.";
return QVariant(false);
}
// transaction error?
if (!future_response.get()) {
return QVariant(false);
}
// remove remote tiles (_remoteTiles)
QMetaObject::invokeMethod(this->_parent /* context */, [this, pIdArray] {
this->_removeTilesRemote(pIdArray);
});
// return success
return QVariant(true);
}
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);
};
// call service.
this->_pRosBridge->callService("/nemo/clear_tiles", responseHandler,
rapidjson::Document(rapidjson::kObjectType));
// wait for response.
auto tStart = hrc::now();
bool abort = true;
do {
auto status = future_response.wait_for(std::chrono::milliseconds(100));
if (status == std::future_status::ready) {
abort = false;
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
return QVariant(false);
}
// transaction failed?
if (!future_response.get()) {
return QVariant(false);
}
// clear remote tiles (_remoteTiles)
QMetaObject::invokeMethod(this->_parent,
std::bind(&Impl::_clearTilesRemote, this));
// return success
return QVariant(true);
}
QVariant
NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
// create json object
rapidjson::Document request(rapidjson::kObjectType);
auto &allocator = request.GetAllocator();
rapidjson::Value jsonIdArray(rapidjson::kArrayType);
for (const auto id : *pIdArray) {
using namespace ros_bridge::messages;
jsonIdArray.PushBack(rapidjson::Value(id), allocator);
} // for
rapidjson::Value tileKey("ids");
request.AddMember(tileKey, jsonIdArray, allocator);
// 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 {
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.c_str();
promise_response->set_value(nullptr);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_progress message parse error (" << d.GetParseError()
<< "): " << msg.c_str();
promise_response->set_value(nullptr);
}
connection->send_close(1000);
};
// call service.
this->_pRosBridge->callService("/nemo/get_progress", responseHandler,
request);
// wait for response.
auto tStart = hrc::now();
bool abort = true;
do {
auto status = future_response.wait_for(std::chrono::milliseconds(100));
if (status == std::future_status::ready) {
abort = false;
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "remove_tiles(): Websocket not responding to request.";
return QVariant(false);
}
// transaction error?
auto pArray = future_response.get();
if (pArray == nullptr) {
return QVariant(false);
}
// remove remote tiles (_remoteTiles)
QMetaObject::invokeMethod(this->_parent /* context */,
[this, pArray] { this->_updateProgress(pArray); });
// return success
return QVariant(true);
}
void NemoInterface::Impl::_addTilesRemote(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray) {
auto pArrayDup = std::make_shared<QVector<std::shared_ptr<Tile>>>();
for (auto pTile : *pTileArray) {
pArrayDup->push_back(std::make_shared<Tile>(*pTile));
}
_addTilesRemote(pArrayDup);
}
void NemoInterface::Impl::_addTilesRemote(
std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray) {
bool anyChange = false;
for (auto pTile : *pTileArray) {
auto id = pTile->id();
auto it = _remoteTiles.find(id);
qCritical() << "this assert is triggered sometimes! 1212341242";
Q_ASSERT(it == _remoteTiles.end());
if (Q_LIKELY(it == _remoteTiles.end())) {
auto ret = _remoteTiles.insert(std::make_pair(id, pTile));
Q_ASSERT(ret.second == true);
Q_UNUSED(ret);
anyChange = true;
} else {
qCWarning(NemoInterfaceLog)
<< "_addTilesRemote: tile with id " << id << " already added.";
}
}
if (anyChange) {
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
this->_parent->tilesChanged();
}
}
void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray) {
bool anyChange = false;
for (const auto id : *idArray) {
auto it = _remoteTiles.find(id);
Q_ASSERT(it != _remoteTiles.end());
if (Q_LIKELY(it != _remoteTiles.end())) {
_remoteTiles.erase(it);
anyChange = true;
} else {
qCWarning(NemoInterfaceLog)
<< "_removeTilesRemote: tile with unknown id " << id << ".";
}
}
if (anyChange) {
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
this->_parent->tilesChanged();
}
}
void NemoInterface::Impl::_clearTilesRemote() {
if (_remoteTiles.size() > 0) {
_remoteTiles.clear();
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
this->_parent->tilesChanged();
}
}
bool NemoInterface::Impl::_setState(STATE newState) {
if (newState != this->_state) {
auto oldState = this->_state.load();
this->_state = newState;
qCDebug(NemoInterfaceLog)
<< "state: " << _toString(oldState) << " -> " << _toString(newState);
auto oldStatus = _status(oldState);
auto newStatus = _status(newState);
if (oldStatus != newStatus) {
qCDebug(NemoInterfaceLog) << "status: " << _toString(oldStatus) << " -> "
<< _toString(newStatus);
emit this->_parent->statusChanged();
}
if (_running(oldState) != _running(newState)) {
emit this->_parent->runningChanged();
}
return true;
} else {
return false;
......@@ -517,10 +1148,90 @@ bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) {
return s == STATE::READY;
}
bool NemoInterface::Impl::_userSync(NemoInterface::Impl::STATE s) {
return s == STATE::SYNC_USER;
}
bool NemoInterface::Impl::_sysSync(NemoInterface::Impl::STATE s) {
return s == STATE::SYNC_SYS;
}
bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) {
return s != STATE::STOPPED;
}
NemoInterface::STATUS
NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
NemoInterface::STATUS status;
switch (state) {
case STATE::STOPPED:
status = NemoInterface::STATUS::NOT_CONNECTED;
break;
case STATE::START_BRIDGE:
status = NemoInterface::STATUS::NOT_CONNECTED;
break;
case STATE::WEBSOCKET_DETECTED:
status = NemoInterface::STATUS::WEBSOCKET_DETECTED;
break;
case STATE::TRY_TOPIC_SERVICE_SETUP:
status = NemoInterface::STATUS::WEBSOCKET_DETECTED;
break;
case STATE::READY:
status = NemoInterface::STATUS::READY;
break;
case STATE::SYNC_USER:
case STATE::SYNC_SYS:
status = NemoInterface::STATUS::SYNC;
break;
case STATE::WEBSOCKET_TIMEOUT:
case STATE::HEARTBEAT_TIMEOUT:
status = NemoInterface::STATUS::TIMEOUT;
break;
}
return status;
}
QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
switch (s) {
case STATE::STOPPED:
return QString("STOPPED");
case STATE::START_BRIDGE:
return QString("START_BRIDGE");
case STATE::WEBSOCKET_DETECTED:
return QString("WEBSOCKET_DETECTED");
case STATE::TRY_TOPIC_SERVICE_SETUP:
return QString("TRY_TOPIC_SERVICE_SETUP");
case STATE::READY:
return QString("READY");
case STATE::SYNC_USER:
return QString("SYNC_USER");
case STATE::SYNC_SYS:
return QString("SYNC_SYS");
case STATE::WEBSOCKET_TIMEOUT:
return QString("WEBSOCKET_TIMEOUT");
case STATE::HEARTBEAT_TIMEOUT:
return QString("HEARTBEAT_TIMEOUT");
}
return "unknown state!";
}
QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
switch (s) {
case NemoInterface::STATUS::NOT_CONNECTED:
return QString("NOT_CONNECTED");
case NemoInterface::STATUS::READY:
return QString("READY");
case NemoInterface::STATUS::TIMEOUT:
return QString("TIMEOUT");
case NemoInterface::STATUS::WEBSOCKET_DETECTED:
return QString("WEBSOCKET_DETECTED");
case NemoInterface::STATUS::SYNC:
return QString("SYNC");
}
return "unknown state!";
}
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface()
......@@ -539,15 +1250,28 @@ void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
void NemoInterface::addTiles(const TileArray &tileArray) {
this->pImpl->addTiles(tileArray);
std::shared_future<QVariant>
NemoInterface::addTiles(const TileArray &tileArray) {
TilePtrArray ptrArray;
for (const auto &tile : tileArray) {
ptrArray.push_back(const_cast<MeasurementTile *>(&tile));
}
return this->pImpl->addTiles(ptrArray);
}
void NemoInterface::removeTiles(const IDArray &idArray) {
this->pImpl->removeTiles(idArray);
std::shared_future<QVariant>
NemoInterface::addTiles(const TilePtrArray &tileArray) {
return this->pImpl->addTiles(tileArray);
}
void NemoInterface::clearTiles() { this->pImpl->clearTiles(); }
std::shared_future<QVariant>
NemoInterface::removeTiles(const IDArray &idArray) {
return this->pImpl->removeTiles(idArray);
}
std::shared_future<QVariant> NemoInterface::clearTiles() {
return this->pImpl->clearTiles();
}
TileArray NemoInterface::getTiles(const IDArray &idArray) {
return this->pImpl->getTiles(idArray);
......
src/MeasurementComplexItem/NemoInterface.h
View file @ 22c1b55e
......@@ -2,7 +2,9 @@
#include <QGeoCoordinate>
#include <QObject>
#include <QVariant>
#include <future>
#include <memory>
#include "IDArray.h"
......@@ -28,10 +30,10 @@ public:
enum class STATUS {
NOT_CONNECTED,
SYNC,
READY,
WEBSOCKET_DETECTED,
TIMEOUT,
INVALID_HEARTBEAT
};
Q_ENUM(STATUS)
......@@ -49,9 +51,28 @@ public:
Q_INVOKABLE void stop();
// Tile editing.
void addTiles(const TileArray &tileArray);
void removeTiles(const IDArray &idArray);
void clearTiles();
//!
//! \brief addTiles
//! \param tileArray
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> addTiles(const TileArray &tileArray);
std::shared_future<QVariant> addTiles(const TilePtrArray &tileArray);
//!
//! \brief removeTiles
//! \param idArray
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> removeTiles(const IDArray &idArray);
//!
//! \brief clearTiles
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> clearTiles();
TileArray getTiles(const IDArray &idArray);
TileArray getAllTiles();
LogicalArray containsTiles(const IDArray &idArray);
......
src/MeasurementComplexItem/geometry/GeoArea.cc
View file @ 22c1b55e
......@@ -82,20 +82,7 @@ bool GeoArea::covers(const QGeoCoordinate &c) {
}
}
void GeoArea::init() {
this->setObjectName(nameString);
// connect(this, &GeoArea::pathChanged, [this] {
// if (this->objectName() != "Tile") {
// qDebug() << this->objectName() << " path: " << this->path() << "\n";
// }
// });
// connect(this, &GeoArea::centerChanged, [this] {
// if (this->objectName() != "Tile") {
// qDebug() << this->objectName() << " center: " << this->center() <<
// "\n";
// }
// });
}
void GeoArea::init() { this->setObjectName(nameString); }
void GeoArea::setErrorString(const QString &str) {
this->_errorString = str;
......
src/MeasurementComplexItem/geometry/MeasurementArea.cc
View file @ 22c1b55e
......@@ -33,23 +33,6 @@ namespace {
const char *tileArrayKey = "TileArray";
} // namespace
TileData::TileData() {}
TileData::~TileData() {}
TileData &TileData::operator=(const TileData &other) { return *this; }
bool TileData::operator==(const TileData &other) const { return false; }
bool TileData::operator!=(const TileData &other) const {
return operator==(other);
}
void TileData::saveToJson(QJsonObject &json) {}
bool TileData::loadFromJson(const QJsonObject &json, QString &errorString) {
return false;
}
void TileData::clear() {}
std::size_t TileData::size() const { return 0; }
const char *MeasurementArea::settingsGroup = "MeasurementArea";
const char *tileHeightKey = "TileHeight";
const char *tileWidthName = "TileWidth";
......@@ -107,7 +90,7 @@ MeasurementArea::MeasurementArea(const MeasurementArea &other, QObject *parent)
qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
->clone(_tiles.get()));
}
_tileMap = other._tileMap;
_indexMap = other._indexMap;
enableUpdate();
} else {
enableUpdate();
......@@ -131,7 +114,7 @@ MeasurementArea &MeasurementArea::operator=(const MeasurementArea &other) {
qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
->clone(_tiles.get()));
}
_tileMap = other._tileMap;
_indexMap = other._indexMap;
enableUpdate();
} else {
enableUpdate();
......@@ -270,6 +253,41 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
break;
}
}
if (!tileError) {
this->_indexMap.clear();
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
auto it = _indexMap.find(tile->id());
// find unique id
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
it = _indexMap.find(newId);
if (it == _indexMap.end()) {
break;
} else {
++newId;
}
}
if (counter != counterMax) {
tile->setId(newId);
tile->setProgress(0.0);
} else {
qCritical() << "MeasurementArea::storeTiles(): not able to find "
"unique id!";
continue;
}
}
_indexMap.insert(std::make_pair(tile->id(), i));
}
}
} else {
qCWarning(MeasurementAreaLog)
<< "Not able to load tiles. tileArrayKey missing or wrong type.";
......@@ -307,15 +325,22 @@ bool MeasurementArea::isCorrect() {
void MeasurementArea::updateProgress(const ProgressArray &array) {
if (ready() && !_holdProgress && array.size() > 0) {
bool anyChanges = false;
for (const auto &pair : array) {
const auto &id = pair.first;
const auto &progress = pair.second;
auto it = _tileMap.find(id);
if (it != _tileMap.end()) {
if (!qFuzzyCompare(progress, it->second->progress())) {
it->second->setProgress(progress);
long counter = 0;
for (const auto &lp : array) {
qDebug() << "MeasurementArea::updateProgress: counter = " << counter++;
auto it = _indexMap.find(lp.id());
if (it != _indexMap.end()) {
int tileIndex = it->second;
auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
qDebug() << "MeasurementArea::updateProgress: progress before = "
<< tile->progress();
if (!qFuzzyCompare(lp.progress(), tile->progress())) {
tile->setProgress(lp.progress());
anyChanges = true;
}
qDebug() << "MeasurementArea::updateProgress: progress after = "
<< tile->progress();
}
}
......@@ -330,6 +355,8 @@ void MeasurementArea::randomProgress() {
std::srand(std::time(nullptr));
ProgressArray progressArray;
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = _tiles->value<MeasurementTile *>(i);
......@@ -341,10 +368,10 @@ void MeasurementArea::randomProgress() {
p = 100;
}
tile->setProgress(p);
progressArray.append(LabeledProgress(p, tile->id()));
}
emit progressChanged();
updateProgress(progressArray);
}
}
......@@ -421,7 +448,7 @@ void MeasurementArea::doUpdate() {
geoTile->push_back(geoVertex);
hashValue ^= hashFun(geoVertex);
}
geoTile->setId(long(hashValue));
geoTile->setId(std::int64_t(hashValue));
pData->append(geoTile);
}
}
......@@ -452,7 +479,7 @@ void MeasurementArea::deferUpdate() {
if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
qCDebug(MeasurementAreaLog) << "defereUpdate(): defer update.";
if (this->_state == STATE::IDLE) {
this->_tileMap.clear();
this->_indexMap.clear();
this->_tiles->clearAndDeleteContents();
emit tilesChanged();
emit progressChanged();
......@@ -478,20 +505,20 @@ void MeasurementArea::storeTiles() {
QQmlEngine::CppOwnership);
// update tileMap
this->_tileMap.clear();
this->_indexMap.clear();
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
auto it = _tileMap.find(tile->id());
auto it = _indexMap.find(tile->id());
// find unique id
if (it != _tileMap.end()) {
long newId = tile->id() + 1;
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
it = _tileMap.find(newId);
if (it == _tileMap.end()) {
it = _indexMap.find(newId);
if (it == _indexMap.end()) {
break;
} else {
++newId;
......@@ -508,7 +535,7 @@ void MeasurementArea::storeTiles() {
}
}
_tileMap.insert(std::make_pair(tile->id(), tile));
_indexMap.insert(std::make_pair(tile->id(), i));
}
// This is expensive. Drawing tiles is expensive too.
......@@ -581,18 +608,20 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
void MeasurementArea::updateIds(const QList<TileDiff> &array) {
for (const auto &diff : array) {
auto it = _tileMap.find(diff.oldTile.id());
if (it != _tileMap.end() &&
diff.oldTile.coordinateList() == it->second->coordinateList()) {
// Change id and update _tileMap.
const auto newId = diff.newTile.id();
auto tile = it->second;
tile->setId(newId);
_tileMap.erase(it);
auto ret = _tileMap.insert(std::make_pair(newId, tile));
Q_ASSERT(ret.second == true /*insert success?*/);
Q_UNUSED(ret);
auto it = _indexMap.find(diff.oldTile.id());
if (it != _indexMap.end()) {
int tileIndex = it->second;
auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
if (diff.oldTile.coordinateList() == tile->coordinateList()) {
// Change id and update _tileMap.
const auto newId = diff.newTile.id();
tile->setId(newId);
_indexMap.erase(it);
auto ret = _indexMap.insert(std::make_pair(newId, tileIndex));
Q_ASSERT(ret.second == true /*insert success?*/);
Q_UNUSED(ret);
}
}
}
}
......@@ -673,7 +702,7 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
for (FPolygon &t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
......
src/MeasurementComplexItem/geometry/MeasurementArea.h
View file @ 22c1b55e
......@@ -54,10 +54,10 @@ public:
// Overrides from GeoArea
QString mapVisualQML(void) const override;
QString editorQML(void) const override;
MeasurementArea *clone(QObject *parent = nullptr) const;
MeasurementArea *clone(QObject *parent = nullptr) const override;
bool saveToJson(QJsonObject &json) override;
bool loadFromJson(const QJsonObject &json, QString &errorString) override;
Q_INVOKABLE virtual bool isCorrect();
Q_INVOKABLE virtual bool isCorrect() override;
// Property getters.
Fact *tileHeight();
......@@ -122,7 +122,7 @@ private:
// Tile stuff.
TilePtr _tiles;
std::map<long /*id*/, MeasurementTile *> _tileMap;
std::map<std::int64_t /*id*/, int> _indexMap;
bool _holdProgress;
QTimer _timer;
STATE _state;
......
src/MeasurementComplexItem/geometry/ProgressArray.h
View file @ 22c1b55e
......@@ -4,7 +4,10 @@
#include <QVector>
#include <tuple>
typedef std::pair<long /*id*/, double /*progress*/> TaggedProgress;
typedef QVector<TaggedProgress> ProgressArray;
#include "comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h"
typedef ros_bridge::messages::nemo_msgs::labeled_progress::LabeledProgress
LabeledProgress;
typedef QVector<LabeledProgress> ProgressArray;
#endif // PROGRESSARRAY_H
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h deleted 100644 → 0
View file @ 5beee98a
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
#include <QThread>
#include <QVariant>
#include "Task.h"
namespace nemo_interface {
class CommandDispatcher : public QThread {
public:
CommandDispatcher();
bool interruptionPoint(); // thread safe
std::future<QVariant> dispatch(const Task &c); // thread safe
std::future<QVariant> dispatchNext(const Task &c); // thread safe
void clear(); // thread safe
void stop(); // thread safe
bool running(); // thread safe
private:
std::atomic_bool _running;
std::condition_variable _condVar;
QList<Task> _queue;
std::mutex _queueMutex;
};
} // namespace nemo_interface
#endif // COMMANDDISPATCHER_H
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h 0 → 100644
View file @ 22c1b55e
#ifndef FUTUREWATCHER_H
#define FUTUREWATCHER_H
#include "FutureWatcherInterface.h"
#include <QTimer>
#include <future>
namespace nemo_interface {
//!
//! class similar to QFutureWatcher suitable for a std::future or
//! std::shared_future. \note For single thread use only.
//!
template <class T, template <class> class Future = std::future>
class FutureWatcher : public FutureWatcherInterface {
enum class STATE { EMPTY, STARTED, FINISHED };
public:
typedef Future<T> FutureType;
explicit FutureWatcher(QObject *parent = nullptr)
: FutureWatcherInterface(parent), _state(STATE::EMPTY) {
_init();
}
FutureWatcher(const FutureType &f, QObject *parent = nullptr)
: FutureWatcherInterface(parent), _future(f), _state(STATE::STARTED) {
_init();
}
FutureType future() const { return _future; }
void setFuture(const FutureType &future) {
_future = future;
_state = STATE::STARTED;
_timer.start(1);
emit started();
}
T result() { return _future.get(); }
virtual void waitForFinished() override {
if (_state == STATE::STARTED) {
_timer.stop();
_future.wait();
_state = STATE::FINISHED;
}
}
virtual bool isStarted() override { return _state == STATE::STARTED; }
virtual bool isFinished() override { return _state == STATE::FINISHED; }
private:
void _onTimeout() {
if (_state == STATE::STARTED) {
auto status = _future.wait_for(std::chrono::seconds(0));
if (status == std::future_status::ready) {
_state = STATE::FINISHED;
emit finished();
}
}
}
void _init() {
connect(&_timer, &QTimer::timeout, [this] { this->_onTimeout(); });
}
FutureType _future;
QTimer _timer;
STATE _state;
};
} // namespace nemo_interface
#endif // FUTUREWATCHER_H
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp → src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp
View file @ 22c1b55e
#include "CommandDispatcher.h"
#include "FutureWatcherInterface.h"
namespace nemo_interface {
CommandDispatcher::CommandDispatcher() {}
FutureWatcherInterface::FutureWatcherInterface(QObject *parent)
: QObject(parent) {}
} // namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h 0 → 100644
View file @ 22c1b55e
#ifndef FUTUREWATCHERINTERFACE_H
#define FUTUREWATCHERINTERFACE_H
#include <QObject>
namespace nemo_interface {
class FutureWatcherInterface : public QObject {
Q_OBJECT
public:
FutureWatcherInterface(QObject *parent = nullptr);
virtual void waitForFinished() = 0;
virtual bool isStarted() = 0;
virtual bool isFinished() = 0;
signals:
void finished();
void started();
};
} // namespace nemo_interface
#endif // FUTUREWATCHERINTERFACE_H
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
View file @ 22c1b55e
#include "MeasurementTile.h"
#include <QDebug>
const char *MeasurementTile::settingsGroup = "MeasurementTile";
const char *MeasurementTile::nameString = "MeasurementTile";
MeasurementTile::MeasurementTile(QObject *parent)
: GeoArea(parent), _progress(0), _id(0) {
init();
......@@ -31,7 +36,7 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
this->appendVertex(c);
}
void MeasurementTile::init() { this->setObjectName("Tile"); }
void MeasurementTile::init() { this->setObjectName(nameString); }
int64_t MeasurementTile::id() const { return _id; }
......
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
View file @ 22c1b55e
......@@ -17,7 +17,7 @@ public:
virtual QString mapVisualQML() const override;
virtual QString editorQML() const override;
virtual MeasurementTile *clone(QObject *parent) const;
virtual MeasurementTile *clone(QObject *parent) const override;
void push_back(const QGeoCoordinate &c);
......@@ -29,6 +29,10 @@ public:
QList<QGeoCoordinate> tile() const;
// Static Variables
static const char *settingsGroup;
static const char *nameString;
signals:
void progressChanged();
void idChanged();
......
src/MeasurementComplexItem/nemo_interface/Task.cpp
View file @ 22c1b55e
......@@ -2,10 +2,14 @@
namespace nemo_interface {
Task::Task(const Task::Functor &onExec)
: _onExec(onExec), _isExpired([] { return false; }),
_isReady([] { return true; }) {}
Task::Task(const Task::Functor &onExec) : _onExec(onExec) {}
void Task::exec(std::promise<QVariant> p) { this->_onExec(std::move(p)); }
Task::Task(Task::Functor &&onExec) : _onExec(std::move(onExec)) {}
QVariant Task::exec() { return this->_onExec(); }
void Task::setOnExec(const Task::Functor &onExec) { _onExec = onExec; }
void Task::setOnExec(Task::Functor &&onExec) { _onExec = std::move(onExec); }
} // namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/Task.h
View file @ 22c1b55e
......@@ -10,23 +10,17 @@ namespace nemo_interface {
class Task {
public:
typedef std::function<void(std::promise<QVariant> p)> Functor;
typedef std::function<bool()> BooleanFunctor;
typedef std::function<QVariant(void)> Functor;
Task(const Functor &onExec);
Task(Functor &&onExec);
void exec(std::promise<QVariant> p);
bool isReady(); // default == true
bool isExpired(); // default == false
QVariant exec();
void setOnExec(const Functor &onExec);
void setIsReady(const BooleanFunctor &isReady);
void setIsExpired(const BooleanFunctor &isExpired);
void setOnExec(Functor &&onExec);
private:
Functor _onExec;
BooleanFunctor _isExpired;
BooleanFunctor _isReady;
};
} // namespace nemo_interface
......
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp 0 → 100644
View file @ 22c1b55e
#include "TaskDispatcher.h"
#include "QtConcurrent"
typedef std::unique_lock<std::mutex> ULock;
namespace nemo_interface {
TaskDispatcher::TaskDispatcher() : _running(false) {}
TaskDispatcher::~TaskDispatcher() { stop(); }
std::future<QVariant> TaskDispatcher::dispatch(std::unique_ptr<Task> c) {
ULock lk1(this->_mutex);
this->_taskQueue.push_back(std::move(c));
std::promise<QVariant> promise;
auto future = promise.get_future();
this->_promiseQueue.push_back(std::move(promise));
if (!this->_running) {
lk1.unlock();
this->start();
}
return future;
}
std::future<QVariant> TaskDispatcher::dispatchNext(std::unique_ptr<Task> c) {
ULock lk1(this->_mutex);
this->_taskQueue.push_front(std::move(c));
std::promise<QVariant> promise;
auto future = promise.get_future();
this->_promiseQueue.push_front(std::move(promise));
if (!this->_running) {
lk1.unlock();
this->start();
}
return future;
}
void TaskDispatcher::clear() {
ULock lk(this->_mutex);
this->_taskQueue.clear();
this->_promiseQueue.clear();
}
void TaskDispatcher::start() {
ULock lk1(this->_mutex);
if (!_running) {
this->_running = true;
lk1.unlock();
_future = QtConcurrent::run([this]() mutable { return this->run(); });
}
}
void TaskDispatcher::stop() {
ULock lk1(this->_mutex);
if (_running) {
this->_running = false;
lk1.unlock();
this->_future.waitForFinished();
}
}
void TaskDispatcher::reset() {
clear();
stop();
}
bool TaskDispatcher::isInterruptionRequested() {
ULock lk1(this->_mutex);
return !_running;
}
bool TaskDispatcher::isRunning() {
ULock lk1(this->_mutex);
return _running;
}
std::size_t TaskDispatcher::pendingTasks() {
ULock lk1(this->_mutex);
return this->_taskQueue.size() + (_running ? 1 : 0);
}
void TaskDispatcher::run() {
while (true) {
ULock lk1(this->_mutex);
if (this->_taskQueue.size() > 0 && this->_running) {
// pop task and promise
auto pTask = std::move(this->_taskQueue.front());
auto promise = std::move(this->_promiseQueue.front());
this->_taskQueue.pop_front();
this->_promiseQueue.pop_front();
lk1.unlock();
// exec task
promise.set_value(pTask->exec());
} else {
this->_running = false;
lk1.unlock();
break;
}
}
}
} // namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h 0 → 100644
View file @ 22c1b55e
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
#include <QFuture>
#include <QVariant>
#include <deque>
#include "Task.h"
namespace nemo_interface {
class TaskDispatcher : QObject {
public:
TaskDispatcher();
~TaskDispatcher();
//!
//! \brief dispatch Dispatches a task.
//! \param c The task to dispatch.
//! \return Returns a future containing the QVariant which the task c
//! returned.
//!
std::future<QVariant> dispatch(std::unique_ptr<Task> c);
//!
//! \brief dispatchNext Dispatches the task c as the next task.
//! \param c The task to dispatch.
//! \return Returns a future containing the QVariant which the task c
//! returned.
//!
std::future<QVariant> dispatchNext(std::unique_ptr<Task> c);
//!
//! \brief clear Clears the task list.
//!
void clear();
//!
//! \brief start Starts task dispatching. This function can be called after
//! dispatching has been stopped, e.g. by stop() or reset().
//!
void start();
//!
//! \brief stop Stops task dispatching.
//!
//! Stops taks dispatching. The dispatcher will wait for the current task to
//! complete and than terminate the dispatcher thread. After stop() has been
//! called isInterruptionRequested will return true. This can be used to
//! permaturely terminate a task.
//!
void stop();
//!
//! \brief reset Clears the task list and stops dispatching.
//!
void reset();
bool isInterruptionRequested();
bool isRunning();
std::size_t pendingTasks();
protected:
void run();
private:
QFuture<void> _future;
std::deque<std::unique_ptr<Task>> _taskQueue;
std::deque<std::promise<QVariant>> _promiseQueue;
bool _running;
std::mutex _mutex;
};
} // namespace nemo_interface
#endif // COMMANDDISPATCHER_H
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @ 22c1b55e
......@@ -74,7 +74,8 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name,
#ifdef ROS_BRIDGE_DEBUG
std::thread client_thread([client_name, client]() {
#else
std::thread client_thread([client]() {
std::thread client_thread([client_name, client]() {
// std::thread client_thread([client]() {
#endif
client->start();
......
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @ 22c1b55e
......@@ -41,7 +41,6 @@ bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value,
template <class HeartbeatType>
bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) {
if (!value.HasMember("status") || !value["status"].IsInt()) {
assert(false);
return false;
}
......
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h
View file @ 22c1b55e
......@@ -24,7 +24,7 @@ const char *idKey = "id";
//! @brief C++ representation of nemo_msgs/labeled_progress message
class LabeledProgress {
public:
LabeledProgress() {}
LabeledProgress() : _id(0), _progress(0) {}
LabeledProgress(double progress, long id) : _id(id), _progress(progress) {}
long id() const { return _id; }
......@@ -52,11 +52,11 @@ bool fromJson(const rapidjson::Value &value, ProgressType &p) {
return false;
}
if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
if (!value.HasMember(idKey) || !value[idKey].IsInt64()) {
return false;
}
p.setId(value[idKey].GetInt());
p.setId(value[idKey].GetInt64());
p.setProgress(value[progressKey].GetDouble());
return true;
......
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp 0 → 100644
View file @ 22c1b55e
#include "progress_array.h"
std::string ros_bridge::messages::nemo_msgs::progress_array::messageType() {
return "/nemo_msgs/ProgressArray";
}
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h 0 → 100644
View file @ 22c1b55e
#ifndef PROGRESS_ARRAY_H
#define PROGRESS_ARRAY_H
#pragma once
#include "labeled_progress.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QVector>
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs
//! generation.
namespace nemo_msgs {
//! @brief Namespace containing methodes for nemo_msgs/progress_array message
//! generation.
namespace progress_array {
std::string messageType();
namespace {
const char *progressArrayKey = "progress_array";
} // namespace
//! @brief C++ representation of nemo_msgs/progress_array message
template <template <class, class...> class Container = QVector>
class GenericProgressArray {
public:
typedef Container<labeled_progress::LabeledProgress> ContainerType;
GenericProgressArray() {}
ContainerType &progress_array() { return _progres_array; }
const ContainerType &progress_array() const { return _progres_array; }
void setProgressArray(const ContainerType &c) { _progres_array = c; }
protected:
Container<labeled_progress::LabeledProgress> _progres_array;
};
typedef GenericProgressArray<> ProgressArray;
template <class Array>
bool toJson(const Array &array, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
rapidjson::Value jsonArray(rapidjson::kArrayType);
for (const auto &lp : array.progress_array()) {
rapidjson::Value jsonLp;
if (labeled_progress::toJson(lp, jsonLp, allocator)) {
jsonArray.PushBack(jsonLp, allocator);
} else {
return false;
}
}
value.AddMember(rapidjson::Value(progressArrayKey), jsonArray, allocator);
return true;
}
template <class Array> bool fromJson(const rapidjson::Value &value, Array &p) {
if (!value.HasMember(progressArrayKey) ||
!value[progressArrayKey].IsArray()) {
return false;
}
const auto &jsonArray = value[progressArrayKey].GetArray();
p.progress_array().clear();
p.progress_array().reserve(jsonArray.Size());
for (long i = 0; i < jsonArray.Size(); ++i) {
labeled_progress::LabeledProgress lp;
if (!labeled_progress::fromJson(jsonArray[i], lp)) {
return false;
} else {
p.progress_array().push_back(lp);
}
}
return true;
}
} // namespace progress_array
} // namespace nemo_msgs
} // namespace messages
} // namespace ros_bridge
#endif // PROGRESS_ARRAY_H
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
View file @ 22c1b55e
......@@ -44,9 +44,9 @@ public:
void setTile(const GeoContainer &tile) { _tile = tile; }
protected:
GeoContainer _tile;
long _id;
double _progress;
GeoContainer _tile;
};
typedef GenericTile<> Tile;
......
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