new rosbridge impl added, not tested

qgroundcontrol.pro

@@ -267,7 +267,8 @@ QT += \
     widgets \
     xml \
     texttospeech \
-    core-private
+    core-private \
+    websockets
 
 # Multimedia only used if QVC is enabled
 !contains (DEFINES, QGC_DISABLE_UVC) {
@@ -420,6 +421,7 @@ INCLUDEPATH += \
     src/Audio \
     src/comm \
     src/MeasurementComplexItem \
+    src/MeasurementComplexItem/rosbridge \
     src/MeasurementComplexItem/geometry \
     src/MeasurementComplexItem/nemo_interface \
     src/comm/ros_bridge \
@@ -454,6 +456,8 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/TileDiff.h \
     src/MeasurementComplexItem/geometry/geometry.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/FutureWatcherInterface.h \
     src/MeasurementComplexItem/nemo_interface/Task.h \
@@ -508,18 +512,13 @@ HEADERS += \
     src/GPS/Drivers/src/base_station.h \
     src/Settings/WimaSettings.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/messages/geographic_msgs/geopoint.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
     src/comm/ros_bridge/include/messages/std_msgs/header.h \
-    src/comm/ros_bridge/include/server.h \
     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
 
 contains (DEFINES, QGC_ENABLE_PAIRING) {
@@ -534,6 +533,8 @@ SOURCES += \
     src/MeasurementComplexItem/geometry/SafeArea.cc \
     src/MeasurementComplexItem/geometry/geometry.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/MeasurementTile.cpp \
     src/MeasurementComplexItem/nemo_interface/Task.cpp \
@@ -554,8 +555,6 @@ SOURCES += \
     src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
     src/MeasurementComplexItem/NemoInterface.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/nemo_msgs/heartbeat.cpp \
     src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
@@ -563,11 +562,7 @@ SOURCES += \
     src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
-    src/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/comm/ros_bridge/src/ros_bridge.cpp
 
 contains (DEFINES, QGC_ENABLE_PAIRING) {
     SOURCES += \

src/MeasurementComplexItem/rosbridge/rosbridge.cpp

+#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.";
+  }
+}

src/MeasurementComplexItem/rosbridge/rosbridge.h

+#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

src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h

+#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