Added rate throttling for TfTransform component. Default is 60Hz.

CMakeLists.txt

@@ -96,14 +96,15 @@ if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   find_package(ros_babel_fish_test_msgs REQUIRED)
   find_package(std_srvs REQUIRED)
-  include_directories(${ros_babel_fish_test_msgs_INCLUDE_DIRS} ${std_srvs_INCLUDE_DIRS})
+  find_package(Qt5Test REQUIRED)
+  include_directories(${ros_babel_fish_test_msgs_INCLUDE_DIRS} ${std_srvs_INCLUDE_DIRS} ${Qt5Test_INCLUDE_DIRS})
 
   add_executable(${PROJECT_NAME}_test_action_server test/action_server.cpp)
   target_link_libraries(${PROJECT_NAME}_test_action_server ${PROJECT_NAME})
   set_target_properties(${PROJECT_NAME}_test_action_server PROPERTIES OUTPUT_NAME test_action_server PREFIX "")
 
   add_rostest_gtest(${PROJECT_NAME}_test_communication test/test_communication.test test/communication.cpp)
-  target_link_libraries(${PROJECT_NAME}_test_communication ${PROJECT_NAME})
+  target_link_libraries(${PROJECT_NAME}_test_communication ${PROJECT_NAME} Qt5::Test)
   set_target_properties(${PROJECT_NAME}_test_communication PROPERTIES OUTPUT_NAME test_communication PREFIX "")
 
   add_rostest_gtest(${PROJECT_NAME}_test_image_conversions test/test_image_conversions.test test/image_conversions.cpp)

include/qml_ros_plugin/tf_transform.h

@@ -5,6 +5,7 @@
 #define QML_ROS_PLUGIN_TF_TRANSFORM_H
 
 #include <QObject>
+#include <QTimer>
 #include <QVariantMap>
 #include <memory>
 
@@ -31,11 +32,15 @@ Q_OBJECT
   //! The target frame of the tf transform, i.e., the frame to which the data should be transformed.
   Q_PROPERTY( QString targetFrame READ targetFrame WRITE setTargetFrame NOTIFY targetFrameChanged )
 
-  //! Whether this tf transform is active, i.e., receiving transform updates.
-  Q_PROPERTY( bool active READ active WRITE setActive NOTIFY activeChanged )
+  //! Whether this tf transform is enabled, i.e., receiving transform updates.
+  Q_PROPERTY( bool enabled READ enabled WRITE setEnabled NOTIFY enabledChanged )
+  //! Alias for enabled
+  Q_PROPERTY( bool active READ enabled WRITE setEnabled NOTIFY enabledChanged )
 
   //! The last received transform as a geometry_msgs/TransformStamped with an added boolean valid field and optional
   //! error fields. See TfTransformListener::lookUpTransform
+  Q_PROPERTY( QVariantMap transform READ message NOTIFY messageChanged )
+  //! An alias for transform.
   Q_PROPERTY( QVariantMap message READ message NOTIFY messageChanged )
 
   //! The translation part of the tf transform as a vector with x, y, z fields. Zero if no valid transform available (yet).
@@ -44,6 +49,10 @@ Q_OBJECT
   //! The rotation part of the tf transform as a quaternion with w, x, y, z fields. Identity if no valid transform available (yet).
   Q_PROPERTY( QVariant rotation READ rotation NOTIFY rotationChanged )
 
+  //! The maximum rate in Hz at which tf updates are processed and emitted as changed signals. Default: 60
+  //! Note: The rate can not exceed 1000. To disable rate limiting set to 0.
+  Q_PROPERTY( qreal rate READ rate WRITE setRate NOTIFY rateChanged )
+
   //! Whether the current transform, i.e., the fields message, translation and rotation are valid.
   Q_PROPERTY( bool valid READ valid NOTIFY validChanged )
   // @formatter:on
@@ -60,25 +69,31 @@ Q_OBJECT
 
   void setTargetFrame( const QString &targetFrame );
 
-  bool active() const;
+  bool enabled() const;
+
+  void setEnabled( bool value );
 
-  void setActive( bool value );
+  qreal rate() const;
 
-  const QVariantMap &message() const;
+  void setRate( qreal value );
 
-  const QVariant &translation() const;
+  const QVariantMap &message();
 
-  const QVariant &rotation() const;
+  const QVariant &translation();
 
-  bool valid() const;
+  const QVariant &rotation();
+
+  bool valid();
 
 signals:
 
   void sourceFrameChanged();
 
   void targetFrameChanged();
 
-  void activeChanged();
+  void enabledChanged();
+
+  void rateChanged();
 
   void messageChanged();
 
@@ -92,15 +107,20 @@ protected slots:
 
   void onTransformChanged();
 
+  void updateMessage();
+
 protected:
   void subscribe();
 
   void shutdown();
 
-  bool active_;
+  QTimer throttle_timer_;
+  QVariantMap message_;
   QString source_frame_;
   QString target_frame_;
-  QVariantMap message_;
+  std::chrono::system_clock::time_point last_transform_;
+  std::chrono::milliseconds throttle_time_;
+  bool enabled_;
 };
 } // qml_ros_plugin
 

include/qml_ros_plugin/tf_transform_listener.h

@@ -87,7 +87,7 @@ Q_OBJECT
 
 signals:
 
-  //! Emitted whenever a new transform arrived.
+  //! Emitted whenever a new transform arrived. Warning this signal is not throttled!
   void transformChanged();
 
 protected:

src/subscriber.cpp

@@ -102,33 +102,6 @@ void Subscriber::shutdown()
   is_subscribed_ = false;
 }
 
