Fixed a weird threading bug with the wait mechanism in ServiceClient when the Service was not ready.

Added pendingRequests property.

Author: StefanFabian

include/qml_ros2_plugin/service_client.hpp

@@ -27,6 +27,11 @@ class ServiceClient : public QObjectRos2
   //! Connection timeout in ms to wait for the service to become available when sending a request.
   Q_PROPERTY( int connectionTimeout READ connectionTimeout WRITE setConnectionTimeout NOTIFY
                   connectionTimeoutChanged )
+  //! How many service requests are currently pending and waiting for the response.
+  Q_PROPERTY( int pendingRequests READ pendingRequests NOTIFY pendingRequestsChanged )
+
+  using clock = std::chrono::steady_clock;
+
 public:
   /*!
    * @param name The service topic.
@@ -48,6 +53,8 @@ class ServiceClient : public QObjectRos2
 
   void setConnectionTimeout( int timeout );
 
+  int pendingRequests() const;
+
   /*!
    * Calls a service asynchronously returning immediately.
    * Once the service call finishes, the optional callback is called with the result if provided.
@@ -64,6 +71,8 @@ class ServiceClient : public QObjectRos2
 
   void connectionTimeoutChanged();
 
+  void pendingRequestsChanged();
+
 protected:
   void onRos2Initialized() override;
 
@@ -82,9 +91,15 @@ private slots:
   QString service_type_;
   ros_babel_fish::BabelFishServiceClient::SharedPtr client_;
   QTimer connect_timer_;
-  std::vector<std::thread> waiting_threads_;
-  std::atomic<bool> stop_{ false };
-  int connection_timeout_ = 10'000; // Default connection timeout in ms
+  struct WaitingServiceCallData {
+    QVariantMap request;
+    QJSValue callback;
+    clock::time_point start;
+  };
+  // Queue of service calls waiting for the service to become available
+  std::vector<WaitingServiceCallData> waiting_service_calls_;
+  int pending_requests_ = 0;
+  int connection_timeout_ = 10'000; // Default connection timeout for waiting service calls in ms
 };
 } // namespace qml_ros2_plugin
 

src/service_client.cpp

@@ -21,15 +21,7 @@ ServiceClient::ServiceClient( QString name, QString type, const QoSWrapper &qos
   babel_fish_ = BabelFishDispenser::getBabelFish();
 }
 
-ServiceClient::~ServiceClient()
-{
-  stop_ = true;
-  for ( auto &thread : waiting_threads_ ) {
-    if ( thread.joinable() )
-      thread.join();
-  }
-  waiting_threads_.clear();
-}
+ServiceClient::~ServiceClient() = default;
 
 void ServiceClient::onRos2Initialized()
 {
@@ -49,23 +41,43 @@ void ServiceClient::onRos2Initialized()
   connect_timer_.start();
 }
 
-void ServiceClient::onRos2Shutdown()
-{
-  stop_ = true;
-  for ( auto &thread : waiting_threads_ ) {
-    if ( thread.joinable() )
-      thread.join();
-  }
-  client_.reset();
-}
+void ServiceClient::onRos2Shutdown() { client_.reset(); }
 
 void ServiceClient::checkServiceReady()
 {
-  if ( !isServiceReady() )
+  if ( !isServiceReady() ) {
+    int timeouted = 0;
+    for ( size_t i = 0; i < waiting_service_calls_.size(); ++i, ++timeouted ) {
+      if ( clock::now() - waiting_service_calls_[i].start <
+           std::chrono::milliseconds( connection_timeout_ ) )
+        break;
+      QML_ROS2_PLUGIN_DEBUG(
+          "Request for service '%s' timeouted while waiting for it to become ready.",
+          name_.toStdString().c_str() );
+      invokeCallback( waiting_service_calls_[i].callback, QVariant( false ) );
+      pending_requests_--;
+    }
+    if ( timeouted > 0 ) {
+      waiting_service_calls_.erase( waiting_service_calls_.begin(),
+                                    waiting_service_calls_.begin() + timeouted );
+      emit pendingRequestsChanged();
+    }
     return;
+  }
   connect_timer_.stop();
   disconnect( &connect_timer_, &QTimer::timeout, this, &ServiceClient::checkServiceReady );
   emit serviceReadyChanged();
+
+  if ( waiting_service_calls_.empty() )
+    return;
+
+  blockSignals( true );
+  for ( const auto &call : waiting_service_calls_ ) {
+    pending_requests_--;
+    sendRequestAsync( call.request, call.callback );
+  }
+  blockSignals( false );
+  emit pendingRequestsChanged();
 }
 
 bool ServiceClient::isServiceReady() const
@@ -87,32 +99,15 @@ void ServiceClient::setConnectionTimeout( int timeout )
   emit connectionTimeoutChanged();
 }
 
+int ServiceClient::pendingRequests() const { return pending_requests_; }
+
 void ServiceClient::sendRequestAsync( const QVariantMap &req, const QJSValue &callback )
 {
-  using clock = std::chrono::steady_clock;
+  pending_requests_++;
   if ( client_ == nullptr || !client_->service_is_ready() ) {
     QML_ROS2_PLUGIN_DEBUG( "Service '%s' not ready, waiting up to %d ms.",
                            name_.toStdString().c_str(), connection_timeout_ );
-    waiting_threads_.emplace_back( [this, req, callback] {
-      auto now = clock::now();
-      while ( !isServiceReady() ) {
-        if ( stop_ || clock::now() - now > std::chrono::milliseconds( connection_timeout_ ) ) {
-          QML_ROS2_PLUGIN_DEBUG( "Service '%s' timeouted or client was destroyed while waiting for "
-                                 "it to become ready.",
-                                 name_.toStdString().c_str() );
-          QMetaObject::invokeMethod( this, "invokeCallback", Qt::AutoConnection,
-                                     Q_ARG( QJSValue, callback ),
-                                     Q_ARG( QVariant, QVariant( false ) ) );
-          return;
-        }
-        std::this_thread::sleep_for( std::chrono::milliseconds( 5 ) );
-      }
-      QML_ROS2_PLUGIN_DEBUG(
-          "Service '%s' is ready after waiting %ld ms. Sending goal.", name_.toStdString().c_str(),
-          std::chrono::duration_cast<std::chrono::milliseconds>( clock::now() - now ).count() );
-      QMetaObject::invokeMethod( this, "sendRequestAsync", Qt::AutoConnection,
-                                 Q_ARG( QVariantMap, req ), Q_ARG( QJSValue, callback ) );
-    } );
+    waiting_service_calls_.push_back( { req, callback, clock::now() } );
     return;
   }
   QML_ROS2_PLUGIN_DEBUG( "Service '%s' is ready. Sending request.", name_.toStdString().c_str() );
@@ -123,10 +118,13 @@ void ServiceClient::sendRequestAsync( const QVariantMap &req, const QJSValue &ca
     client_->async_send_request( message, [callback,
                                            this]( BabelFishServiceClient::SharedFuture response ) {
       QML_ROS2_PLUGIN_DEBUG( "Received response from service %s.", name_.toStdString().c_str() );
+      pending_requests_--;
+      emit pendingRequestsChanged();
       QMetaObject::invokeMethod( this, "invokeCallback", Qt::AutoConnection,
                                  Q_ARG( QJSValue, callback ),
                                  Q_ARG( QVariant, msgToMap( response.get() ) ) );
     } );
+    emit pendingRequestsChanged();
     return;
   } catch ( BabelFishException &ex ) {
     QML_ROS2_PLUGIN_ERROR( "Failed to call service: %s", ex.what() );
@@ -135,6 +133,7 @@ void ServiceClient::sendRequestAsync( const QVariantMap &req, const QJSValue &ca
   } catch ( ... ) {
     QML_ROS2_PLUGIN_ERROR( "Failed to call service: Unknown error." );
   }
+  pending_requests_--;
   QMetaObject::invokeMethod( this, "invokeCallback", Qt::AutoConnection,
                              Q_ARG( QJSValue, callback ), Q_ARG( QVariant, QVariant( false ) ) );
 }