Fixes for services playback per review 2

ros2bag/ros2bag/verb/play.py

@@ -25,6 +25,7 @@
 from ros2cli.node import NODE_NAME_PREFIX
 from rosbag2_py import Player
 from rosbag2_py import PlayOptions
+from rosbag2_py import ServiceRequestsSource
 from rosbag2_py import StorageOptions
 import yaml
 
@@ -152,6 +153,11 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
                  'By default, if loaned message can be used, messages are published as loaned '
                  'message. It can help to reduce the number of data copies, so there is a greater '
                  'benefit for sending big data.')
+        parser.add_argument(
+            '--service-requests-source', default='service_introspection',
+            choices=['service_introspection', 'client_introspection'],
+            help='Determine the source of the service requests to be replayed. By default, '
+                 'the service requests replaying from recorded service introspection message.')
 
     def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int:
         nano_scale = 1000 * 1000 * 1000
@@ -219,6 +225,11 @@ def main(self, *, args):  # noqa: D102
         play_options.start_offset = args.start_offset
         play_options.wait_acked_timeout = args.wait_for_all_acked
         play_options.disable_loan_message = args.disable_loan_message
+        if not args.service_requests_source or \
+                args.service_requests_source == 'service_introspection':
+            play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION
+        else:
+            play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION
 
         player = Player()
         try:

rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp

@@ -132,6 +132,8 @@ size_t get_serialization_size_for_service_metadata_event()
     static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
     type_support_handle->data);
 
+  // TODO(morlov): We shall not rely on this arithmetic!!! It is up to the serialization
+  //  implementation
   // endian type (4 size) + service event info size + empty request (4 bytes)
   // + emtpy response (4 bytes)
   size = 4 + service_event_info->size_of_ + 4 + 4;

rosbag2_py/rosbag2_py/__init__.py

@@ -56,6 +56,7 @@
     from rosbag2_py._transport import (
         Player,
         PlayOptions,
+        ServiceRequestsSource,
         Recorder,
         RecordOptions,
         bag_rewrite,
@@ -94,6 +95,7 @@
     'Info',
     'Player',
     'PlayOptions',
+    'ServiceRequestsSource',
     'Recorder',
     'RecordOptions',
 ]

rosbag2_py/src/rosbag2_py/_transport.cpp

@@ -366,6 +366,12 @@ PYBIND11_MODULE(_transport, m) {
     &PlayOptions::setPlaybackUntilTimestamp)
   .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout)
   .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message)
+  .def_readwrite("service_requests_source", &PlayOptions::service_requests_source)
+  ;
+
+  py::enum_<rosbag2_transport::ServiceRequestsSource>(m, "ServiceRequestsSource")
+  .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::SERVICE_INTROSPECTION)
+  .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -28,6 +28,11 @@
 
 namespace rosbag2_transport
 {
+enum class ServiceRequestsSource : int8_t
+{
+  SERVICE_INTROSPECTION = 0,
+  CLIENT_INTROSPECTION = 1
+};
 
 struct PlayOptions
 {
@@ -112,6 +117,9 @@ struct PlayOptions
 
   // Disable to publish as loaned message
   bool disable_loan_message = false;
+
+  // The source of the service request
+  ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION;
 };
 
 }  // namespace rosbag2_transport

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

@@ -19,7 +19,6 @@
 #include <map>
 #include <memory>
 #include <mutex>
-#include <unordered_map>
 #include <utility>
 #include <string>
 #include <tuple>
@@ -38,6 +37,9 @@ class PlayerServiceClientManager;
 class PlayerServiceClient final
 {
 public:
+  using ServiceEventType = service_msgs::msg::ServiceEventInfo::_event_type_type;
+  using ClientGidType = service_msgs::msg::ServiceEventInfo::_client_gid_type;
+
   explicit
   PlayerServiceClient(
     std::shared_ptr<rclcpp::GenericClient> generic_client,
@@ -46,40 +48,51 @@ class PlayerServiceClient final
     rclcpp::Logger logger,
     std::shared_ptr<PlayerServiceClientManager> player_service_client_manager);
 
-  // Note: Call this function only if is_include_request_message() return true
+  const std::string & get_service_name();
+
+  /// \brief Deserialize message to the type erased service event
+  /// \param message - Serialized message
+  /// \return Shared pointer to the byte array with deserialized service event if success,
+  /// otherwise nullptr
+  std::shared_ptr<uint8_t[]> deserialize_service_event(const rcl_serialized_message_t & message);
+
+  std::tuple<PlayerServiceClient::ServiceEventType, PlayerServiceClient::ClientGidType>
+  get_service_event_type_and_client_gid(const std::shared_ptr<uint8_t[]> type_erased_service_event);
+
+  bool is_service_event_include_request_message(
+    const std::shared_ptr<uint8_t[]> type_erased_service_event);
+
+  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()
   {
     return client_;
   }
 
-  // Check if message can be unpacked to get request message
-  bool is_include_request_message(const rcl_serialized_message_t & message);
-
 private:
   std::shared_ptr<rclcpp::GenericClient> client_;
   std::string service_name_;
   const rclcpp::Logger logger_;
   std::shared_ptr<PlayerServiceClientManager> player_service_client_manager_;
-  enum class request_info_from
-  {
-    SERVICE = 0,
-    CLIENT,
-    NO_CONTENT  // Only have META info. Not send request.
-  };
-  bool service_set_introspection_content_ = false;
-
-  using client_id = service_msgs::msg::ServiceEventInfo::_client_gid_type;
-  // Info on request data from service or client
-  std::unordered_map<client_id, request_info_from, rosbag2_cpp::client_id_hash> request_info_;
+  // Note: The service_event_ts_lib_ shall be a member variable to make sure that library loaded
+  // during the liveliness of the instance of this class, since we have raw pointers to its members.
+  std::shared_ptr<rcpputils::SharedLibrary> service_event_ts_lib_;
 
   const rosidl_message_type_support_t * service_event_type_ts_;
   const rosidl_typesupport_introspection_cpp::MessageMembers * service_event_members_;
 
   rcutils_allocator_t allocator_ = rcutils_get_default_allocator();
 
-  std::tuple<uint8_t, client_id, int64_t>
+  std::tuple<uint8_t, ClientGidType, int64_t>
   get_msg_event_type(const rcl_serialized_message_t & message);
 };
 
@@ -97,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/config_options_from_node_params.cpp

@@ -218,6 +218,21 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node)
   play_options.disable_loan_message =
     node.declare_parameter<bool>("play.disable_loan_message", false);
 
+  // "SERVICE_INTROSPECTION" or "CLIENT_INTROSPECTION"
+  auto service_requests_source =
+    node.declare_parameter<std::string>("play.service_requests_source", "SERVICE_INTROSPECTION");
+  if (service_requests_source == "SERVICE_INTROSPECTION") {
+    play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION;
+  } else if (service_requests_source == "CLIENT_INTROSPECTION") {
+    play_options.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION;
+  } else {
+    RCLCPP_ERROR(
+      node.get_logger(),
+      "play.service_requests_source doesn't support %s. It must be one of SERVICE_INTROSPECTION"
+      " and CLIENT_INTROSPECTION. Change it by default value SERVICE_INTROSPECTION.",
+      service_requests_source.c_str());
+  }
+
   return play_options;
 }