Fixes for services playback per review 2

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -254,6 +254,17 @@ class Player : public rclcpp::Node
   ROSBAG2_TRANSPORT_PUBLIC
   void delete_on_play_message_callback(const callback_handle_t & handle);
 
+  /// Wait until sent service requests will receive responses from service servers.
+  /// \param service_name - Name of the service or service event from what to wait responses.
+  /// \note is service_name is empty the function will wait until all service requests sent to all
+  /// service servers will finish. Timeout in this cases will be used for each service name.
+  /// \param timeout - Timeout in fraction of seconds to wait for.
+  /// \return true if service requests successfully finished, otherwise false.
+  ROSBAG2_TRANSPORT_PUBLIC
+  bool wait_for_sent_service_requests_to_finish(
+    const std::string & service_name,
+    std::chrono::duration<double> timeout = std::chrono::seconds(5));
+
 protected:
   /// \brief Getter for publishers corresponding to each topic
   /// \return Hashtable representing topic to publisher map excluding inner clock_publisher

rosbag2_transport/include/rosbag2_transport/player_service_client.hpp

@@ -64,6 +64,13 @@ class PlayerServiceClient final
 
   void async_send_request(const std::shared_ptr<uint8_t[]> type_erased_service_event);
 
+  /// Wait until sent service requests will receive responses from service servers.
+  /// \param service_name - Name of the service or service event from what to wait responses.
+  /// \param timeout - Timeout in fraction of seconds to wait for.
+  /// \return true if service requests successfully finished, otherwise false.
+  bool wait_for_sent_requests_to_finish(
+    std::chrono::duration<double> timeout = std::chrono::seconds(5));
+
   void async_send_request(const rcl_serialized_message_t & message);
 
   std::shared_ptr<rclcpp::GenericClient> generic_client()
@@ -103,6 +110,8 @@ class PlayerServiceClientManager final
     rclcpp::GenericClient::FutureAndRequestId & request_future,
     std::weak_ptr<rclcpp::GenericClient> client);
 
+  bool wait_for_all_futures(std::chrono::duration<double> timeout = std::chrono::seconds(5));
+
 private:
   using time_point = std::chrono::steady_clock::time_point;
   using ptr_future_and_request_id = std::unique_ptr<rclcpp::GenericClient::FutureAndRequestId>;

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -171,6 +171,14 @@ class PlayerImpl
   /// #add_on_play_message_post_callback
   void delete_on_play_message_callback(const callback_handle_t & handle);
 
+  /// TBD
+  /// \param service_name
+  /// \param timeout
+  /// \return
+  bool wait_for_sent_service_requests_to_finish(
+    const std::string & service_name,
+    std::chrono::duration<double> timeout = std::chrono::seconds(5));
+
   /// \brief Getter for publishers corresponding to each topic
   /// \return Hashtable representing topic to publisher map excluding inner clock_publisher
   std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> get_publishers();
@@ -787,6 +795,29 @@ void PlayerImpl::delete_on_play_message_callback(const callback_handle_t & handl
     });
 }
 
+bool PlayerImpl::wait_for_sent_service_requests_to_finish(
+  const std::string & service_name,
+  std::chrono::duration<double> timeout)
+{
+  bool is_requests_complete = true;
+  if (!service_name.empty()) {
+    auto service_event_name = rosbag2_cpp::service_name_to_service_event_topic_name(service_name);
+    auto client_iter = service_clients_.find(service_event_name);
+    if (client_iter != service_clients_.end()) {
+      is_requests_complete = client_iter->second->wait_for_sent_requests_to_finish(timeout);
+    } else {
+      is_requests_complete = false;
+    }
+  } else {
+    for (const auto & [service_event_name, client] : service_clients_) {
+      if (!client->wait_for_sent_requests_to_finish(timeout)) {
+        return false;
+      }
+    }
+  }
+  return is_requests_complete;
+}
+
 std::unordered_map<std::string,
   std::shared_ptr<rclcpp::GenericPublisher>> PlayerImpl::get_publishers()
 {
@@ -1630,6 +1661,12 @@ void Player::delete_on_play_message_callback(const Player::callback_handle_t & h
   pimpl_->delete_on_play_message_callback(handle);
 }
 
+bool Player::wait_for_sent_service_requests_to_finish(
+  const std::string & service_name, std::chrono::duration<double> timeout)
+{
+  return pimpl_->wait_for_sent_service_requests_to_finish(service_name, timeout);
+}
+
 std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> Player::get_publishers()
 {
   return pimpl_->get_publishers();

rosbag2_transport/src/rosbag2_transport/player_service_client.cpp

@@ -24,6 +24,8 @@
 
 #include "logging.hpp"
 
+using namespace std::chrono_literals;
+
 namespace rosbag2_transport
 {
 
@@ -168,6 +170,11 @@ void PlayerServiceClient::async_send_request(
   }  // else { /* No service request in the service event. Do nothing, just skip it. */ }
 }
 
+bool PlayerServiceClient::wait_for_sent_requests_to_finish(std::chrono::duration<double> timeout)
+{
+  return player_service_client_manager_->wait_for_all_futures(timeout);
+}
+
 void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message)
 {
   if (!client_->service_is_ready()) {
@@ -258,6 +265,39 @@ bool PlayerServiceClientManager::register_request_future(
   return false;
 }
 
+bool PlayerServiceClientManager::wait_for_all_futures(std::chrono::duration<double> timeout)
+{
+  auto is_all_futures_ready = [&]() {
+      bool is_ready = true;
+      for (auto & [timestamp, future_request_id_and_client] : request_futures_list_) {
+        if (!future_request_id_and_client.first->future.valid()) {
+          std::stringstream ss;
+          ss << "request's " << future_request_id_and_client.first->request_id <<
+            " future is not valid!\n";
+          throw std::runtime_error(ss.str());
+        }
+        if (future_request_id_and_client.first->wait_for(0s) != std::future_status::ready) {
+          return false;
+        }
+      }
+      return is_ready;
+    };
+
+  auto sleep_time = std::chrono::milliseconds(10);
+  if (timeout < std::chrono::seconds(1)) {
+    sleep_time = std::chrono::duration_cast<std::chrono::milliseconds>(timeout);
+  }
+  using clock = std::chrono::system_clock;
+  auto start = clock::now();
+
+  std::lock_guard<std::mutex> lock(request_futures_list_mutex_);
+  while (!is_all_futures_ready() && (clock::now() - start) < timeout) {
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  return is_all_futures_ready();
+}
+
 void PlayerServiceClientManager::remove_complete_request_future()
 {
   std::vector<time_point> remove_keys;