Skip to content

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

temp

parent 67247b3b
Hide 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"
......@@ -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;
};