Added ImageTransportManager which handles the image_transport subscri…

…ptions to ensure that work is not performed multiple times if multiple ImageTransportSubscribers subscribe to the same camera.

Added throttleRate to enable throttling of camera messages.

CMakeLists.txt

@@ -47,6 +47,7 @@ set(SOURCES
   include/qml_ros_plugin/console.h
   include/qml_ros_plugin/goal_handle.h
   include/qml_ros_plugin/image_buffer.h
+  include/qml_ros_plugin/image_transport_manager.h
   include/qml_ros_plugin/image_transport_subscriber.h
   include/qml_ros_plugin/io.h
   include/qml_ros_plugin/message_conversions.h
@@ -68,6 +69,7 @@ set(SOURCES
   src/console.cpp
   src/goal_handle.cpp
   src/image_buffer.cpp
+  src/image_transport_manager.cpp
   src/image_transport_subscriber.cpp
   src/io.cpp
   src/message_conversions.cpp

README.md

@@ -70,6 +70,10 @@ The stream is exposed to QML as a `QObject` with a `QAbstractVideoSurface` based
 (see [QML VideoOutput docs](https://doc.qt.io/qt-5/qml-qtmultimedia-videooutput.html#source-prop)) and can be used
 directly as source for the `VideoOutput` control.
 
+**New:** Multiple ImageTransportSubscribers for the same topic now share a subscription to ensure the image is converted
+to a QML compatible format only once. Additionally, a throttleRate property was introduced to throttle the camera rate by
+subscribing for one frame and shutting down again at the given rate (see documentation).
+
 Usage example:
 ```qml
 import QtMultimedia 5.4
@@ -82,6 +86,7 @@ Item {
   ImageTransportSubscriber {
     id: imageSubscriber
     topic: "/front_rgb_cam"
+    throttleRate: 0.2 // 1 frame every 5 seconds
   }
 
   VideoOutput {

docs/pages/ImageTransport.rst

@@ -0,0 +1,49 @@
+==============
+ImageTransport
+==============
+
+Seeing what the robot sees is one of the most important features of any user interface.
+To enable this, this library provides the :cpp:class:`ImageTransportSubscriber <qml_ros_plugin::ImageTransportSubscriber>`.
+It allows easy subscription of camera messages and provides them in a QML native format as a VideoSource.
+
+Example:
+
+.. code-block:: qml
+
+  ImageTransportSubscriber {
+    id: imageSubscriber
+    // Enter a valid image topic here
+    topic: "/front_rgbd_cam/color/image_rect_color"
+    // This is the default transport, change if compressed is not available
+    defaultTransport: "compressed"
+  }
+
+  VideoOutput {
+    anchors.fill: parent
+    // Can be used in increments of 90 to rotate the video
+    orientation: 90
+    source: imageSubscriber
+  }
+
+The :cpp:class:`ImageTransportSubscriber <qml_ros_plugin::ImageTransportSubscriber>` can be used as the source of a
+``VideoOutput`` to display the camera images as they are received.
+Additionally, it can be configured to show a blank image after x milliseconds using the ``timeout`` property which is set
+to 3000ms (3s) by default. This can be disabled by setting the ``timeout`` to 0.
+If you do not want the full camera rate, you can throttle the rate by setting ``throttleRate`` to a value greater than 0
+(which is the default and disables throttling). E.g. a rate of 0.2 would show a new frame every 5 seconds.
+Since there is no ROS functionality for a throttled subscription, this means the ``image_transport::Subscriber`` is shut
+down and newly subscribed for each frame. This comes at some overhead, hence, it should only be used to throttle to low
+rates <1.
+To avoid all throttled subscribers subscribing at the same time causing huge network spikes, the throttled rates are
+load balanced by default. This can be disabled globally using
+:cpp:func:`ImageTransportManager::setLoadBalancingEnabled <qml_ros_plugin::ImageTransportManagerSingletonWrapper::setLoadBalancingEnabled>`
+which is available in QML using the singleton ``ImageTransportManager``.
+
+API
+---
+
+.. doxygenclass:: qml_ros_plugin::ImageTransportSubscriber
+   :members:
+
+.. doxygenclass:: qml_ros_plugin::ImageTransportManagerSingletonWrapper
+   :members:

docs/pages/Logging.rst

@@ -18,13 +18,13 @@ To log a message you can use one of the following methods ``debug``, ``info``, `
 
 This will produce the following output:
 
-::
+:: code-block:: bash
 
   [ INFO] [1583062360.048922959]: Button clicked.
 
 and publish the following on ``/rosout`` (unless ``NoRosout`` was specified in the ``RosInitOptions``).
 
-::
+:: code-block:: bash
 
   header:
     seq: 1

include/qml_ros_plugin/image_transport_manager.h

@@ -0,0 +1,100 @@
+//
+// Created by stefan on 22.02.21.
+//
+
+#ifndef QML_ROS_PLUGIN_IMAGE_TRANSPORT_MANAGER_H
+#define QML_ROS_PLUGIN_IMAGE_TRANSPORT_MANAGER_H
+
+#include "qml_ros_plugin/node_handle.h"
+
+#include <image_transport/image_transport.h>
+#include <QAbstractVideoSurface>
+
+namespace qml_ros_plugin
+{
+class ImageTransportSubscriptionHandle;
+
+class ImageTransportManagerSingletonWrapper : public QObject
+{
+Q_OBJECT
+public:
+  //! @copydoc ImageTransportManager::setLoadBalancingEnabled
+  Q_INVOKABLE void setLoadBalancingEnabled( bool value );
+};
+
+/*!
+ * Encapsulates the image transport communication to share the subscription resources, avoiding multiple conversions of
+ *  the same image and subscription overhead if multiple cameras are set to throttle.
+ */
+class ImageTransportManager
+{
+  ImageTransportManager();
+
+  struct SubscriptionManager;
+  struct Subscription;
+public:
+
+  static ImageTransportManager &getInstance();
+
+  //! Sets whether the manager should try to balance throttled subscriptions_ to ensure they don't update at the same
+  //!   time which would result in network spikes.
+  void setLoadBalancingEnabled( bool value );
+
+  /*!
+   * Note: Can only be called with a ready NodeHandle!
+   *
+   * Subscribes to the given topic with the given settings. Makes sure that multiple subscriptions_ (especially throttled)
+   *  only result in a single (throttled) subscription.
+   * If multiple subscriptions_ of the same topic and namespace are created, the settings of the first subscription are used.
+   * Except for the throttle interval where the minimum value across all active subscriptions_ is used.
+   * @param nh The NodeHandle the subscription is made from if it is the first subscribe call
+   * @param qtopic
+   * @param queue_size
+   * @param transport_hints
+   * @param callback
+   * @param surface
+   * @param throttle_interval
+   * @return
+   */
+  std::shared_ptr<ImageTransportSubscriptionHandle>
+  subscribe( const NodeHandle::Ptr &nh, const QString &qtopic, quint32 queue_size,
+             const image_transport::TransportHints &transport_hints,
+             const std::function<void( const QVideoFrame & )> &callback, QAbstractVideoSurface *surface = nullptr,
+             int throttle_interval = 0 );
+
+private:
+  int getLoadBalancedTimeout( int desired_throttle_interval );
+
+  std::map<std::string, std::shared_ptr<SubscriptionManager>> subscriptions_;
+  std::vector<long> timeouts_;
+  bool load_balancing_enabled_ = true;
+
+  friend class ImageTransportSubscriptionHandle;
+};
+
+class ImageTransportSubscriptionHandle
+{
+public:
+
+  ~ImageTransportSubscriptionHandle();
+
+  //! The interval the subscription waits between receiving images.
+  int throttleInterval() const { return throttle_interval; }
+
+  //! Set the interval the subscription may wait between images.
+  //! The images may still arrive at a higher rate if other subscriptions_ request it.
+  void updateThrottleInterval( int interval );
+
+  std::string getTopic();
+
+private:
+  int throttle_interval = 0;
+  std::shared_ptr<ImageTransportManager::Subscription> subscription;
+  QAbstractVideoSurface *surface = nullptr;
+  std::function<void( const QVideoFrame & )> callback;
+
+  friend class ImageTransportManager;
+};
+}
+
+#endif //QML_ROS_PLUGIN_IMAGE_TRANSPORT_MANAGER_H

include/qml_ros_plugin/image_transport_subscriber.h

@@ -18,12 +18,12 @@
 namespace qml_ros_plugin
 {
 
-class ImageBuffer;
+class ImageTransportSubscriptionHandle;
 
 class ImageTransportSubscriber : public QObjectRos
 {
 Q_OBJECT
-  //! @formatter:off
+  // @formatter:off
   //! Interface for QML. This is the surface the images are passed to.
   Q_PROPERTY(QAbstractVideoSurface *videoSurface READ videoSurface WRITE setVideoSurface)
   //! The image base topic (without image_raw etc.). This value may change once the subscriber is connected and private
@@ -36,7 +36,10 @@ Q_OBJECT
   //! The timeout when no image is received until a blank frame is served. Set to 0 to disable and always show last frame.
   //! Default is 3000 ms.
   Q_PROPERTY(int timeout READ timeout WRITE setTimeout NOTIFY timeoutChanged)
-  //! @formatter:on
+  //! The update rate to throttle image receiving in images per second. Set to 0 to disable throttling.
+  //! Default is 0 (disabled).
+  Q_PROPERTY(double throttleRate READ throttleRate WRITE setThrottleRate NOTIFY throttleRateChanged)
+  // @formatter:on
 public:
 
   ImageTransportSubscriber( NodeHandle::Ptr nh, QString topic, quint32 queue_size );
@@ -61,6 +64,10 @@ Q_OBJECT
 
   void setTimeout( int value );
 
+  double throttleRate() const;
+
+  void setThrottleRate( double value );
+
 signals:
 
   void topicChanged();
@@ -71,35 +78,33 @@ Q_OBJECT
 
   void timeoutChanged();
 
+  void throttleRateChanged();
+
 private slots:
 
   void onNodeHandleReady();
 
-private:
-  void imageCallback( const sensor_msgs::ImageConstPtr &img );
+  void onNoImageTimeout();
 
-  Q_INVOKABLE void processImage();
+  void presentFrame( const QVideoFrame &frame );
 
+private:
   void onRosShutdown() override;
 
-  void subscribe();
+  void initSubscriber();
 
-  void unsubscribe();
+  void shutdownSubscriber();
 
   QTimer no_image_timer_;
   QVideoSurfaceFormat format_;
   QString topic_;
   QString default_transport_;
-  std::shared_ptr<ros::CallbackQueue> background_queue_;
   NodeHandle::Ptr nh_;
-  image_transport::Subscriber subscriber_;
-  std::mutex image_lock_;
-  sensor_msgs::ImageConstPtr last_image_ = nullptr;
-  ImageBuffer *buffer_ = nullptr;
-  std::unique_ptr<image_transport::ImageTransport> transport_;
+  std::shared_ptr<ImageTransportSubscriptionHandle> subscription_;
   QAbstractVideoSurface *surface_ = nullptr;
   ros::Time last_frame_timestamp_;
   quint32 queue_size_;
+  int throttle_interval_ = 0;
   int timeout_ = 3000;
   bool subscribed_ = false;
 };