Support service 2/2 --- rosbag2 service play

ros2bag/ros2bag/verb/burst.py

@@ -18,6 +18,7 @@
 from ros2bag.api import add_standard_reader_args
 from ros2bag.api import check_not_negative_int
 from ros2bag.api import check_positive_float
+from ros2bag.api import convert_service_to_service_event_topic
 from ros2bag.api import convert_yaml_to_qos_profile
 from ros2bag.api import print_error
 from ros2bag.verb import VerbExtension
@@ -41,8 +42,12 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
                  'delay of message playback.')
         parser.add_argument(
             '--topics', type=str, default=[], nargs='+',
-            help='topics to replay, separated by space. If none specified, all topics will be '
-                 'replayed.')
+            help='topics to replay, separated by space. At least one topic needs to be '
+            "specified. If this parameter isn\'t specified, all topics will be replayed.")
+        parser.add_argument(
+            '--services', type=str, default=[], nargs='+',
+            help='services to replay, separated by space. At least one service needs to be '
+                 "specified. If this parameter isn\'t specified, all services will be replayed.")
         parser.add_argument(
             '--qos-profile-overrides-path', type=FileType('r'),
             help='Path to a yaml file defining overrides of the QoS profile for specific topics.')
@@ -90,6 +95,8 @@ def main(self, *, args):  # noqa: D102
         play_options.node_prefix = NODE_NAME_PREFIX
         play_options.rate = 1.0
         play_options.topics_to_filter = args.topics
+        # Convert service name to service event topic name
+        play_options.services_to_filter = convert_service_to_service_event_topic(args.services)
         play_options.topic_qos_profile_overrides = qos_profile_overrides
         play_options.loop = False
         play_options.topic_remapping_options = topic_remapping

ros2bag/ros2bag/verb/play.py

@@ -18,12 +18,14 @@
 from ros2bag.api import add_standard_reader_args
 from ros2bag.api import check_not_negative_int
 from ros2bag.api import check_positive_float
+from ros2bag.api import convert_service_to_service_event_topic
 from ros2bag.api import convert_yaml_to_qos_profile
 from ros2bag.api import print_error
 from ros2bag.verb import VerbExtension
 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
 
@@ -49,16 +51,23 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '-r', '--rate', type=check_positive_float, default=1.0,
             help='rate at which to play back messages. Valid range > 0.0.')
         parser.add_argument(
-            '--topics', type=str, default=[], nargs='+',
+            '--topics', type=str, default=[], metavar='topic', nargs='+',
             help='Space-delimited list of topics to play.')
+        parser.add_argument(
+            '--services', type=str, default=[], metavar='service', nargs='+',
+            help='Space-delimited list of services to play.')
         parser.add_argument(
             '-e', '--regex', default='',
-            help='filter topics by regular expression to replay, separated by space. If none '
-                 'specified, all topics will be replayed.')
+            help='Play only topics and services matches with regular expression.')
+        parser.add_argument(
+            '-x', '--exclude-regex', default='',
+            help='regular expressions to exclude topics and services from replay.')
+        parser.add_argument(
+            '--exclude-topics', type=str, default=[], metavar='topic', nargs='+',
+            help='Space-delimited list of topics not to play.')
         parser.add_argument(
-            '-x', '--exclude', default='',
-            help='regular expressions to exclude topics from replay, separated by space. If none '
-                 'specified, all topics will be replayed.')
+            '--exclude-services', type=str, default=[], metavar='service', nargs='+',
+            help='Space-delimited list of services not to play.')
         parser.add_argument(
             '--qos-profile-overrides-path', type=FileType('r'),
             help='Path to a yaml file defining overrides of the QoS profile for specific topics.')
@@ -144,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
@@ -182,8 +200,19 @@ def main(self, *, args):  # noqa: D102
         play_options.node_prefix = NODE_NAME_PREFIX
         play_options.rate = args.rate
         play_options.topics_to_filter = args.topics
-        play_options.topics_regex_to_filter = args.regex
-        play_options.topics_regex_to_exclude = args.exclude
+
+        # Convert service name to service event topic name
+        play_options.services_to_filter = convert_service_to_service_event_topic(args.services)
+
+        play_options.regex_to_filter = args.regex
+
+        play_options.exclude_regex_to_filter = args.exclude_regex
+
+        play_options.exclude_topics_to_filter = args.exclude_topics if args.exclude_topics else []
+
+        play_options.exclude_service_events_to_filter = \
+            convert_service_to_service_event_topic(args.exclude_services)
+
         play_options.topic_qos_profile_overrides = qos_profile_overrides
         play_options.loop = args.loop
         play_options.topic_remapping_options = topic_remapping
