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,15 @@ 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(
+            '--publish-service-requests', action='store_true', default=False,
+            help='Publish recorded service requests instead of recorded service events')
+        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. This option only '
+                 'makes sense if the "--publish-service-requests" option is set. 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 +229,12 @@ 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
+        play_options.publish_service_requests = args.publish_service_requests
+        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/include/rosbag2_cpp/service_utils.hpp

@@ -26,7 +26,7 @@ namespace rosbag2_cpp
 {
 ROSBAG2_CPP_PUBLIC
 bool
-is_service_event_topic(const std::string & topic, const std::string & topic_type);
+is_service_event_topic(const std::string & topic_name, const std::string & topic_type);
 
 // Call this function after is_service_event_topic() return true
 ROSBAG2_CPP_PUBLIC
@@ -38,18 +38,10 @@ ROSBAG2_CPP_PUBLIC
 std::string
 service_event_topic_type_to_service_type(const std::string & topic_type);
 
-ROSBAG2_CPP_PUBLIC
-size_t
-get_serialization_size_for_service_metadata_event();
-
 ROSBAG2_CPP_PUBLIC
 std::string
 service_name_to_service_event_topic_name(const std::string & service_name);
 
-ROSBAG2_CPP_PUBLIC
-bool
-service_event_include_metadata_and_contents(size_t message_size);
-
 ROSBAG2_CPP_PUBLIC
 std::string
 client_id_to_string(std::array<uint8_t, 16> & client_id);

rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp

@@ -17,153 +17,101 @@
 #include "rcl/service_introspection.h"
 
 #include "rosbag2_cpp/service_utils.hpp"
-#include "rosidl_typesupport_cpp/message_type_support.hpp"
-#include "rosidl_typesupport_cpp/message_type_support_dispatch.hpp"
-#include "rosidl_typesupport_introspection_cpp/identifier.hpp"
-#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
-
 #include "service_msgs/msg/service_event_info.hpp"
 
 namespace rosbag2_cpp
 {
-const char * service_event_topic_type_postfix = "_Event";
-const char * service_event_topic_type_middle = "/srv/";
+const char * kServiceEventTopicTypePostfix = "_Event";
+const char * kServiceEventTopicTypeMiddle = "/srv/";
+const size_t kServiceEventTopicPostfixLen = strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX);
+const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix);
 
-bool is_service_event_topic(const std::string & topic, const std::string & topic_type)
+bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type)
 {
-  if (topic.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) {
+  if (topic_name.length() <= kServiceEventTopicPostfixLen) {
     return false;
+  } else {
+    // The end of the topic name should be "/_service_event"
+    if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) !=
+      RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)
+    {
+      return false;
+    }
   }
 
-  std::string end_topic_name = topic.substr(
-    topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX));
-
-  // Should be "/_service_event"
-  if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) {
+  if (topic_type.length() <= kServiceEventTypePostfixLen) {
     return false;
-  }
+  } else {
+    // Should include '/srv/' in type
+    if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) {
+      return false;
+    }
 
-  if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) {
-    return false;
-  }
-
-  // Should include '/srv/' in type
-  if (topic_type.find(service_event_topic_type_middle) == std::string::npos) {
-    return false;
-  }
+    if (topic_type.length() <= kServiceEventTypePostfixLen) {
+      return false;
+    }
 
-  if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) {
-    return false;
+    return topic_type.compare(
+      topic_type.length() - kServiceEventTypePostfixLen,
+      kServiceEventTypePostfixLen,
+      kServiceEventTopicTypePostfix) == 0;
   }
-
-  return topic_type.compare(
-    topic_type.length() - std::strlen(service_event_topic_type_postfix),
-    std::strlen(service_event_topic_type_postfix),
-    service_event_topic_type_postfix) == 0;
 }
 
 std::string service_event_topic_name_to_service_name(const std::string & topic_name)
 {
   std::string service_name;
-  if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) {
+  if (topic_name.length() <= kServiceEventTopicPostfixLen) {
     return service_name;
-  }
-
-  if (topic_name.substr(
-      topic_name.length() -
-      strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) !=
-    RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)
-  {
+  } else {
+    // The end of the topic name should be "/_service_event"
+    if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) !=
+      RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)
+    {
+      return service_name;
+    }
+    service_name = topic_name.substr(0, topic_name.length() - kServiceEventTopicPostfixLen);
     return service_name;
   }
