diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index d38da819d9b2291cc728b640b0c7834df13934f3..606d00bf20e1bf81a0a13e307f1cf66e5bef2a25 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -453,8 +453,9 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \
+ src/MeasurementComplexItem/nemo_interface/Task.h \
+ src/MeasurementComplexItem/nemo_interface/tileHelper.h \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
- src/MeasurementComplexItem/nemo_interface/Command.h \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
src/QmlControls/QmlUnitsConversion.h \
@@ -496,11 +497,6 @@ HEADERS += \
src/MeasurementComplexItem/geometry/GeoPoint3D.h \
src/MeasurementComplexItem/NemoInterface.h \
src/MeasurementComplexItem/nemo_interface/QNemoHeartbeat.h \
- src/MeasurementComplexItem/nemo_interface/QNemoProgress.h \
- src/MeasurementComplexItem/nemo_interface/QNemoProgress.h \
- src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h \
- src/MeasurementComplexItem/nemo_interface/SnakeTiles.h \
- src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h \
src/MeasurementComplexItem/call_once.h \
src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \
@@ -513,10 +509,6 @@ HEADERS += \
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/geometry_msgs/point32.h \
- src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h \
- src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \
- src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
src/comm/ros_bridge/include/messages/std_msgs/header.h \
@@ -537,9 +529,9 @@ SOURCES += \
src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/MeasurementComplexItem/HashFunctions.cpp \
- src/MeasurementComplexItem/nemo_interface/Command.cpp \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
+ src/MeasurementComplexItem/nemo_interface/Task.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
@@ -555,15 +547,10 @@ SOURCES += \
src/MeasurementComplexItem/geometry/clipper/clipper.cpp \
src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
src/MeasurementComplexItem/NemoInterface.cpp \
- src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \
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/geometry_msgs/point32.cpp \
- src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp \
- src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \
- src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
diff --git a/src/MeasurementComplexItem/HashFunctions.h b/src/MeasurementComplexItem/HashFunctions.h
index ce8830660333629293d3f405acb94e4d2e465c1c..058f1086a57e00d5584e57aeeb15ab597d3d4935 100644
--- a/src/MeasurementComplexItem/HashFunctions.h
+++ b/src/MeasurementComplexItem/HashFunctions.h
@@ -11,7 +11,7 @@ namespace std {
template <> struct hash<QGeoCoordinate> {
std::size_t operator()(const QGeoCoordinate &c) {
hash<double> h;
- return h(c.latitude()) ^ h(c.longitude()) ^ h(c.altitude());
+ return h(c.latitude()) ^ (h(c.longitude()) << 1) ^ (h(c.altitude()) << 2);
}
};
diff --git a/src/MeasurementComplexItem/IDArray.h b/src/MeasurementComplexItem/IDArray.h
index 6052cec19198cd28f626ca53443516c0e3d27bb8..6ad3d32ea79c1b41285ac9a4198cfbe6b0f00037 100644
--- a/src/MeasurementComplexItem/IDArray.h
+++ b/src/MeasurementComplexItem/IDArray.h
@@ -1,5 +1,8 @@
#ifndef IDARRAY_H
#define IDARRAY_H
-typedef QVector<long> IDArray;
+
+#include <QVector>
+
+typedef QVector<std::int64_t> IDArray;
#endif // IDARRAY_H
diff --git a/src/MeasurementComplexItem/LogicalArray.h b/src/MeasurementComplexItem/LogicalArray.h
index 359eb67a3891776dc6bc4cf33be4dc0bd4dcf818..011917d7a5b79c5e273d9697b2dc02a32eefd691 100644
--- a/src/MeasurementComplexItem/LogicalArray.h
+++ b/src/MeasurementComplexItem/LogicalArray.h
@@ -1,5 +1,8 @@
#ifndef LOGICALARRAY_H
#define LOGICALARRAY_H
+
+#include <QVector>
+
typedef QVector<bool> LogicalArray;
#endif // LOGICALARRAY_H
diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp
index 5b3b5ba625fa6f7ef1d022ac7dfdb3d12f80319d..9e887d5d79d340ba78e6a18474198a367a0e1b95 100644
--- a/src/MeasurementComplexItem/NemoInterface.cpp
+++ b/src/MeasurementComplexItem/NemoInterface.cpp
@@ -1,5 +1,4 @@
#include "NemoInterface.h"
-#include "nemo_interface/SnakeTilesLocal.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
@@ -15,14 +14,14 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
+#include "nemo_interface/CommandDispatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
-#include "nemo_interface/QNemoProgress.h"
+#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
-#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
-#include "ros_bridge/include/messages/nemo_msgs/progress.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"
@@ -30,38 +29,35 @@
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
-#define EVENT_TIMER_INTERVAL 100 // ms
-auto constexpr static timeoutInterval = std::chrono::milliseconds(3000);
+#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;
-using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
-using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
+using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;
using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
using SharedLock = std::shared_lock<std::shared_timed_mutex>;
-using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class NemoInterface::Impl {
-public:
enum class STATE {
STOPPED,
- RUNNING,
+ START_BRIDGE,
WEBSOCKET_DETECTED,
- HEARTBEAT_DETECTED,
TRY_TOPIC_SERVICE_SETUP,
READY,
- SYNCHRONIZING,
- TIMEOUT,
- INVALID_HEARTBEAT
- }
+ TIMEOUT
+ };
+public:
Impl(NemoInterface *p);
+ ~Impl();
void start();
void stop();
// Tile editing.
// Functions that require communication to device.
- void addTiles(const TilePtrArray &tileArray);
- void addTiles(const TileArray &tileArray);
+ std::future<QVariant> addTiles(const TileArray &tileArray);
void removeTiles(const IDArray &idArray);
void clearTiles();
@@ -77,42 +73,53 @@ public:
ProgressArray getProgress(const IDArray &idArray);
NemoInterface::STATUS status();
- bool running();
+ bool running(); // thread safe
+ bool ready(); // thread safe
+
+ const QString &infoString();
+ const QString &warningString();
-private slots:
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 loop();
+ void _doTopicServiceSetup();
+ void _doAction(); // does action according to state
- bool _setState(STATE s);
+ bool _setState(STATE s); // not thread safe
+ static bool _ready(STATE s);
static bool _running(STATE s);
static void _translate(STATE state, NemoInterface::STATUS &status);
static void _translate(Heartbeat hb, STATE &state);
- TimePoint nextTimeout;
- mutable std::shared_timed_mutex timeoutMutex;
-
- STATE _state;
+ std::atomic<STATE> _state;
ROSBridgePtr _pRosBridge;
TileMap _tileMap;
NemoInterface *_parent;
+ Dispatcher _dispatcher;
+ QString _infoString;
+ QString _warningString;
+ QTimer _timeoutTimer;
+ QTimer _connectionTimer;
+ QNemoHeartbeat _lastHeartbeat;
};
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::HEARTBEAT_DETECTED, "Heartbeat Detected"),
+ 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>(
@@ -121,8 +128,7 @@ static StatusMap statusMap{
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
NemoInterface::Impl::Impl(NemoInterface *p)
- : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
- running_(false), topicServiceSetupDone(false), _parent(p) {
+ : _state(STATE::STOPPED), _parent(p) {
// ROS Bridge.
WimaSettings *wimaSettings =
@@ -130,37 +136,151 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this] {
auto connectionString = connectionStringFact->rawValue().toString();
- if (ros_bridge::isValidConnectionString(
- connectionString.toLocal8Bit().data())) {
+ if (is_valid_port_path(connectionString.toLocal8Bit().data())) {
} else {
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
- this->_pRosBridge.reset(
- new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
+
+ if (this->_pRosBridge) {
+ this->_pRosBridge->reset();
+ }
+ this->_pRosBridge = std::make_shared<RosbridgeWsClient>(
+ connectionString.toLocal8Bit().data());
+ this->_pRosBridge->reset();
+ qCritical() << "NemoInterface: add reset code here";
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
- // Periodic.
- connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
- this->loopTimer.start(EVENT_TIMER_INTERVAL);
+ // Heartbeat timeout.
+ connect(&this->_timeoutTimer, &QTimer::timeout,
+ [this] { this->_setState(STATE::TIMEOUT); });
+
+ // 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->_setState(STATE::WEBSOCKET_DETECTED);
+ this->_doAction();
+ }
+ } else {
+ if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP ||
+ this->_state == STATE::READY) {
+ this->_setState(STATE::TIMEOUT);
+ this->_doAction();
+ }
+ }
+ });
}
+NemoInterface::Impl::~Impl() {}
+
void NemoInterface::Impl::start() {
- this->running_ = true;
- emit this->_parent->runningChanged();
+ if (!running()) {
+ this->_setState(STATE::START_BRIDGE);
+ this->_doAction();
+ }
}
void NemoInterface::Impl::stop() {
- this->running_ = false;
- emit this->_parent->runningChanged();
+ if (running()) {
+ this->_setState(STATE::STOPPED);
+ this->_connectionTimer.stop();
+ this->_doAction();
+ }
}
-void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {}
+std::future<QVariant>
+NemoInterface::Impl::addTiles(const TileArray &tileArray) {
+ using namespace nemo_interface;
+
+ if (this->ready()) {
+
+ // 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());
+
+ if (abort) {
+ qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
+ promise.set_value(QVariant(false));
+ return;
+ }
+
+ qCritical() << "addTiles(): ToDo: add return value checking here.";
+
+ // update local tiles
+ QMetaObject::invokeMethod(
+ this->_parent, std::bind(&Impl::_addTilesLocal, this, tileArray));
+
+ // return success
+ promise.set_value(QVariant(true));
+ return;
+ }); // sendTilesCommand
+
+ // dispatch command and return.
+ auto ret = _dispatcher.dispatch(sendTilesCommand);
+ return ret;
+ } else {
+ std::promise<QVariant> p;
+ p.set_value(QVariant(false));
+ return p.get_future();
+ }
+}
TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray tileArray;
@@ -215,7 +335,7 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
for (const auto &id : idArray) {
const auto it = _tileMap.find(id);
- if (id != _tileMap.end()) {
+ if (it != _tileMap.end()) {
progressArray.append(TaggedProgress{it->first, it->second.progress()});
}
}
@@ -223,78 +343,19 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
return progressArray;
}
-void NemoInterface::Impl::setTileData(const TileData &tileData) {
- this->tileData = tileData;
- if (tileData.tiles.count() > 0) {
- std::lock(this->ENUOriginMutex, this->tilesENUMutex);
- UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
- UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
-
- const auto *obj = tileData.tiles[0];
- const auto *tile = qobject_cast<const MeasurementTile *>(obj);
- if (tile != nullptr) {
- if (tile->coordinateList().size() > 0) {
- if (tile->coordinateList().first().isValid()) {
- this->ENUOrigin = tile->coordinateList().first();
- const auto &origin = this->ENUOrigin;
- this->tilesENU.polygons().clear();
- for (int i = 0; i < tileData.tiles.count(); ++i) {
- obj = tileData.tiles[i];
- tile = qobject_cast<const MeasurementTile *>(obj);
- if (tile != nullptr) {
- SnakeTileLocal tileENU;
- geometry::areaToEnu(origin, tile->coordinateList(),
- tileENU.path());
- this->tilesENU.polygons().push_back(std::move(tileENU));
- } else {
- qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
- break;
- }
- }
- } else {
- qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
- }
- } else {
- qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
- }
- }
- } else {
- this->tileData.clear();
-
- std::lock(this->ENUOriginMutex, this->tilesENUMutex);
- UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
- UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
- this->ENUOrigin = QGeoCoordinate(0, 0, 0);
- this->tilesENU = SnakeTilesLocal();
- }
-}
-
-bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
- return this->tileData == tileData;
+NemoInterface::STATUS NemoInterface::Impl::status() {
+ NemoInterface::STATUS status;
+ _translate(this->_state, status);
+ return status;
}
-void NemoInterface::Impl::publishTileData() {
- std::lock(this->ENUOriginMutex, this->tilesENUMutex);
- UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
- UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
+bool NemoInterface::Impl::running() { return _running(this->_state); }
- if (this->tilesENU.polygons().size() > 0 && this->running_ &&
- this->topicServiceSetupDone) {
- this->publishENUOrigin();
- this->publishTilesENU();
- }
-}
+bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); }
-NemoInterface::STATUS NemoInterface::Impl::status() {
- return _translate(this->_state);
-}
+const QString &NemoInterface::Impl::infoString() { return _infoString; }
-QVector<int> NemoInterface::Impl::progress() {
- SharedLock lk(this->progressMutex);
- return this->qProgress.progress();
-}
-
-bool NemoInterface::Impl::running() { return _running(this->_state); }
+const QString &NemoInterface::Impl::warningString() { return _warningString; }
void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
bool anyChanges = false;
@@ -363,133 +424,88 @@ void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
emit _parent->progressChanged(progressArray);
}
-void NemoInterface::Impl::doTopicServiceSetup() {
+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::_setInfoString(const QString &info) {
+ if (_infoString != info) {
+ _infoString = info;
+ emit this->_parent->infoStringChanged();
+ }
+}
+
+void NemoInterface::Impl::_setWarningString(const QString &warning) {
+ if (_warningString != warning) {
+ _warningString = warning;
+ emit this->_parent->warningStringChanged();
+ }
+}
+
+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(
- "/nemo/progress",
- /* callback */ [this](JsonDocUPtr pDoc) {
- std::lock(this->progressMutex, this->tilesENUMutex,
- this->ENUOriginMutex);
- UniqueLock lk1(this->progressMutex, std::adopt_lock);
- UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
- UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
-
- int requiredSize = this->tilesENU.polygons().size();
- auto &progressMsg = this->qProgress;
- if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
- progressMsg.progress().size() !=
- requiredSize) { // Some error occured.
- progressMsg.progress().clear();
- qgcApp()->informationMessageBoxOnMainThread(
- "Nemo Interface", "Invalid progress message received.");
- }
- emit this->_parent->progressChanged();
-
- lk1.unlock();
- lk2.unlock();
- lk3.unlock();
+ progressClient, "/nemo/progress",
+ [this](std::shared_ptr<WsClient::Connection> connection,
+ std::shared_ptr<WsClient::InMessage> in_message) {
+ qDebug() << "doTopicServiceSetup(): /nemo/progress: "
+ << in_message->string().c_str();
+ qDebug() << "impl missing";
});
-
- // Subscribe /nemo/heartbeat.
+ // Subscribe heartbeat msg.
+ const char *heartbeatClient = "client:/nemo/heartbeat";
+ this->_pRosBridge->addClient(heartbeatClient);
this->_pRosBridge->subscribe(
- "/nemo/heartbeat",
- /* callback */ [this](JsonDocUPtr pDoc) {
- nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
- if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
- this->_setState(STATUS::INVALID_HEARTBEAT);
- } else {
- this->_setState(heartbeatToStatus(heartbeatMsg));
- }
- if (this->status_ == STATUS::INVALID_HEARTBEAT) {
- UniqueLock lk(this->timeoutMutex);
- this->nextTimeout = TimePoint::max();
- } else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
- UniqueLock lk(this->timeoutMutex);
- this->nextTimeout =
- std::chrono::high_resolution_clock::now() + timeoutInterval;
- }
+ heartbeatClient, "/nemo/heartbeat",
+ [this](std::shared_ptr<WsClient::Connection> connection,
+ std::shared_ptr<WsClient::InMessage> in_message) {
+ qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: "
+ << in_message->string().c_str();
+ qDebug() << "impl missing";
});
}
-void NemoInterface::Impl::loop() {
+void NemoInterface::Impl::_doAction() {
// Check ROS Bridge status and do setup if necessary.
- if (this->running_) {
- if (!this->_pRosBridge->isRunning()) {
- this->_pRosBridge->start();
- this->loop();
- } else if (this->_pRosBridge->isRunning() &&
- this->_pRosBridge->connected() && !this->topicServiceSetupDone) {
- this->doTopicServiceSetup();
- this->topicServiceSetupDone = true;
-
- this->_setState(STATUS::WEBSOCKET_DETECTED);
- } else if (this->_pRosBridge->isRunning() &&
- !this->_pRosBridge->connected() && this->topicServiceSetupDone) {
+ switch (this->_state) {
+ case STATE::STOPPED:
+ if (this->_pRosBridge->running()) {
this->_pRosBridge->reset();
- this->_pRosBridge->start();
- this->topicServiceSetupDone = false;
-
- this->_setState(STATUS::TIMEOUT);
}
- } else if (this->_pRosBridge->isRunning()) {
- this->_pRosBridge->reset();
- this->topicServiceSetupDone = false;
- }
-
- // Check if heartbeat timeout occured.
- if (this->running_ && this->topicServiceSetupDone) {
- UniqueLock lk(this->timeoutMutex);
- if (this->nextTimeout != TimePoint::max() &&
- this->nextTimeout < std::chrono::high_resolution_clock::now()) {
- lk.unlock();
- if (this->_pRosBridge->isRunning() && this->_pRosBridge->connected()) {
- this->_setState(STATUS::WEBSOCKET_DETECTED);
- } else {
- this->_setState(STATUS::TIMEOUT);
- }
- }
- }
-}
+ break;
-NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus(
- const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) {
- if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED)
- return STATUS::HEARTBEAT_DETECTED;
- else
- return STATUS::INVALID_HEARTBEAT;
-}
-
-void NemoInterface::Impl::publishTilesENU() {
- using namespace ros_bridge::messages;
- JsonDocUPtr jSnakeTiles(
- std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
- if (jsk_recognition_msgs::polygon_array::toJson(
- this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
- this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
- } else {
- qCWarning(NemoInterfaceLog)
- << "Impl::publishTilesENU: could not create json document.";
- }
-}
-
-void NemoInterface::Impl::publishENUOrigin() {
- using namespace ros_bridge::messages;
- JsonDocUPtr jOrigin(
- std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
- if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
- jOrigin->GetAllocator())) {
- this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
- } else {
- qCWarning(NemoInterfaceLog)
- << "Impl::publishENUOrigin: could not create json document.";
- }
+ case STATE::START_BRIDGE:
+ case STATE::TIMEOUT:
+ this->_pRosBridge->reset();
+ this->_pRosBridge->run();
+ this->_connectionTimer.start(EVENT_TIMER_INTERVAL);
+ break;
+ case STATE::WEBSOCKET_DETECTED:
+ this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
+ this->_doAction();
+ break;
+ case STATE::TRY_TOPIC_SERVICE_SETUP:
+ this->_doTopicServiceSetup();
+ this->_setState(STATE::READY);
+ break;
+ case STATE::READY:
+ break;
+ };
}
bool NemoInterface::Impl::_setState(STATE s) {
- if (s != this->status_) {
- this->status_ = s;
+ if (s != this->_state) {
+ this->_state = s;
emit this->_parent->statusChanged();
return true;
} else {
@@ -497,6 +513,14 @@ bool NemoInterface::Impl::_setState(STATE s) {
}
}
+bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) {
+ return s == STATE::READY;
+}
+
+bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) {
+ return s != STATE::STOPPED;
+}
+
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface()
@@ -515,16 +539,8 @@ void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
-void NemoInterface::addTiles(const NemoInterface::TilePtrArray &tileArray) {
- this->pImpl -
-}
-
void NemoInterface::addTiles(const TileArray &tileArray) {
- TilePtrArray ptrArray;
- for (const auto &tile : tileArray) {
- ptrArray.append(&tile);
- }
- addTiles(ptrArray);
+ this->pImpl->addTiles(tileArray);
}
void NemoInterface::removeTiles(const IDArray &idArray) {
@@ -537,9 +553,7 @@ TileArray NemoInterface::getTiles(const IDArray &idArray) {
return this->pImpl->getTiles(idArray);
}
-TileArray NemoInterface::getAllTiles() {
- return this->pImpl->getTiles(idArray);
-}
+TileArray NemoInterface::getAllTiles() { return this->pImpl->getAllTiles(); }
LogicalArray NemoInterface::containsTiles(const IDArray &idArray) {
return this->pImpl->containsTiles(idArray);
@@ -557,20 +571,6 @@ ProgressArray NemoInterface::getProgress(const IDArray &idArray) {
return this->pImpl->getProgress(idArray);
}
-void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
-
-void NemoInterface::requestProgress() {
- qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
-}
-
-void NemoInterface::setTileData(const TileData &tileData) {
- this->pImpl->setTileData(tileData);
-}
-
-bool NemoInterface::hasTileData(const TileData &tileData) const {
- return this->pImpl->hasTileData(tileData);
-}
-
NemoInterface::STATUS NemoInterface::status() const {
return this->pImpl->status();
}
@@ -579,7 +579,11 @@ QString NemoInterface::statusString() const {
return statusMap.at(this->pImpl->status());
}
-QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }
+QString NemoInterface::infoString() const { return this->pImpl->infoString(); }
+
+QString NemoInterface::warningString() const {
+ return this->pImpl->warningString();
+}
QString NemoInterface::editorQml() {
return QStringLiteral("NemoInterface.qml");
diff --git a/src/MeasurementComplexItem/NemoInterface.h b/src/MeasurementComplexItem/NemoInterface.h
index 512d528b659ff2787fe1b848fd5d9511119c3419..bfd84c07cfbc7a5dd923c0c4b8149d1040d36d99 100644
--- a/src/MeasurementComplexItem/NemoInterface.h
+++ b/src/MeasurementComplexItem/NemoInterface.h
@@ -37,6 +37,9 @@ public:
Q_PROPERTY(STATUS status READ status NOTIFY statusChanged)
Q_PROPERTY(QString statusString READ statusString NOTIFY statusChanged)
+ Q_PROPERTY(QString infoString READ infoString NOTIFY infoStringChanged)
+ Q_PROPERTY(
+ QString warningString READ warningString NOTIFY warningStringChanged)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(bool running READ running NOTIFY runningChanged)
@@ -46,7 +49,6 @@ public:
Q_INVOKABLE void stop();
// Tile editing.
- void addTiles(const TilePtrArray &tileArray);
void addTiles(const TileArray &tileArray);
void removeTiles(const IDArray &idArray);
void clearTiles();
@@ -63,10 +65,14 @@ public:
// Status.
STATUS status() const;
QString statusString() const;
+ QString infoString() const;
+ QString warningString() const;
bool running();
signals:
void statusChanged();
+ void infoStringChanged();
+ void warningStringChanged();
void progressChanged(const ProgressArray &progressArray);
void tilesChanged();
void runningChanged();
diff --git a/src/MeasurementComplexItem/TileArray.h b/src/MeasurementComplexItem/TileArray.h
index c611f02756fcab1d2c35f049350271330824ad02..c92e6c12175b416f47c590903af4500c47e87134 100644
--- a/src/MeasurementComplexItem/TileArray.h
+++ b/src/MeasurementComplexItem/TileArray.h
@@ -1,5 +1,7 @@
#ifndef TILEARRAY_H
#define TILEARRAY_H
+#include "MeasurementTile.h"
+
typedef QVector<MeasurementTile> TileArray;
#endif // TILEARRAY_H
diff --git a/src/MeasurementComplexItem/TilePtrArray.h b/src/MeasurementComplexItem/TilePtrArray.h
index e40d27d85d4950dfbee469b0f143f100aad2b2ab..740547fac7ec68805dd9f7789d3792aab988125c 100644
--- a/src/MeasurementComplexItem/TilePtrArray.h
+++ b/src/MeasurementComplexItem/TilePtrArray.h
@@ -1,5 +1,8 @@
#ifndef TILEPTRARRAY_H
#define TILEPTRARRAY_H
+
+#include "MeasurementTile.h"
+
typedef QVector<MeasurementTile *> TilePtrArray;
#endif // TILEPTRARRAY_H
diff --git a/src/MeasurementComplexItem/nemo_interface/Command.cpp b/src/MeasurementComplexItem/nemo_interface/Command.cpp
deleted file mode 100644
index c935845d1926de9d466394be4d917bc6c8f23be0..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/Command.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "Command.h"
-
-namespace nemo_interface {
-
-Command::Command(Functor onExec) : _onExec(onExec) {}
-
-QFuture<Command::ERROR> Command::exec() { return _onExec(); }
-
-} // namespace nemo_interface
diff --git a/src/MeasurementComplexItem/nemo_interface/Command.h b/src/MeasurementComplexItem/nemo_interface/Command.h
deleted file mode 100644
index b0133990a098f183b2945ce8f1c0abf201a9f116..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/Command.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef COMMAND_H
-#define COMMAND_H
-
-#include <QFuture>
-#include <functional>
-
-namespace nemo_interface {
-
-class Command {
-public:
- enum class ERROR {
- NO_ERROR,
- NETWORK_TIMEOUT,
- PARAMETER_ERROR,
- UNEXPECTED_SERVER_RESPONSE
- };
-
- typedef QFuture<ERROR> ReturnType;
- typedef std::function<ReturnType()> Functor;
-
- Command(Functor onExec);
-
- QFuture<ERROR> exec();
-
-private:
- Functor _onExec;
-};
-} // namespace nemo_interface
-
-#endif // COMMAND_H
diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp
index 2314523d886eff07a6ca3b0c79692f2f8194f5ca..96a91a114f6b4d9d4792c4fd5cbd26da831489e6 100644
--- a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp
+++ b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp
@@ -1,6 +1,7 @@
#include "CommandDispatcher.h"
-CommandDispatcher::CommandDispatcher()
-{
+namespace nemo_interface {
-}
+CommandDispatcher::CommandDispatcher() {}
+
+} // namespace nemo_interface
diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h
index d2f22f738f2b804b595aaa244180c003a1a85169..501bdd59f9142e18bda561a752a6d973c6a40bb6 100644
--- a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h
+++ b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h
@@ -1,11 +1,31 @@
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
+#include <QThread>
+#include <QVariant>
-class CommandDispatcher
-{
+#include "Task.h"
+
+namespace nemo_interface {
+
+class CommandDispatcher : public QThread {
public:
- CommandDispatcher();
+ 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
diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
index bcdfc1d4fc8d62204adfe27190f62084bf4de7bf..888d0477f89249cee885e7a45423917843e6ea78 100644
--- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
+++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
@@ -1,4 +1,4 @@
-#include "MeasurementTile.h"
+#include "MeasurementTile.h"
MeasurementTile::MeasurementTile(QObject *parent)
: GeoArea(parent), _progress(0), _id(0) {
@@ -33,15 +33,17 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
void MeasurementTile::init() { this->setObjectName("Tile"); }
-uint64_t MeasurementTile::id() const { return _id; }
+int64_t MeasurementTile::id() const { return _id; }
-void MeasurementTile::setId(const uint64_t &id) {
+void MeasurementTile::setId(const int64_t &id) {
if (_id != id) {
_id = id;
emit idChanged();
}
}
+QList<QGeoCoordinate> MeasurementTile::tile() const { return coordinateList(); }
+
double MeasurementTile::progress() const { return _progress; }
void MeasurementTile::setProgress(double progress) {
diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
index a9409c00bf73bd223aadaa47575b426647360353..490828751bd582a3ce2297aa008d2c11f72ee9d8 100644
--- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
+++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
@@ -14,7 +14,6 @@ public:
MeasurementTile &operator=(const MeasurementTile &other);
Q_PROPERTY(double progress READ progress NOTIFY progressChanged)
- Q_PROPERTY(long id READ id NOTIFY idChanged)
virtual QString mapVisualQML() const override;
virtual QString editorQML() const override;
@@ -25,8 +24,10 @@ public:
double progress() const;
void setProgress(double progress);
- uint64_t id() const;
- void setId(const uint64_t &id);
+ int64_t id() const;
+ void setId(const int64_t &id);
+
+ QList<QGeoCoordinate> tile() const;
signals:
void progressChanged();
@@ -35,5 +36,5 @@ signals:
private:
void init();
double _progress;
- long _id;
+ int64_t _id;
};
diff --git a/src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc b/src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc
deleted file mode 100644
index d7dc1a396652cc9e5e39dac379334ecf7838f641..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "QNemoProgress.h"
-
diff --git a/src/MeasurementComplexItem/nemo_interface/QNemoProgress.h b/src/MeasurementComplexItem/nemo_interface/QNemoProgress.h
deleted file mode 100644
index e57ec2fdf108993dff80f7d5544b6d10a77c39c2..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/QNemoProgress.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#pragma once
-
-#include <QVector>
-
-#include "ros_bridge/include/messages/nemo_msgs/progress.h"
-
-namespace nemo = ros_bridge::messages::nemo_msgs;
-typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;
diff --git a/src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h b/src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h
deleted file mode 100644
index cd2a9c8c4a76cb6eb53a8c68f401f396d466ed1f..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h
+++ /dev/null
@@ -1,4 +0,0 @@
-#pragma once
-#include "geometry/GenericPolygon.h"
-
-using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;
diff --git a/src/MeasurementComplexItem/nemo_interface/SnakeTiles.h b/src/MeasurementComplexItem/nemo_interface/SnakeTiles.h
deleted file mode 100644
index 2a0ee2d9fd641feab7cb1b51fef086959208d6b6..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/SnakeTiles.h
+++ /dev/null
@@ -1,5 +0,0 @@
-#pragma once
-#include "MeasurementTile.h"
-#include "Wima/Geometry/GenericPolygonArray.h"
-
-using SnakeTiles = GenericPolygonArray<MeasurementTile, QVector>;
diff --git a/src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h b/src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h
deleted file mode 100644
index 8517882e35d8a722dafee1df8ba5bb120a12ce53..0000000000000000000000000000000000000000
--- a/src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#pragma once
-
-#include "MeasurementComplexItem//geometry/GenericPolygonArray.h"
-#include "MeasurementComplexItem//nemo_interface/SnakeTileLocal.h"
-#include <vector>
-typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;
diff --git a/src/MeasurementComplexItem/nemo_interface/Task.cpp b/src/MeasurementComplexItem/nemo_interface/Task.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1033e055b69b7ea28f570d28c552794a37f631d
--- /dev/null
+++ b/src/MeasurementComplexItem/nemo_interface/Task.cpp
@@ -0,0 +1,11 @@
+#include "Task.h"
+
+namespace nemo_interface {
+
+Task::Task(const Task::Functor &onExec)
+ : _onExec(onExec), _isExpired([] { return false; }),
+ _isReady([] { return true; }) {}
+
+void Task::exec(std::promise<QVariant> p) { this->_onExec(std::move(p)); }
+
+} // namespace nemo_interface
diff --git a/src/MeasurementComplexItem/nemo_interface/Task.h b/src/MeasurementComplexItem/nemo_interface/Task.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f3a6361cb9e49a3cdcdd39768a3ac134d77ed3d
--- /dev/null
+++ b/src/MeasurementComplexItem/nemo_interface/Task.h
@@ -0,0 +1,33 @@
+#ifndef COMMAND_H
+#define COMMAND_H
+
+#include <functional>
+#include <future>
+
+#include <QVariant>
+
+namespace nemo_interface {
+
+class Task {
+public:
+ typedef std::function<void(std::promise<QVariant> p)> Functor;
+ typedef std::function<bool()> BooleanFunctor;
+
+ Task(const Functor &onExec);
+
+ void exec(std::promise<QVariant> p);
+ bool isReady(); // default == true
+ bool isExpired(); // default == false
+
+ void setOnExec(const Functor &onExec);
+ void setIsReady(const BooleanFunctor &isReady);
+ void setIsExpired(const BooleanFunctor &isExpired);
+
+private:
+ Functor _onExec;
+ BooleanFunctor _isExpired;
+ BooleanFunctor _isReady;
+};
+} // namespace nemo_interface
+
+#endif // COMMAND_H
diff --git a/src/MeasurementComplexItem/nemo_interface/tileHelper.h b/src/MeasurementComplexItem/nemo_interface/tileHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..e5e373962e7f5f6d82950f6336ca8b0fa5731c81
--- /dev/null
+++ b/src/MeasurementComplexItem/nemo_interface/tileHelper.h
@@ -0,0 +1,57 @@
+#ifndef TILEHELPER_H
+#define TILEHELPER_H
+
+#include <iostream>
+
+namespace nemo_interface {
+
+namespace coordinate {
+template <class Coordinate> inline std::int64_t getHash(const Coordinate &t) {
+ return double(t.latitude()) ^ double(t.longitude()) ^ double(t.altitude());
+}
+} // namespace coordinate
+
+namespace coordinate_array {
+template <class Container> inline std::int64_t getHash(const Container &c) {
+ std::int64_t hash = 0;
+ for (const auto &entry : c) {
+ hash ^= coordinate::getHash(entry);
+ }
+}
+} // namespace coordinate_array
+
+namespace tile {
+template <class Tile> inline std::int64_t getHash(const Tile &t) {
+ return coordinate_array::getHash(t.tile());
+}
+} // namespace tile
+
+namespace tile_array {
+template <class Container> inline getHash(const Container &c) {
+ std::int64_t hash = 0;
+ for (const auto &entry : c) {
+ hash ^= tile::getHash(entry);
+ }
+}
+} // namespace tile_array
+
+namespace labeled_progress {
+template <class LabeledProgress> inline getHash(const LabeledProgress &lp) {
+ return std::int64_t(lp.id()) ^ double(lp.progress());
+}
+
+namespace progress_array {
+template <class Container> inline getHash(const Container &c) {
+ std::int64_t hash = 0;
+ for (const auto &entry : c) {
+ hash ^= labeled_progress::getHash(entry);
+ }
+}
+
+} // namespace progress_array
+
+} // namespace labeled_progress
+
+} // namespace nemo_interface
+
+#endif // TILEHELPER_H
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp
index 5b6d22c09cd9baae8563688159cbea52f7f3a552..52d638b7bf300b86d9db711b37a72852cdbb0992 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.cpp
+++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp
@@ -302,6 +302,8 @@ void RosbridgeWsClient::run() {
});
}
+bool RosbridgeWsClient::running() { return !this->stopped->load(); }
+
void RosbridgeWsClient::stop() {
if (stopped->load())
return;
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h
index 251922941a1bbb2fc5207300557fa058044201a7..e305216151fd7713a25fcdacd181d665a324edc9 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.h
+++ b/src/comm/ros_bridge/include/RosBridgeClient.h
@@ -1,56 +1,55 @@
#pragma once
/*
-* Created on: Apr 16, 2018
-* Author: Poom Pianpak
-*/
+ * Created on: Apr 16, 2018
+ * Author: Poom Pianpak
+ */
#include "Simple-WebSocket-Server/client_ws.hpp"
#include "rapidjson/include/rapidjson/document.h"
-#include "rapidjson/include/rapidjson/writer.h"
#include "rapidjson/include/rapidjson/stringbuffer.h"
+#include "rapidjson/include/rapidjson/writer.h"
+#include <deque>
#include <functional>
#include <mutex>
-#include <tuple>
-#include <deque>
#include <thread>
+#include <tuple>
bool is_valid_port_path(std::string server_port_path);
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
-using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
+using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>,
+ std::shared_ptr<WsClient::InMessage>)>;
template <typename T>
-constexpr typename std::underlying_type<T>::type integral(T value)
-{
- return static_cast<typename std::underlying_type<T>::type>(value);
+constexpr typename std::underlying_type<T>::type integral(T value) {
+ return static_cast<typename std::underlying_type<T>::type>(value);
}
-class RosbridgeWsClient
-{
- enum class EntryType{
- SubscribedTopic,
- AdvertisedTopic,
- AdvertisedService,
- };
-
- using EntryData = std::tuple<EntryType,
- std::string /*service/topic name*/,
- std::string /*client name*/,
- std::weak_ptr<WsClient> /*client*/>;
-
- enum class EntryEnum{
- EntryType = 0,
- ServiceTopicName = 1,
- ClientName = 2,
- WPClient = 3
- };
+class RosbridgeWsClient {
+ enum class EntryType {
+ SubscribedTopic,
+ AdvertisedTopic,
+ AdvertisedService,
+ };
+
+ using EntryData = std::tuple<EntryType, std::string /*service/topic name*/,
+ std::string /*client name*/,
+ std::weak_ptr<WsClient> /*client*/>;
+
+ enum class EntryEnum {
+ EntryType = 0,
+ ServiceTopicName = 1,
+ ClientName = 2,
+ WPClient = 3
+ };
const std::string server_port_path;
std::unordered_map<std::string /*client name*/,
- std::shared_ptr<WsClient> /*client*/> client_map;
+ std::shared_ptr<WsClient> /*client*/>
+ client_map;
std::deque<EntryData> service_topic_list;
std::mutex mutex;
std::shared_ptr<std::atomic_bool> isConnected;
@@ -59,38 +58,39 @@ class RosbridgeWsClient
std::string available_services;
std::shared_ptr<std::thread> periodic_thread;
-
-
- void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message);
+ void start(const std::string &client_name, std::shared_ptr<WsClient> client,
+ const std::string &message);
public:
- RosbridgeWsClient(const std::string& server_port_path);
- RosbridgeWsClient(const std::string& server_port_path, bool run);
+ RosbridgeWsClient(const std::string &server_port_path);
+ RosbridgeWsClient(const std::string &server_port_path, bool run);
~RosbridgeWsClient();
- bool connected();
+ bool connected();
- void run();
- void stop();
- void reset();
+ void run();
+ bool running();
+ void stop();
+ void reset();
// Adds a client to the client_map.
- void addClient(const std::string& client_name);
+ void addClient(const std::string &client_name);
- std::shared_ptr<WsClient> getClient(const std::string& client_name);
+ std::shared_ptr<WsClient> getClient(const std::string &client_name);
- void stopClient(const std::string& client_name);
+ void stopClient(const std::string &client_name);
- void removeClient(const std::string& client_name);
+ void removeClient(const std::string &client_name);
//!
//! \brief Returns a string containing all advertised topics.
//! \return Returns a string containing all advertised topics.
//!
- //! \note This function will wait until the /rosapi/topics service is available.
- //! \note Call connected() in advance to ensure that a connection was established.
- //! \pre Connection must be established, \see \fn connected().
+ //! \note This function will wait until the /rosapi/topics service is
+ //! available. \note Call connected() in advance to ensure that a connection
+ //! was established. \pre Connection must be established, \see \fn
+ //! connected().
//!
std::string getAdvertisedTopics();
@@ -98,16 +98,18 @@ public:
//! \brief Returns a string containing all advertised services.
//! \return Returns a string containing all advertised services.
//!
- //! \note This function will wait until the /rosapi/services service is available.
- //! \note Call connected() in advance to ensure that a connection was established.
+ //! \note This function will wait until the /rosapi/services service is
+ //! available. \note Call connected() in advance to ensure that a connection
+ //! was established.
//!
std::string getAdvertisedServices();
bool topicAvailable(const std::string &topic);
- // Gets the client from client_map and starts it. Advertising is essentially sending a message.
- // One client per topic!
- void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "");
+ // Gets the client from client_map and starts it. Advertising is essentially
+ // sending a message. One client per topic!
+ void advertise(const std::string &client_name, const std::string &topic,
+ const std::string &type, const std::string &id = "");
//!
//! \brief Unadvertises the topice \p topic
@@ -115,19 +117,19 @@ public:
//! \param id
//! \pre The topic \p topic must be advertised, \see topicAvailable().
//!
- void unadvertise(const std::string& topic,
- const std::string& id = "");
+ void unadvertise(const std::string &topic, const std::string &id = "");
void unadvertiseAll();
- //!
- //! \brief Publishes the message \p msg to the topic \p topic.
- //! \param topic The topic to publish the message.
- //! \param msg The message to publish.
- //! \param id
- //! \pre The topic \p topic must be advertised, \see topicAvailable().
- //!
- void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "");
+ //!
+ //! \brief Publishes the message \p msg to the topic \p topic.
+ //! \param topic The topic to publish the message.
+ //! \param msg The message to publish.
+ //! \param id
+ //! \pre The topic \p topic must be advertised, \see topicAvailable().
+ //!
+ void publish(const std::string &topic, const rapidjson::Document &msg,
+ const std::string &id = "");
//!
//! \brief Subscribes the client \p client_name to the topic \p topic.
@@ -142,57 +144,71 @@ public:
//! \param compression
//! \pre The topic \p topic must be advertised, \see topicAvailable().
//!
- void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "");
+ void subscribe(const std::string &client_name, const std::string &topic,
+ const InMessage &callback, const std::string &id = "",
+ const std::string &type = "", int throttle_rate = -1,
+ int queue_length = -1, int fragment_size = -1,
+ const std::string &compression = "");
- void unsubscribe(const std::string& topic,
- const std::string& id = "");
+ void unsubscribe(const std::string &topic, const std::string &id = "");
void unsubscribeAll();
- void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback);
+ void advertiseService(const std::string &client_name,
+ const std::string &service, const std::string &type,
+ const InMessage &callback);
- void unadvertiseService(const std::string& service);
+ void unadvertiseService(const std::string &service);
void unadvertiseAllServices();
- void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values);
+ void serviceResponse(const std::string &service, const std::string &id,
+ bool result, const rapidjson::Document &values);
- void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "");
+ void callService(const std::string &service, const InMessage &callback,
+ const rapidjson::Document &args = {},
+ const std::string &id = "", int fragment_size = -1,
+ const std::string &compression = "");
//!
//! \brief Checks if the service \p service is available.
//! \param service Service name.
//! \return Returns true if the service is available, false either.
- //! \note Don't call this function to frequently. Use \fn waitForService() instead.
+ //! \note Don't call this function to frequently. Use \fn waitForService()
+ //! instead.
//!
- bool serviceAvailable(const std::string& service);
+ bool serviceAvailable(const std::string &service);
//!
//! \brief Waits until the service with the name \p service is available.
//! \param service Service name.
//! @note This function will block as long as the service is not available.
//!
- void waitForService(const std::string& service);
+ void waitForService(const std::string &service);
//!
//! \brief Waits until the service with the name \p service is available.
//! \param service Service name.
//! \param stop Flag to stop waiting.
- //! @note This function will block as long as the service is not available or \p stop is false.
+ //! @note This function will block as long as the service is not available or
+ //! \p stop is false.
//!
- void waitForService(const std::string& service, const std::function<bool(void)> stop);
+ void waitForService(const std::string &service,
+ const std::function<bool(void)> stop);
//!
//! \brief Waits until the topic with the name \p topic is available.
//! \param topic Topic name.
//! @note This function will block as long as the topic is not available.
//!
- void waitForTopic(const std::string& topic);
+ void waitForTopic(const std::string &topic);
//!
//! \brief Waits until the topic with the name \p topic is available.
//! \param topic Topic name.
//! \param stop Flag to stop waiting.
- //! @note This function will block as long as the topic is not available or \p stop is false.
+ //! @note This function will block as long as the topic is not available or \p
+ //! stop is false.
//!
- void waitForTopic(const std::string& topic, const std::function<bool(void)> stop);
+ void waitForTopic(const std::string &topic,
+ const std::function<bool(void)> stop);
};
diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp
deleted file mode 100644
index 844dc874423d8080446692bc52dec3b953956cd4..0000000000000000000000000000000000000000
--- a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "polygon_array.h"
-
-std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
- return "jsk_recognition_msgs/PolygonArray";
-}
-
diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
deleted file mode 100644
index 05a5802ee9ff9d24132d031d7f543c217bc19484..0000000000000000000000000000000000000000
--- a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
+++ /dev/null
@@ -1,328 +0,0 @@
-#pragma once
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/message_traits.h"
-#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
-#include "ros_bridge/include/messages/std_msgs/header.h"
-
-#include <type_traits>
-#include <vector>
-
-namespace ros_bridge {
-//! @brief Namespace containing classes and methodes ros message generation.
-namespace messages {
-//! @brief Namespace containing classes and methodes for geometry_msgs generation.
-namespace jsk_recognition_msgs {
-//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
-namespace polygon_array {
-
-
-std::string messageType();
-
-
-//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
-template <class PolygonType = geometry_msgs::polygon_stamped::PolygonStamped,
- template <class, class...> class ContainerType = std::vector,
- class HeaderType = std_msgs::header::Header,
- class IntType = long,
- class FloatType = double>
-class GenericPolygonArray{
-public:
- GenericPolygonArray() {}
- GenericPolygonArray(const GenericPolygonArray &other)
- : _header(other.header())
- , _polygons(other.polygons())
- , _labels(other.labels())
- , _likelihood(other.likelihood())
- {}
- GenericPolygonArray(const HeaderType &header,
- const ContainerType<PolygonType> &polygons,
- const ContainerType<IntType> &labels,
- const ContainerType<FloatType> &likelihood)
- : _header(header)
- , _polygons(polygons)
- , _labels(labels)
- , _likelihood(likelihood)
- {}
-
- const HeaderType &header() const {return _header;}
- HeaderType &header() {return _header;}
- const ContainerType<PolygonType> &polygons() const {return _polygons;}
- ContainerType<PolygonType> &polygons() {return _polygons;}
- const ContainerType<IntType> &labels() const {return _labels;}
- ContainerType<IntType> &labels() {return _labels;}
- const ContainerType<FloatType> &likelyhood() const {return _likelihood;}
- ContainerType<FloatType> &likelyhood() {return _likelihood;}
-
-
-private:
- HeaderType _header;
- ContainerType<PolygonType> _polygons;
- ContainerType<IntType> _labels;
- ContainerType<FloatType> _likelihood;
-};
-typedef GenericPolygonArray<> PolygonArray;
-
-
-namespace detail {
- //! Helper functions to generate Json entries for labels and likelihood.
-
- //! \note \p p has member \fn labels().
- template <class PolygonArrayType, int k>
- void labelsToJson(const PolygonArrayType &p,
- rapidjson::Value &labels,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<k>){
- for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
- labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
- }
-
- //! \note \p p has no member \fn labels().
- template <class PolygonArrayType>
- void labelsToJson(const PolygonArrayType &p,
- rapidjson::Value &labels,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<0>){
- for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
- labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
- }
-
- //! \note \p p has member \fn likelihood().
- template <class PolygonArrayType, int k>
- void likelihoodToJson(const PolygonArrayType &p,
- rapidjson::Value &likelyhood,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<k>){
- p.likelyhood().clear();
- for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
- likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
- }
-
- //! \note \p p has no member \fn likelihood().
- template <class PolygonArrayType>
- void likelihoodToJson(const PolygonArrayType &p,
- rapidjson::Value &likelyhood,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<0>){
- for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
- likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
- }
-
- //! \note \p p has member \fn labels().
- template <class PolygonArrayType, int k>
- void setLabels(const rapidjson::Value &doc,
- PolygonArrayType &p,
- traits::Int2Type<k>){
- p.labels().clear();
- for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
- p.labels().push_back(doc[i]);
- }
-
- //! \note \p p has no member \fn labels().
- template <class PolygonArrayType>
- void setLabels(const rapidjson::Value &doc,
- PolygonArrayType &p,
- traits::Int2Type<0>){
- (void)doc;
- (void)p;
- }
-
- //! \note \p p has member \fn likelihood().
- template <class PolygonArrayType, int k>
- void setLikelihood(const rapidjson::Value &doc,
- PolygonArrayType &p,
- traits::Int2Type<k>){
- for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
- p.likelihood().push_back(doc[i]);
- }
-
- //! \note \p p has no member \fn likelihood().
- template <class PolygonArrayType>
- void setLikelihood(const rapidjson::Value &doc,
- PolygonArrayType &p,
- traits::Int2Type<0>){
- (void)doc;
- (void)p;
- }
-
- template <class PolygonArrayType, class HeaderType>
- bool _toJson(const PolygonArrayType &p,
- const HeaderType &h,
- rapidjson::Value &value,
- rapidjson::Document::AllocatorType &allocator)
- {
- using namespace std_msgs;
- using namespace geometry_msgs;
-
- rapidjson::Document jHeader(rapidjson::kObjectType);
- if (!header::toJson(h, jHeader, allocator)){
- assert(false);
- return false;
- }
- value.AddMember("header", jHeader, allocator);
-
- rapidjson::Value jPolygons(rapidjson::kArrayType);
- for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
- rapidjson::Document jPolygon(rapidjson::kObjectType);
- if (!polygon_stamped::toJson(p.polygons()[i].polygon(), h, jPolygon, allocator)){
- assert(false);
- return false;
- }
- jPolygons.PushBack(jPolygon, allocator);
- }
- value.AddMember("polygons", jPolygons, allocator);
-
-
- rapidjson::Value jLabels(rapidjson::kArrayType);
- typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
- detail::labelsToJson(p, jLabels, allocator, traits::Int2Type<HasLabels::value>());
- value.AddMember("labels", jLabels, allocator);
-
-
- rapidjson::Value jLikelihood(rapidjson::kArrayType);
- typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
- detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
- value.AddMember("likelihood", jLikelihood, allocator);
-
- return true;
- }
-
- template<class PolygonArrayType, int k>
- bool _toJson(const PolygonArrayType &p,
- rapidjson::Value &value,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<k>)
- {
- // U has member header(), use integraded header.
- return _toJson(p, p.header(), value, allocator);
- }
-
- template<class PolygonArrayType>
- bool _toJson(const PolygonArrayType &p,
- rapidjson::Value &value,
- rapidjson::Document::AllocatorType &allocator,
- traits::Int2Type<0>)
- {
- // U has no member header(), generate one on the fly.
- using namespace std_msgs::header;
- return _toJson(p, Header(), value, allocator);
- }
-}
-
-
-//!
-//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
-//! \param p Class implementing the PolygonArray interface.
-//! \param h Class implementing the Header interface.
-//! \param doc Rapidjson document used to store the PolygonArray message.
-//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-//!
-//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-//! will be replaced by arrays filled with zero and size p.polygons.size().
-//!
-//! \note If this function is called, the headers in p.polygons[i] (entries implement the the PolygonStamped interface)
-//! are ignored.
-template <class PolygonArrayType, class HeaderType>
-bool toJson(const PolygonArrayType &p,
- const HeaderType &h,
- rapidjson::Value &value,
- rapidjson::Document::AllocatorType &allocator)
-{
- return detail::_toJson(p, h, value, allocator);
-}
-
-
-
-//!
-//! Create PolygonArray message from \p p. \p p contains a header.
-//! \param p Class implementing the PolygonArrayType interface.
-//! \param doc Rapidjson document used to store the PolygonArray message.
-//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-//!
-//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-//! will be replaced by arrays filled with zero and size p.polygons.size().
-//!
-//! \note If the header() function is missing, a default constructed header is used.
-template <class PolygonArrayType>
-bool toJson(const PolygonArrayType &p,
- rapidjson::Value &value,
- rapidjson::Document::AllocatorType &allocator)
-{
- typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
- return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
-}
-
-
-
-template <class PolygonArrayType>
-bool fromJson(const rapidjson::Value &value,
- PolygonArrayType &p)
-{
- using namespace geometry_msgs;
- if ( !value.HasMember("header")){
- assert(false);
- return false;
- }
- if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
- assert(false);
- return false;
- }
- if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
- assert(false);
- return false;
- }
- if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
- assert(false);
- return false;
- }
-
-
- typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
- if ( !polygon_stamped::detail::setHeader(value["header"],
- p,
- traits::Int2Type<HasHeader::value>())){
- assert(false);
- return false;
- }
-
- const auto &jPolygonStamped = value["polygons"];
- p.polygons().clear();
- p.polygons().reserve(jPolygonStamped.Size());
- typedef decltype (p.polygons()[0]) PolyStampedCVR;
- typedef typename std::remove_cv_t<
- typename std::remove_reference_t<PolyStampedCVR>>
- PolyStamped;
- for (unsigned int i=0; i < jPolygonStamped.Size(); ++i) {
- if ( !jPolygonStamped[i].HasMember("header") ){
- assert(false);
- return false;
- }
- PolyStamped polygonStamped;
-
- if ( !polygon_stamped::detail::setHeader(jPolygonStamped[i]["header"],
- polygonStamped,
- traits::Int2Type<HasHeader::value>())){
- assert(false);
- return false;
- }
-
- if ( !polygon::fromJson(jPolygonStamped[i]["polygon"], polygonStamped.polygon())){
- assert(false);
- return false;
- }
- p.polygons().push_back(std::move(polygonStamped));
- }
-
- typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
- detail::setLabels(value["labels"], p, traits::Int2Type<HasLabels::value>());
-
- typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
- detail::setLikelihood(value["likelihood"], p, traits::Int2Type<HasLikelihood::value>());
-
- return true;
-}
-
-
-} // namespace polygon_array
-} // namespace geometry_msgs
-} // namespace messages
-} // namespace ros_bridge
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
index c244688a5a7c53f32f853ec8d0c249e53ea845a2..6d6a753cf0f2031a8d2b177af57ce6eeca87996e 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
@@ -5,46 +5,48 @@
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
-//! @brief Namespace containing classes and methodes for geometry_msgs generation.
+//! @brief Namespace containing classes and methodes for geometry_msgs
+//! generation.
namespace nemo_msgs {
-//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
+//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message
+//! generation.
namespace heartbeat {
-
std::string messageType();
-
//! @brief C++ representation of nemo_msgs/Heartbeat message
-class Heartbeat{
+class Heartbeat {
public:
- Heartbeat() : _status(0){}
- Heartbeat(int status) :_status(status){}
- Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()){}
+ Heartbeat() : _status(0) {}
+ Heartbeat(int status) : _status(status) {}
+ Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()) {}
+
+ bool operator==(const Heartbeat &hb) { return hb._status == this->_status; }
+ bool operator!=(const Heartbeat &hb) { return !operator==(hb); }
+
+ virtual int status(void) const { return _status; }
+ virtual void setStatus(int status) { _status = status; }
- virtual int status (void)const {return _status;}
- virtual void setStatus (int status) {_status = status;}
protected:
- int _status;
+ int _status;
};
-
template <class HeartbeatType>
-bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-{
- value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
- return true;
+bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value,
+ rapidjson::Document::AllocatorType &allocator) {
+ value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
+ return true;
}
template <class HeartbeatType>
-bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
-{
- if (!value.HasMember("status") || !value["status"].IsInt()){
- assert(false);
- return false;
- }
-
- heartbeat.setStatus(value["status"].GetInt());
- return true;
+bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) {
+ if (!value.HasMember("status") || !value["status"].IsInt()) {
+ assert(false);
+ return false;
+ }
+
+ heartbeat.setStatus(value["status"].GetInt());
+ return true;
}
} // namespace heartbeat
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h
index e6fad66f8a564a63a356d41baa1a5ca74e77c789..c20111918d1945699b64da5934378ec7e57a980f 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h
@@ -41,8 +41,8 @@ protected:
template <class LabeledProgressType>
bool toJson(const LabeledProgressType &lp, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
- value.AddMember(idKey, lb.id(), allocator);
- value.AddMember(progressKey, lb.progress(), allocator);
+ value.AddMember(idKey, lp.id(), allocator);
+ value.AddMember(progressKey, lp.progress(), allocator);
return true;
}
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
index 3b09304102ca26f5d1f26131227c55168d44680a..b5091013817796f2c68efc7d7a2f4bc8a77235c7 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
@@ -1,6 +1,5 @@
#include "tile.h"
-tile::tile()
-{
-
+std::string ros_bridge::messages::nemo_msgs::tile::messageType() {
+ return "nemo_msgs/Tile";
}
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
index a20e25342e206c352ee292a5f19e2e507165a49c..5b5915820bd65229153fa2f83074509f72309a02 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
@@ -13,7 +13,7 @@ namespace messages {
namespace nemo_msgs {
//! @brief Namespace containing methodes for nemo_msgs/tile message
//! generation.
-namespace labeled_progress {
+namespace tile {
std::string messageType();
@@ -24,7 +24,8 @@ const char *tileKey = "tile";
} // namespace
//! @brief C++ representation of nemo_msgs/tile message
-template <class GeoPoint, template <class, class> class Container = std::vector>
+template <class GeoPoint = geographic_msgs::geo_point::GeoPoint,
+ template <class, class...> class Container = std::vector>
class GenericTile {
public:
typedef Container<GeoPoint> GeoContainer;
@@ -52,25 +53,30 @@ typedef GenericTile<> Tile;
template <class TileType>
bool toJson(const TileType &tile, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
- value.AddMember(idKey, tile.id(), allocator);
- value.AddMember(progressKey, tile.progress(), allocator);
+
+ using namespace rapidjson;
+ value.AddMember(Value(idKey, allocator), Value(tile.id()), allocator);
+ value.AddMember(Value(progressKey, allocator), Value(tile.progress()),
+ allocator);
using namespace messages::geographic_msgs;
rapidjson::Value geoPoints(rapidjson::kArrayType);
for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) {
- rapidjson::Document geoPoint(rapidjson::kObjectType);
+ rapidjson::Value geoPoint(rapidjson::kObjectType);
if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) {
return false;
}
geoPoints.PushBack(geoPoint, allocator);
}
- value.AddMember(tileKey, geoPoints, allocator);
+
+ rapidjson::Value key(tileKey, allocator);
+ value.AddMember(key, geoPoints, allocator);
return true;
}
-template <class ProgressType>
-bool fromJson(const rapidjson::Value &value, ProgressType &p) {
+template <class TileType>
+bool fromJson(const rapidjson::Value &value, TileType &p) {
if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
return false;
}
@@ -79,13 +85,34 @@ bool fromJson(const rapidjson::Value &value, ProgressType &p) {
return false;
}
+ if (!value.HasMember(tileKey) || !value[tileKey].IsArray()) {
+ return false;
+ }
+
p.setId(value[idKey].GetInt());
p.setProgress(value[progressKey].GetDouble());
+ using namespace geographic_msgs;
+
+ const auto &jsonArray = value[tileKey].GetArray();
+ p.tile().clear();
+ p.tile().reserve(jsonArray.Size());
+ typedef decltype(p.tile()[0]) PointTypeCVR;
+ typedef
+ typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>>
+ PointType;
+ for (long i = 0; i < jsonArray.Size(); ++i) {
+ PointType pt;
+ if (!geo_point::fromJson(jsonArray[i], pt)) {
+ return false;
+ }
+ p.tile().push_back(std::move(pt));
+ }
+
return true;
}
-} // namespace labeled_progress
+} // namespace tile
} // namespace nemo_msgs
} // namespace messages
} // namespace ros_bridge