@@ -200,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/CMakeLists.txt

@@ -270,7 +270,11 @@ if(BUILD_TESTING)
   ament_add_gmock(test_service_utils
     test/rosbag2_cpp/test_service_utils.cpp)
   if(TARGET test_service_utils)
-    target_link_libraries(test_service_utils ${PROJECT_NAME})
+    target_link_libraries(test_service_utils
+      ${PROJECT_NAME}
+      rosbag2_test_common::rosbag2_test_common
+      ${test_msgs_TARGETS}
+    )
   endif()
 endif()
 

rosbag2_cpp/include/rosbag2_cpp/logging.hpp

@@ -28,36 +28,36 @@
   RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__)
 
 #define ROSBAG2_CPP_LOG_INFO_STREAM(args) do { \
-  std::stringstream __ss; \
-  __ss << args; \
-  RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
+    std::stringstream __ss; \
+    __ss << args; \
+    RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
 } while (0)
 
 #define ROSBAG2_CPP_LOG_ERROR(...) \
   RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__)
 
 #define ROSBAG2_CPP_LOG_ERROR_STREAM(args) do { \
-  std::stringstream __ss; \
-  __ss << args; \
-  RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
+    std::stringstream __ss; \
+    __ss << args; \
+    RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
 } while (0)
 
 #define ROSBAG2_CPP_LOG_WARN(...) \
   RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__)
 
 #define ROSBAG2_CPP_LOG_WARN_STREAM(args) do { \
-  std::stringstream __ss; \
-  __ss << args; \
-  RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
+    std::stringstream __ss; \
+    __ss << args; \
+    RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
 } while (0)
 
 #define ROSBAG2_CPP_LOG_DEBUG(...) \
   RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__)
 
 #define ROSBAG2_CPP_LOG_DEBUG_STREAM(args) do { \
-  std::stringstream __ss; \
-  __ss << args; \
-  RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
+    std::stringstream __ss; \
+    __ss << args; \
+    RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \
 } while (0)
 
 // *INDENT-ON*

rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp

@@ -15,15 +15,18 @@
 #ifndef ROSBAG2_CPP__SERVICE_UTILS_HPP_
 #define ROSBAG2_CPP__SERVICE_UTILS_HPP_
 
+#include <array>
 #include <string>
 
 #include "rosbag2_cpp/visibility_control.hpp"
 
+#include "service_msgs/msg/service_event_info.hpp"
+
 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
@@ -36,12 +39,20 @@ 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();
+std::string
+service_name_to_service_event_topic_name(const std::string & service_name);
 
 ROSBAG2_CPP_PUBLIC
 std::string
-service_name_to_service_event_topic_name(const std::string & service_name);
+client_id_to_string(std::array<uint8_t, 16> & client_id);
+
+struct client_id_hash
+{
+  static_assert(
+    std::is_same<std::array<uint8_t, 16>,
+    service_msgs::msg::ServiceEventInfo::_client_gid_type>::value);
+  std::size_t operator()(const std::array<uint8_t, 16> & client_id) const;
+};
 }  // namespace rosbag2_cpp
 
 #endif  // ROSBAG2_CPP__SERVICE_UTILS_HPP_

rosbag2_cpp/src/rosbag2_cpp/info.cpp

@@ -60,23 +60,6 @@ rosbag2_storage::BagMetadata Info::read_metadata(
 
 namespace
 {
-struct client_id_hash
-{
-  static_assert(
-    std::is_same<std::array<uint8_t, 16>,
-    service_msgs::msg::ServiceEventInfo::_client_gid_type>::value);
-  std::size_t operator()(const std::array<uint8_t, 16> & client_id) const
-  {
-    std::hash<uint8_t> hasher;
-    std::size_t seed = 0;
-    for (const auto & value : client_id) {
-      // 0x9e3779b9 is from https://cryptography.fandom.com/wiki/Tiny_Encryption_Algorithm
-      seed ^= hasher(value) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
-    }
-    return seed;
-  }
-};
-
 using client_id = service_msgs::msg::ServiceEventInfo::_client_gid_type;
 using sequence_set = std::unordered_set<int64_t>;
 struct service_req_resp_info