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 ServiceRequestFrom
 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-request-from', default='service_introspection',
+            choices=['service_introspection', 'client_introspection'],
+            help='Determine the source of the service request to be replayed. '
+                 'By default, the service request is 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,10 @@ 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_request_from or args.service_request_from == 'service':
+            play_options.service_request_from = ServiceRequestFrom.SERVICE_INTROSPECTION
+        else:
+            play_options.service_request_from = ServiceRequestFrom.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,
+        ServiceRequestFrom,
         Recorder,
         RecordOptions,
         bag_rewrite,
@@ -94,6 +95,7 @@
     'Info',
     'Player',
     'PlayOptions',
+    'ServiceRequestFrom',
     '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_request_from", &PlayOptions::service_request_from)
+  ;
+
+  py::enum_<rosbag2_transport::ServiceRequestFrom>(m, "ServiceRequestFrom")
+  .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::SERVICE_INTROSPECTION)
+  .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::CLIENT_INTROSPECTION)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -28,6 +28,11 @@
 
 namespace rosbag2_transport
 {
+enum class ServiceRequestFrom : int
+{
+  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
+  ServiceRequestFrom service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION;
 };
 
 }  // namespace rosbag2_transport

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,44 @@ 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);
+
   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);
 };
 

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_request_from =
+    node.declare_parameter<std::string>("play.service_request_from", "SERVICE_INTROSPECTION");
+  if (service_request_from == "SERVICE_INTROSPECTION") {
+    play_options.service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION;
+  } else if (service_request_from == "CLIENT_INTROSPECTION") {
+    play_options.service_request_from = ServiceRequestFrom::CLIENT_INTROSPECTION;
+  } else {
+    RCLCPP_ERROR(
+      node.get_logger(),
+      "play.service_request_from doesn't support %s. It must be one of SERVICE_INTROSPECTION"
+      " and CLIENT_INTROSPECTION. Change it by default value SERVICE_INTROSPECTION.",
+      service_request_from.c_str());
+  }
+
   return play_options;
 }
 

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -21,7 +21,6 @@
 #include <string>
 #include <unordered_map>
 #include <utility>
-#include <variant>
 #include <vector>
 #include <thread>
 
@@ -36,7 +35,6 @@
 #include "rosbag2_cpp/clocks/time_controller_clock.hpp"
 #include "rosbag2_cpp/reader.hpp"
 #include "rosbag2_cpp/service_utils.hpp"
-#include "rosbag2_cpp/typesupport_helpers.hpp"
 
 #include "rosbag2_storage/storage_filter.hpp"
 #include "rosbag2_storage/qos.hpp"
@@ -1170,13 +1168,11 @@ void PlayerImpl::run_play_msg_post_callbacks(
 
 bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message)
 {
-  bool message_published = false;
-
   auto pub_iter = publishers_.find(message->topic_name);
   if (pub_iter != publishers_.end()) {
     // Calling on play message pre-callbacks
     run_play_msg_pre_callbacks(message);
-
+    bool message_published = false;
     try {
       pub_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data));
       message_published = true;
@@ -1191,17 +1187,73 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr
     return message_published;
   }
 
+  // Try to publish message as service request
   auto client_iter = service_clients_.find(message->topic_name);
   if (client_iter != service_clients_.end()) {
-    if (!client_iter->second->is_include_request_message(*message->serialized_data)) {
-      return message_published;
+    const auto & service_client = client_iter->second;
+    // TODO(morlov):
+    //  Wrap deserialize_service_event and get_service_event_type_and_client_gid in try-catch
+    auto service_event = service_client->deserialize_service_event(*message->serialized_data);
+    if (!service_event) {
+      RCLCPP_ERROR_STREAM(
+        owner_->get_logger(), "Failed to deserialize service event message for '" <<
+          service_client->get_service_name() << "' service!\n");
+      return false;
+    }
+
+    auto [service_event_type, client_gid] =
+      service_client->get_service_event_type_and_client_gid(service_event);
+    // Ignore response message
+    if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT ||
+      service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED)
+    {
+      // TODO(morlov): Shall we ignore REQUEST_RECEIVED as well?
+      return false;
+    }
+
+    if (play_options_.service_request_from == ServiceRequestFrom::SERVICE_INTROSPECTION &&
+      service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED)
+    {
+      return false;
+    }
+
+    if (play_options_.service_request_from == ServiceRequestFrom::CLIENT_INTROSPECTION &&
+      service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT)
+    {
+      return false;
+    }
+
+    if (!service_client->generic_client()->service_is_ready()) {
+      RCLCPP_ERROR(
+        owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !",
+        service_client->get_service_name().c_str());
+      return false;
+    }
+
+    if (!service_client->is_service_event_include_request_message(service_event)) {
+      if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) {
+        RCUTILS_LOG_WARN_ONCE_NAMED(
+          ROSBAG2_TRANSPORT_PACKAGE_NAME,
+          "Can't send service request. "
+          "The configuration of introspection for '%s' was metadata only on service side!",
+          service_client->get_service_name().c_str());
+      } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) {
+        RCUTILS_LOG_WARN_ONCE_NAMED(
+          ROSBAG2_TRANSPORT_PACKAGE_NAME,
+          "Can't send service request. "
+          "The configuration of introspection for '%s' client [ID: %s]` was metadata only!",
+          service_client->get_service_name().c_str(),
+          rosbag2_cpp::client_id_to_string(client_gid).c_str());
+      }
+      return false;
     }
 
     // Calling on play message pre-callbacks
     run_play_msg_pre_callbacks(message);
 
+    bool message_published = false;
     try {
-      client_iter->second->async_send_request(*message->serialized_data);
+      client_iter->second->async_send_request(service_event);
       message_published = true;
     } catch (const std::exception & e) {
       RCLCPP_ERROR_STREAM(
@@ -1215,9 +1267,11 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr
     return message_published;
   }
 
-  RCLCPP_WARN_STREAM(
-    owner_->get_logger(), "Not find sender for topic '" << message->topic_name << "' topic.");
-  return message_published;
+  RCUTILS_LOG_WARN_ONCE_NAMED(
+    ROSBAG2_TRANSPORT_PACKAGE_NAME,
+    "Publisher for topic '%s' not found", message->topic_name.c_str());
+
+  return false;
 }
 
 void PlayerImpl::add_key_callback(