Skip to content

Q
qgroundcontrol
Commit 5beee98a authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

temp

parent 67247b3b
Show whitespace changes
Inline Side-by-side
Showing with 575 additions and 794 deletions
qgroundcontrol.pro
View file @ 5beee98a
......@@ -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 \
......
src/MeasurementComplexItem/HashFunctions.h
View file @ 5beee98a
......@@ -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);
}
};
......
src/MeasurementComplexItem/IDArray.h
View file @ 5beee98a
#ifndef IDARRAY_H
#define IDARRAY_H
typedef QVector<long> IDArray;
#include <QVector>
typedef QVector<std::int64_t> IDArray;
#endif // IDARRAY_H
src/MeasurementComplexItem/LogicalArray.h
View file @ 5beee98a
#ifndef LOGICALARRAY_H
#define LOGICALARRAY_H
#include <QVector>
typedef QVector<bool> LogicalArray;
#endif // LOGICALARRAY_H
src/MeasurementComplexItem/NemoInterface.cpp
View file @ 5beee98a
#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"
......@@ -31,37 +30,34 @@
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define EVENT_TIMER_INTERVAL 100 // ms
auto constexpr static timeoutInterval = std::chrono::milliseconds(3000);
#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);
if (this->tilesENU.polygons().size() > 0 && this->running_ &&
this->topicServiceSetupDone) {
this->publishENUOrigin();
this->publishTilesENU();
}
}
bool NemoInterface::Impl::running() { return _running(this->_state); }
NemoInterface::STATUS NemoInterface::Impl::status() {
return _translate(this->_state);
}
bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); }
QVector<int> NemoInterface::Impl::progress() {
SharedLock lk(this->progressMutex);
return this->qProgress.progress();
}
const QString &NemoInterface::Impl::infoString() { return _infoString; }
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() {
using namespace ros_bridge::messages;
// Subscribe nemo progress.
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();
});
// Subscribe /nemo/heartbeat.
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));
void NemoInterface::Impl::_setHeartbeat(const QNemoHeartbeat &hb) {
if (this->_lastHeartbeat != hb) {
_lastHeartbeat = hb;
if (ready()) {
this->_timeoutTimer.stop();
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
}
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;
}
});
}
void NemoInterface::Impl::loop() {
// 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) {
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);
}
}
void NemoInterface::Impl::_setInfoString(const QString &info) {
if (_infoString != info) {
_infoString = info;
emit this->_parent->infoStringChanged();
}
}
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::_setWarningString(const QString &warning) {
if (_warningString != warning) {
_warningString = warning;
emit this->_parent->warningStringChanged();
}
}
void NemoInterface::Impl::publishTilesENU() {
void NemoInterface::Impl::_doTopicServiceSetup() {
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.";
}
// Subscribe nemo progress.
const char *progressClient = "client:/nemo/progress";
this->_pRosBridge->addClient(progressClient);
this->_pRosBridge->subscribe(
progressClient, "/nemo/progress",
[this](std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) {
qDebug() << "doTopicServiceSetup(): /nemo/progress: "
<< in_message->string().c_str();
qDebug() << "impl missing";
});
// 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,
std::shared_ptr<WsClient::InMessage> in_message) {
qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: "
<< in_message->string().c_str();
qDebug() << "impl missing";
});
}
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.";
void NemoInterface::Impl::_doAction() {
// Check ROS Bridge status and do setup if necessary.
switch (this->_state) {
case STATE::STOPPED:
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);
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");
......
src/MeasurementComplexItem/NemoInterface.h
View file @ 5beee98a
......@@ -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();
......
src/MeasurementComplexItem/TileArray.h
View file @ 5beee98a
#ifndef TILEARRAY_H
#define TILEARRAY_H
#include "MeasurementTile.h"
typedef QVector<MeasurementTile> TileArray;
#endif // TILEARRAY_H
src/MeasurementComplexItem/TilePtrArray.h
View file @ 5beee98a
#ifndef TILEPTRARRAY_H
#define TILEPTRARRAY_H
#include "MeasurementTile.h"
typedef QVector<MeasurementTile *> TilePtrArray;
#endif // TILEPTRARRAY_H
src/MeasurementComplexItem/nemo_interface/Command.cpp deleted 100644 → 0
View file @ 67247b3b
#include "Command.h"
namespace nemo_interface {
Command::Command(Functor onExec) : _onExec(onExec) {}
QFuture<Command::ERROR> Command::exec() { return _onExec(); }
} // namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/Command.h deleted 100644 → 0
View file @ 67247b3b
#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
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp
View file @ 5beee98a
#include "CommandDispatcher.h"
CommandDispatcher::CommandDispatcher()
{
namespace nemo_interface {
}
CommandDispatcher::CommandDispatcher() {}
} // namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h
View file @ 5beee98a
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
#include <QThread>
#include <QVariant>
class CommandDispatcher
{
#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/MeasurementTile.cpp
View file @ 5beee98a
#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) {
......
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
View file @ 5beee98a
......@@ -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;
};
src/MeasurementComplexItem/nemo_interface/QNemoProgress.h deleted 100644 → 0
View file @ 67247b3b
#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;
src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h deleted 100644 → 0
View file @ 67247b3b
#pragma once
#include "geometry/GenericPolygon.h"
using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;
src/MeasurementComplexItem/nemo_interface/SnakeTiles.h deleted 100644 → 0
View file @ 67247b3b
#pragma once
#include "MeasurementTile.h"
#include "Wima/Geometry/GenericPolygonArray.h"
using SnakeTiles = GenericPolygonArray<MeasurementTile, QVector>;
src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h deleted 100644 → 0
View file @ 67247b3b
#pragma once
#include "MeasurementComplexItem//geometry/GenericPolygonArray.h"
#include "MeasurementComplexItem//nemo_interface/SnakeTileLocal.h"
#include <vector>
typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;
src/MeasurementComplexItem/nemo_interface/Task.cpp 0 → 100644
View file @ 5beee98a
#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
src/MeasurementComplexItem/nemo_interface/Task.h 0 → 100644
View file @ 5beee98a
#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
src/MeasurementComplexItem/nemo_interface/tileHelper.h 0 → 100644
View file @ 5beee98a
#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
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @ 5beee98a
......@@ -302,6 +302,8 @@ void RosbridgeWsClient::run() {
});
}
bool RosbridgeWsClient::running() { return !this->stopped->load(); }
void RosbridgeWsClient::stop() {
if (stopped->load())
return;
......
src/comm/ros_bridge/include/RosBridgeClient.h
View file @ 5beee98a
#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)
{
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{
class RosbridgeWsClient {
enum class EntryType {
SubscribedTopic,
AdvertisedTopic,
AdvertisedService,
};
using EntryData = std::tuple<EntryType,
std::string /*service/topic name*/,
using EntryData = std::tuple<EntryType, std::string /*service/topic name*/,
std::string /*client name*/,
std::weak_ptr<WsClient> /*client*/>;
enum class EntryEnum{
enum class EntryEnum {
EntryType = 0,
ServiceTopicName = 1,
ClientName = 2,
......@@ -50,7 +48,8 @@ class RosbridgeWsClient
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();
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,8 +117,7 @@ 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();
......@@ -127,7 +128,8 @@ public:
//! \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 = "");
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);
};
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp deleted 100644 → 0
View file @ 67247b3b
#include "polygon_array.h"
std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
return "jsk_recognition_msgs/PolygonArray";
}
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h deleted 100644 → 0
View file @ 67247b3b
#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
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @ 5beee98a
......@@ -5,40 +5,42 @@
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;
};
template <class HeartbeatType>
bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
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()){
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 @ 5beee98a
......@@ -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;
}
......
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
View file @ 5beee98a
#include "tile.h"
tile::tile()
{
std::string ros_bridge::messages::nemo_msgs::tile::messageType() {
return "nemo_msgs/Tile";
}
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
View file @ 5beee98a
......@@ -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
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