Commit 9e620065 authored by Valentin Platzgummer's avatar Valentin Platzgummer

new rosbridge impl added, not tested

parent b94dfebd
...@@ -267,7 +267,8 @@ QT += \ ...@@ -267,7 +267,8 @@ QT += \
widgets \ widgets \
xml \ xml \
texttospeech \ texttospeech \
core-private core-private \
websockets
# Multimedia only used if QVC is enabled # Multimedia only used if QVC is enabled
!contains (DEFINES, QGC_DISABLE_UVC) { !contains (DEFINES, QGC_DISABLE_UVC) {
...@@ -420,6 +421,7 @@ INCLUDEPATH += \ ...@@ -420,6 +421,7 @@ INCLUDEPATH += \
src/Audio \ src/Audio \
src/comm \ src/comm \
src/MeasurementComplexItem \ src/MeasurementComplexItem \
src/MeasurementComplexItem/rosbridge \
src/MeasurementComplexItem/geometry \ src/MeasurementComplexItem/geometry \
src/MeasurementComplexItem/nemo_interface \ src/MeasurementComplexItem/nemo_interface \
src/comm/ros_bridge \ src/comm/ros_bridge \
...@@ -454,6 +456,8 @@ HEADERS += \ ...@@ -454,6 +456,8 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \ src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \ src/MeasurementComplexItem/HashFunctions.h \
src/MeasurementComplexItem/rosbridge/rosbridge.h \
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \ src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \ src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \
src/MeasurementComplexItem/nemo_interface/Task.h \ src/MeasurementComplexItem/nemo_interface/Task.h \
...@@ -508,18 +512,13 @@ HEADERS += \ ...@@ -508,18 +512,13 @@ HEADERS += \
src/GPS/Drivers/src/base_station.h \ src/GPS/Drivers/src/base_station.h \
src/Settings/WimaSettings.h \ src/Settings/WimaSettings.h \
src/comm/QmlObjectListHelper.h \ src/comm/QmlObjectListHelper.h \
src/comm/ros_bridge/include/RosBridgeClient.h \
src/comm/ros_bridge/include/com_private.h \
src/comm/ros_bridge/include/message_traits.h \ src/comm/ros_bridge/include/message_traits.h \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \ src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \ src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \ src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
src/comm/ros_bridge/include/messages/std_msgs/header.h \ src/comm/ros_bridge/include/messages/std_msgs/header.h \
src/comm/ros_bridge/include/server.h \
src/comm/ros_bridge/include/messages/std_msgs/time.h \ src/comm/ros_bridge/include/messages/std_msgs/time.h \
src/comm/ros_bridge/include/topic_publisher.h \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h src/comm/utilities.h
contains (DEFINES, QGC_ENABLE_PAIRING) { contains (DEFINES, QGC_ENABLE_PAIRING) {
...@@ -534,6 +533,8 @@ SOURCES += \ ...@@ -534,6 +533,8 @@ SOURCES += \
src/MeasurementComplexItem/geometry/SafeArea.cc \ src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \ src/MeasurementComplexItem/geometry/geometry.cpp \
src/MeasurementComplexItem/HashFunctions.cpp \ src/MeasurementComplexItem/HashFunctions.cpp \
src/MeasurementComplexItem/rosbridge/rosbridge.cpp \
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \ src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
src/MeasurementComplexItem/nemo_interface/Task.cpp \ src/MeasurementComplexItem/nemo_interface/Task.cpp \
...@@ -554,8 +555,6 @@ SOURCES += \ ...@@ -554,8 +555,6 @@ SOURCES += \
src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \ src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
src/MeasurementComplexItem/NemoInterface.cpp \ src/MeasurementComplexItem/NemoInterface.cpp \
src/comm/QmlObjectListHelper.cpp \ src/comm/QmlObjectListHelper.cpp \
src/comm/ros_bridge/include/RosBridgeClient.cpp \
src/comm/ros_bridge/include/com_private.cpp \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \ src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.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/labeled_progress.cpp \
...@@ -563,11 +562,7 @@ SOURCES += \ ...@@ -563,11 +562,7 @@ SOURCES += \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
src/comm/ros_bridge/include/messages/std_msgs/header.cpp \ src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \ src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
src/comm/ros_bridge/include/server.cpp \
src/comm/ros_bridge/include/topic_publisher.cpp \
src/comm/ros_bridge/include/topic_subscriber.cpp \
src/Settings/WimaSettings.cc \ src/Settings/WimaSettings.cc \
src/comm/ros_bridge/src/ros_bridge.cpp
contains (DEFINES, QGC_ENABLE_PAIRING) { contains (DEFINES, QGC_ENABLE_PAIRING) {
SOURCES += \ SOURCES += \
......
#include "rosbridge.h"
#include "rosbridgeimpl.h"
#include <QJsonArray>
#include <QJsonDocument>
#include <QJsonObject>
#include <QThread>
Q_DECLARE_METATYPE(Rosbridge::CallBack)
Q_DECLARE_METATYPE(Rosbridge::CallBackWReturn)
static const char *topicsKey = "topics";
static const char *servicesKey = "services";
static const char *rosServices = "/rosapi/services";
static const char *rosTopics = "/rosapi/topics";
static constexpr auto pollIntervall = std::chrono::milliseconds(200);
Rosbridge::Rosbridge(const QUrl url, QObject *parent)
: QObject(parent), _url(url), _running(false) {
static std::once_flag flag;
std::call_once(flag, [] {
qRegisterMetaType<Rosbridge::CallBack>();
qRegisterMetaType<Rosbridge::CallBackWReturn>();
});
}
Rosbridge::~Rosbridge() { stop(); }
void Rosbridge::start() {
if (!_running) {
_running = true;
_t = new QThread();
_impl = new RosbridgeImpl(_url);
_impl->moveToThread(_t);
connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start);
connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop);
connect(this, &Rosbridge::_advertiseTopic, _impl,
&RosbridgeImpl::advertiseTopic);
connect(this, &Rosbridge::_publish, _impl, &RosbridgeImpl::publish);
connect(this, &Rosbridge::_unadvertiseTopic, _impl,
&RosbridgeImpl::unadvertiseTopic);
connect(this, &Rosbridge::_unadvertiseAllTopics, _impl,
&RosbridgeImpl::unadvertiseAllTopics);
connect(this, &Rosbridge::_subscribeTopic, _impl,
&RosbridgeImpl::subscribeTopic);
connect(this, &Rosbridge::_unsubscribeTopic, _impl,
&RosbridgeImpl::unsubscribeTopic);
connect(this, &Rosbridge::_unsubscribeAllTopics, _impl,
&RosbridgeImpl::unsubscribeAllTopics);
connect(this, &Rosbridge::_callService, _impl, &RosbridgeImpl::callService);
connect(this, &Rosbridge::_advertiseService, _impl,
&RosbridgeImpl::advertiseService);
connect(this, &Rosbridge::_unadvertiseService, _impl,
&RosbridgeImpl::unadvertiseService);
connect(this, &Rosbridge::_unadvertiseAllServices, _impl,
&RosbridgeImpl::unadvertiseAllServices);
_t->start();
emit _start();
}
}
void Rosbridge::stop() {
if (_running) {
_running = false;
_impl->deleteLater();
_t->quit();
_t->wait();
_t->deleteLater();
}
}
Rosbridge::STATE Rosbridge::state() {
if (_running) {
switch (_impl->state()) {
case RosbridgeImpl::STATE::STOPPED:
case RosbridgeImpl::STATE::STOPPING:
return STATE::STOPPED;
case RosbridgeImpl::STATE::STARTING:
return STATE::STARTED;
case RosbridgeImpl::STATE::CONNECTED:
return STATE::CONNECTED;
case RosbridgeImpl::STATE::TIMEOUT:
return STATE::TIMEOUT;
break;
}
qCritical() << "unreachable branch!";
return STATE::STOPPED;
} else {
return STATE::STOPPED;
}
}
void Rosbridge::waitConnected() {
while (this->state() != Rosbridge::STATE::CONNECTED) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
void Rosbridge::advertiseTopic(const QString &topic, const QString &type) {
emit _advertiseTopic(topic, type);
}
void Rosbridge::publish(const QString &topic, const QJsonObject &msg) {
emit _publish(topic, msg);
}
void Rosbridge::unadvertiseTopic(const QString &topic) {
emit _unadvertiseTopic(topic);
}
void Rosbridge::unadvertiseAllTopics() { emit _unadvertiseAllTopics(); }
void Rosbridge::subscribeTopic(const QString &topic,
Rosbridge::CallBack callback) {
emit _subscribeTopic(topic, callback);
}
std::future<bool> Rosbridge::topicAvailable(const QString &topic) {
auto pPromise = std::make_shared<std::promise<bool>>();
auto future = pPromise->get_future();
if (this->state() == STATE::CONNECTED) {
auto setBool = [pPromise,
topic](const QJsonObject &topics) mutable -> void {
if (topics.contains(topicsKey) && topics[topicsKey].isArray()) {
QJsonValue v(topic);
pPromise->set_value(topics[topicsKey].toArray().contains(v));
} else {
pPromise->set_value(false);
}
};
emit this->_callService(rosTopics, setBool, QJsonObject());
} else {
pPromise->set_value(false);
}
return future;
}
std::future<QJsonObject> Rosbridge::getAdvertisedTopics() {
auto pPromise = std::make_shared<std::promise<QJsonObject>>();
auto future = pPromise->get_future();
if (this->state() == STATE::CONNECTED) {
auto setString = [pPromise](const QJsonObject &topics) mutable {
pPromise->set_value(topics);
};
emit this->_callService(rosTopics, setString, QJsonObject());
} else {
pPromise->set_value(QJsonObject());
}
return future;
}
void Rosbridge::unsubscribeTopic(const QString &topic) {
emit _unsubscribeTopic(topic);
}
void Rosbridge::unsubscribeAllTopics() { emit _unsubscribeAllTopics(); }
void Rosbridge::waitForTopic(const QString &topic) {
if (this->state() == STATE::CONNECTED) {
std::future<bool> future;
while (true) {
future = topicAvailable(topic);
auto available = future.get();
if (available) {
break;
}
std::this_thread::sleep_for(pollIntervall);
if (this->state() != STATE::CONNECTED) {
qDebug() << "waitForTopic: Rosbridge not connected.";
break;
}
}
} else {
qDebug() << "waitForTopic: Rosbridge not connected.";
}
}
void Rosbridge::advertiseService(const QString &service, const QString &type,
CallBackWReturn callback) {
emit _advertiseService(service, type, callback);
}
void Rosbridge::callService(const QString &service,
Rosbridge::CallBack callback,
const QJsonObject &args) {
emit _callService(service, callback, args);
}
std::future<bool> Rosbridge::serviceAvailable(const QString &service) {
auto pPromise = std::make_shared<std::promise<bool>>();
auto future = pPromise->get_future();
if (this->state() == STATE::CONNECTED) {
auto setBool = [pPromise,
service](const QJsonObject &services) mutable -> void {
if (services.contains(servicesKey) && services[servicesKey].isArray()) {
QJsonValue v(service);
pPromise->set_value(services[servicesKey].toArray().contains(v));
} else {
pPromise->set_value(false);
}
};
emit this->_callService(rosServices, setBool, QJsonObject());
} else {
pPromise->set_value(false);
}
return future;
}
std::future<QJsonObject> Rosbridge::getAdvertisedServices() {
auto pPromise = std::make_shared<std::promise<QJsonObject>>();
auto future = pPromise->get_future();
if (this->state() == STATE::CONNECTED) {
auto setString = [pPromise](const QJsonObject &services) mutable {
pPromise->set_value(services);
};
emit this->_callService(rosServices, setString, QJsonObject());
} else {
pPromise->set_value(QJsonObject());
}
return future;
}
void Rosbridge::unadvertiseService(const QString &service) {
emit _unadvertiseService(service);
}
void Rosbridge::unadvertiseAllServices() { emit _unadvertiseAllServices(); }
void Rosbridge::waitForService(const QString &service) {
if (this->state() == STATE::CONNECTED) {
std::future<bool> future;
while (true) {
future = serviceAvailable(service);
auto available = future.get();
if (available) {
break;
}
std::this_thread::sleep_for(pollIntervall);
if (this->state() != STATE::CONNECTED) {
qDebug() << "waitForService: Rosbridge not connected.";
break;
}
}
} else {
qDebug() << "waitForService: Rosbridge not connected.";
}
}
#ifndef ROSBRIDGE_H
#define ROSBRIDGE_H
#include <QJsonObject>
#include <QObject>
#include <QtWebSockets/QWebSocket>
#include <deque>
#include <functional>
#include <future>
#include <map>
#include <memory>
class RosbridgeImpl;
class QThread;
class Rosbridge : public QObject {
Q_OBJECT
public:
enum class STATE {
STOPPED,
STARTED,
CONNECTED,
TIMEOUT,
};
typedef std::function<void(const QJsonObject &)> CallBack;
typedef std::function<QJsonObject(const QJsonObject &)> CallBackWReturn;
Rosbridge(QUrl _url, QObject *parent = nullptr);
~Rosbridge();
void start();
void stop();
STATE state();
//!
//! \brief wait Waits for connection.
//! Waits for connection. This function can be called after start() to wait
//! for the state() to change to STATE::CONNECTED.
//!
void waitConnected();
// Topics.
void advertiseTopic(const QString &topic, const QString &type);
void publish(const QString &topic, const QJsonObject &msg);
void unadvertiseTopic(const QString &topic);
void unadvertiseAllTopics();
void subscribeTopic(const QString &topic, Rosbridge::CallBack callback);
std::future<bool> topicAvailable(const QString &topic);
std::future<QJsonObject> getAdvertisedTopics();
void unsubscribeTopic(const QString &topic);
void unsubscribeAllTopics();
void waitForTopic(const QString &topic);
// Services.
void advertiseService(const QString &service, const QString &type,
Rosbridge::CallBackWReturn callback);
void unadvertiseService(const QString &service);
void unadvertiseAllServices();
void callService(const QString &service, Rosbridge::CallBack callback,
const QJsonObject &args = QJsonObject());
std::future<bool> serviceAvailable(const QString &service);
std::future<QJsonObject> getAdvertisedServices();
void waitForService(const QString &service);
signals:
void stateChanged();
private:
signals:
void _start();
void _stop();
// Topics
void _advertiseTopic(const QString &topic, const QString &type);
void _publish(const QString &topic, const QJsonObject &msg);
void _unadvertiseTopic(const QString &topic);
void _unadvertiseAllTopics();
void _subscribeTopic(const QString &topic, Rosbridge::CallBack callback);
void _unsubscribeTopic(const QString &topic);
void _unsubscribeAllTopics();
// Services.
void _advertiseService(const QString &service, const QString &type,
Rosbridge::CallBackWReturn callback);
void _callService(const QString &service, Rosbridge::CallBack callback,
const QJsonObject &args = QJsonObject());
void _unadvertiseService(const QString &service);
void _unadvertiseAllServices();
private:
RosbridgeImpl *_impl;
QThread *_t;
QUrl _url;
std::atomic_bool _running;
};
#endif // ROSBRIDGE_H
This diff is collapsed.
#ifndef ROSBRIDGEIMPL_H
#define ROSBRIDGEIMPL_H
#include "rosbridge.h"
#include <QObject>
#include <QtWebSockets/QWebSocket>
#include <deque>
#include <memory>
#include <set>
struct ServiceCall;
class RosbridgeImpl : public QObject {
Q_OBJECT
public:
enum class STATE { STOPPED, STOPPING, STARTING, CONNECTED, TIMEOUT };
RosbridgeImpl(const QUrl &url, QObject *parent = nullptr);
~RosbridgeImpl();
STATE state(); // thread safe
public slots:
void start();
void stop();
// Topics.
void advertiseTopic(const QString &topic, const QString &type);
void publish(const QString &topic, const QJsonObject &msg);
void unadvertiseTopic(const QString &topic);
void unadvertiseAllTopics();
void subscribeTopic(const QString &topic,
const Rosbridge::CallBack &callback);
void unsubscribeTopic(const QString &topic);
void unsubscribeAllTopics();
// Services.
void advertiseService(const QString &service, const QString &type,
const Rosbridge::CallBackWReturn &callback);
void callService(const QString &service, const Rosbridge::CallBack &callback,
const QJsonObject &req = QJsonObject());
void unadvertiseService(const QString &service);
void unadvertiseAllServices();
signals:
void stateChanged();
private slots:
void _onConnected();
void _onDisconnected();
void _doAction();
void _onTextMessageReceived(const QString &message);
private:
void _processTopic(const QJsonObject &o);
void _processServiceResponse(const QJsonObject &o);
void _processServiceCall(const QJsonObject &o);
void _setState(STATE newState);
int _getMessageId();
void _serviceResponse(const QString &service, const QString &id, bool result,
const QJsonObject &values);
void _clearAllPendingServiceCalls();
QWebSocket _webSocket;
QUrl _url;
std::set<QString> _advertisedTopics;
std::map<QString, Rosbridge::CallBack> _subscribedTopics;
std::map<QString, Rosbridge::CallBackWReturn> _advertisedServices;
std::map<QString, std::deque<std::unique_ptr<ServiceCall>>>
_pendingServiceCalls;
std::atomic<STATE> _state;
};
#endif // ROSBRIDGEIMPL_H
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