-//namespace
-//{
-//
-//// For debugging
-//void dumpMap( const QVariant &node, const QString &prefix = "" )
-//{
-//  if ( node.typeName() == QString( "QVariantMap" ))
-//  {
-//    const QVariantMap &map = *reinterpret_cast<const QVariantMap *>(node.data());
-//    for ( auto &key : map.keys())
-//    {
-//      std::cout << (prefix + key).toStdString() << ": " << map[key].typeName() << std::endl;
-//      dumpMap( map[key], prefix + "-" );
-//    }
-//  }
-//  else if ( node.typeName() == QString( "QVariantList" ))
-//  {
-//    const QVariantList &list = node.value<QVariantList>();
-//    for ( int i = 0; i < list.size(); ++i )
-//    {
-//      std::cout << prefix.toStdString() << " [" << i << "]:" << std::endl;
-//      dumpMap( list[i], prefix + "-" );
-//    }
-//  }
-//}
-//}
-
 void Subscriber::messageCallback( const ros_babel_fish::BabelFishMessage::ConstPtr &msg )
 {
   TranslatedMessage::Ptr translated = babel_fish_.translateMessage( msg );

src/tf_transform.cpp

@@ -12,11 +12,13 @@ using namespace qml_ros_plugin::conversion;
 namespace qml_ros_plugin
 {
 
-TfTransform::TfTransform() : active_( true )
+TfTransform::TfTransform() : throttle_time_( 1000 / 60 ), enabled_( true )
 {
   geometry_msgs::TransformStamped transform;
   message_ = msgToMap( transform );
   message_.insert( "valid", false );
+  connect( &throttle_timer_, &QTimer::timeout, this, &TfTransform::updateMessage );
+  throttle_timer_.setSingleShot( true );
 }
 
 TfTransform::~TfTransform()
@@ -31,6 +33,7 @@ void TfTransform::setSourceFrame( const QString &value )
   source_frame_ = value;
   if ( source_frame_.isEmpty()) shutdown();
   else subscribe();
+
   emit sourceFrameChanged();
 }
 
@@ -41,63 +44,93 @@ void TfTransform::setTargetFrame( const QString &targetFrame )
   target_frame_ = targetFrame;
   if ( target_frame_.isEmpty()) shutdown();
   else subscribe();
+
   emit targetFrameChanged();
 }
 
-bool TfTransform::active() const { return active_; }
+bool TfTransform::enabled() const { return enabled_; }
 
-void TfTransform::setActive( bool value )
+void TfTransform::setEnabled( bool value )
 {
-  if ( active_ == value ) return;
-  active_ = value;
-  if ( active_ ) subscribe();
+  if ( enabled_ == value ) return;
+  enabled_ = value;
+  if ( enabled_ ) subscribe();
   else shutdown();
-  emit activeChanged();
+
+  emit enabledChanged();
 }
 
-const QVariantMap &TfTransform::message() const { return message_; }
+qreal TfTransform::rate() const
+{
+  if ( throttle_time_.count() == 0 ) return 0;
+  return 1000.0 / throttle_time_.count();
+}
 
-const QVariant &TfTransform::translation() const
+void TfTransform::setRate( qreal value )
+{
+  if ( value <= 0 ) throttle_time_ = std::chrono::milliseconds::zero();
+  throttle_time_ = std::chrono::milliseconds( int( 1000 / value )); // Rounding down as value is strictly positive
+
+  emit rateChanged();
+}
+
+const QVariantMap &TfTransform::message() { return message_; }
+
+const QVariant &TfTransform::translation()
 {
   const QVariantMap &transform = *static_cast<const QVariantMap *>(message_["transform"].data());
   return transform.find( "translation" ).value();
 }
 
-const QVariant &TfTransform::rotation() const
+const QVariant &TfTransform::rotation()
 {
   const QVariantMap &transform = *static_cast<const QVariantMap *>(message_["transform"].data());
   return transform.find( "rotation" ).value();
 }
 
-bool TfTransform::valid() const
+bool TfTransform::valid()
 {
   return message_.contains( "valid" ) && message_["valid"].toBool();
 }
 
 void TfTransform::onTransformChanged()
 {
-  bool wasValid = valid();
-  message_ = TfTransformListener::getInstance().lookUpTransform( target_frame_, source_frame_ );
-  if ( valid() != wasValid )
-    emit validChanged();
-  emit rotationChanged();
-  emit messageChanged();
-  emit translationChanged();
+  if ( !enabled_ ) return;
+  std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
+  if ( !throttle_timer_.isActive())
+  {
+    std::chrono::milliseconds delta_t = std::chrono::duration_cast<std::chrono::milliseconds>( now - last_transform_ );
+    if ( delta_t >= throttle_time_ )
+      updateMessage();
+    else
+      throttle_timer_.start( throttle_time_ - delta_t );
+  }
+  last_transform_ = now;
 }
 
 void TfTransform::subscribe()
 {
-  if ( source_frame_.isEmpty() || target_frame_.isEmpty() || !active_ ) return;
+  if ( source_frame_.isEmpty() || target_frame_.isEmpty() || !enabled_ ) return;
 
   QObject::connect( &TfTransformListener::getInstance(), &TfTransformListener::transformChanged,
                     this, &TfTransform::onTransformChanged );
   // Load transform
-  onTransformChanged();
+  if ( valid()) onTransformChanged();
 }
 
 void TfTransform::shutdown()
 {
   QObject::disconnect( &TfTransformListener::getInstance(), &TfTransformListener::transformChanged,
                        this, &TfTransform::onTransformChanged );
 }
+
+void TfTransform::updateMessage()
+{
+  bool was_valid = valid();
+  message_ = TfTransformListener::getInstance().lookUpTransform( target_frame_, source_frame_ );
+  if ( valid() != was_valid ) emit validChanged();
+  emit rotationChanged();
+  emit messageChanged();
+  emit translationChanged();
+}
 }