-
-  service_name = topic_name.substr(
-    0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX));
-
-  return service_name;
 }
 
 std::string service_event_topic_type_to_service_type(const std::string & topic_type)
 {
   std::string service_type;
-  if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) {
+  if (topic_type.length() <= kServiceEventTypePostfixLen) {
     return service_type;
   }
-
   // Should include '/srv/' in type
-  if (topic_type.find(service_event_topic_type_middle) == std::string::npos) {
+  if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) {
     return service_type;
   }
 
-  if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) !=
-    service_event_topic_type_postfix)
+  if (topic_type.substr(topic_type.length() - kServiceEventTypePostfixLen) !=
+    kServiceEventTopicTypePostfix)
   {
     return service_type;
   }
-
-  service_type = topic_type.substr(
-    0, topic_type.length() - strlen(service_event_topic_type_postfix));
+  service_type = topic_type.substr(0, topic_type.length() - kServiceEventTypePostfixLen);
 
   return service_type;
 }
 
-size_t get_serialization_size_for_service_metadata_event()
-{
-  // Since the size is fixed, it only needs to be calculated once.
-  static size_t size = 0;
-
-  if (size != 0) {
-    return size;
-  }
-
-  const rosidl_message_type_support_t * type_support_info =
-    rosidl_typesupport_cpp::get_message_type_support_handle<service_msgs::msg::ServiceEventInfo>();
-
-  // Get the serialized size of service event info
-  const rosidl_message_type_support_t * type_support_handle =
-    rosidl_typesupport_cpp::get_message_typesupport_handle_function(
-    type_support_info,
-    rosidl_typesupport_introspection_cpp::typesupport_identifier);
-  if (type_support_handle == nullptr) {
-    throw std::runtime_error("Cannot get ServiceEventInfo typesupport handle !");
-  }
-
-  auto service_event_info =
-    static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
-    type_support_handle->data);
-
-  // 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;
-
-  return size;
-}
-
 std::string service_name_to_service_event_topic_name(const std::string & service_name)
 {
   if (service_name.empty()) {
     return service_name;
   }
 
   // If the end of string is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX, do nothing
-  if ((service_name.length() > strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) &&
-    (service_name.substr(service_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) ==
+  if ((service_name.length() > kServiceEventTopicPostfixLen) &&
+    (service_name.substr(service_name.length() - kServiceEventTopicPostfixLen) ==
     RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX))
   {
     return service_name;
   }
-
   return service_name + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
 }
 
-bool service_event_include_metadata_and_contents(size_t message_size)
-{
-  if (message_size <= rosbag2_cpp::get_serialization_size_for_service_metadata_event()) {
-    return false;
-  }
-  return true;
-}
-
 std::string client_id_to_string(std::array<uint8_t, 16> & client_id)
 {
   // output format:

rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp

@@ -97,21 +97,6 @@ TEST_F(ServiceUtilsTest, check_service_name_to_service_event_topic_name)
   }
 }
 
-TEST_F(ServiceUtilsTest, check_service_event_include_metadata_and_contents)
-{
-  auto msg = std::make_shared<test_msgs::srv::BasicTypes_Event>();
-  msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT;
-  auto serialized_service_event = memory_management_.serialize_message(msg);
-
-  size_t metadata_event_size = rosbag2_cpp::get_serialization_size_for_service_metadata_event();
-
-  EXPECT_EQ(serialized_service_event->buffer_length, metadata_event_size);
-
-  EXPECT_FALSE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size - 1));
-  EXPECT_FALSE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size));
-  EXPECT_TRUE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size + 1));
-}
-
 TEST_F(ServiceUtilsTest, check_client_id_to_string)
 {
   service_msgs::msg::ServiceEventInfo::_client_gid_type client_id = {

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,13 @@ 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("publish_service_requests", &PlayOptions::publish_service_requests)
+  .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,12 @@ struct PlayOptions
 
   // Disable to publish as loaned message
   bool disable_loan_message = false;
+
+  // Publish service requests instead of service events
+  bool publish_service_requests = 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,19 @@ 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.
+  /// \note The player node shall be spun in the executor in a parallel thread to be able to wait
+  /// for responses.
+  /// \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
@@ -263,7 +276,7 @@ class Player : public rclcpp::Node
   /// \brief Getter for clients corresponding to each service name
   /// \return Hashtable representing service name to client
   ROSBAG2_TRANSPORT_PUBLIC
-  std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericClient>> get_services_clients();
+  std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericClient>> get_service_clients();
 
   /// \brief Getter for inner clock_publisher
   /// \return Shared pointer to the inner clock_publisher