[WIP] Support recording and replay service

docs/message_definition_encoding.md

@@ -16,7 +16,7 @@ This set of definitions with all field types recursively included can be called
 
 ## `ros2msg` encoding
 
-This encoding consists of definitions in [.msg](https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html#message-description-specification) format, concatenated together in human-readable form with
+This encoding consists of definitions in [.msg](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#messages) and [.srv](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#services) format, concatenated together in human-readable form with
 a delimiter.
 
 The top-level message definition is present first, with no delimiter. All dependent .msg definitions are preceded by a two-line delimiter:
@@ -38,6 +38,18 @@ MSG: my_msgs/msg/BasicMsg
 float32 my_float
 ```
 
+Another example is a service message definition for `my_msgs/srv/ExampleSrv` in `ros2msg` form
+```
+# defines a service message that includes a field of a custom message type
+my_msgs/BasicMsg request
+---
+my_msgs/BasicMsg response
+================================================================================
+MSG: my_msgs/msg/BasicMsg
+# defines a message with a primitive type field
+float32 my_float
+```
+
 ## `ros2idl` encoding
 
 The IDL definition of the type specified by name along with all dependent types are stored together. The IDL definitions can be stored in any order. Every definition is preceded by a two-line delimiter:

ros2bag/ros2bag/api/__init__.py

@@ -55,7 +55,15 @@ def _split_lines(self, text, width):
 
 
 def print_error(string: str) -> str:
-    return '[ERROR] [ros2bag]: {}'.format(string)
+    return _print_base('ERROR', string)
+
+
+def print_warn(string: str) -> str:
+    return _print_base('WARN', string)
+
+
+def _print_base(print_type: str, string: str) -> str:
+    return '[{}] [ros2bag]: {}'.format(print_type, string)
 
 
 def dict_to_duration(time_dict: Optional[Dict[str, int]]) -> Duration:
@@ -200,3 +208,16 @@ def add_writer_storage_plugin_extensions(parser: ArgumentParser) -> None:
              'Settings in this profile can still be overridden by other explicit options '
              'and --storage-config-file. Profiles:\n' +
              '\n'.join([f'{preset[0]}: {preset[1]}' for preset in preset_profiles]))
+
+
+def convert_service_to_service_event_topic(services):
+    services_event_topics = []
+
+    if not services:
+        return services_event_topics
+
+    for service in services:
+        name = '/' + service if service[0] != '/' else service
+        services_event_topics.append(name + '/_service_event')
+
+    return services_event_topics

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/info.py

@@ -26,11 +26,35 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '-t', '--topic-name', action='store_true',
             help='Only display topic names.'
         )
+        parser.add_argument(
+            '-v', '--verbose', action='store_true',
+            help='Display request/response information for services'
+        )
+
+    def _is_service_event_topic(self, topic_name, topic_type) -> bool:
+
+        service_event_type_middle = '/srv/'
+        service_event_type_postfix = '_Event'
+
+        if (service_event_type_middle not in topic_type
+                or not topic_type.endswith(service_event_type_postfix)):
+            return False
+
+        service_event_topic_postfix = '/_service_event'
+        if not topic_name.endswith(service_event_topic_postfix):
+            return False
+
+        return True
 
     def main(self, *, args):  # noqa: D102
         m = Info().read_metadata(args.bag_path, args.storage)
         if args.topic_name:
             for topic_info in m.topics_with_message_count:
-                print(topic_info.topic_metadata.name)
+                if not self._is_service_event_topic(topic_info.topic_metadata.name,
+                                                    topic_info.topic_metadata.type):
+                    print(topic_info.topic_metadata.name)
         else:
-            print(m)
+            if args.verbose:
+                Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage)
+            else:
+                print(m)

ros2bag/ros2bag/verb/play.py

@@ -18,8 +18,10 @@
 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.api import print_warn
 from ros2bag.verb import VerbExtension
 from ros2cli.node import NODE_NAME_PREFIX
 from rosbag2_py import Player
@@ -51,14 +53,26 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '--topics', type=str, default=[], nargs='+',
             help='Space-delimited list of topics to play.')
+        parser.add_argument(
+            '--services', type=str, default=[], 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.')
         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. This argument is deprecated and please '
+                 'use --exclude-topics.')
+        parser.add_argument(
+            '--exclude-topics', default='',
             help='regular expressions to exclude topics from replay, separated by space. If none '
                  'specified, all topics will be replayed.')
+        parser.add_argument(
+            '--exclude-services', default='',
+            help='regular expressions to exclude services from replay, separated by space. If '
+                 'none 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.')
@@ -163,6 +177,10 @@ def main(self, *, args):  # noqa: D102
             except (InvalidQoSProfileException, ValueError) as e:
                 return print_error(str(e))
 
+        if args.exclude and args.exclude_topics:
+            return print_error(str('-x/--exclude and --exclude_topics cannot be used at the '
+                                   'same time.'))
+
         storage_config_file = ''
         if args.storage_config_file:
             storage_config_file = args.storage_config_file.name
@@ -182,8 +200,23 @@ 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
+
+        if args.exclude:
+            print(print_warn(str('-x/--exclude argument is deprecated. Please use '
+                                 '--exclude-topics.')))
+            play_options.topics_regex_to_exclude = args.exclude
+        else:
+            play_options.topics_regex_to_exclude = args.exclude_topics
+
+        if args.exclude_services:
+            play_options.services_regex_to_exclude = args.exclude_services + '/_service_event'
+        else:
+            play_options.services_regex_to_exclude = args.exclude_services
         play_options.topic_qos_profile_overrides = qos_profile_overrides
         play_options.loop = args.loop
         play_options.topic_remapping_options = topic_remapping

ros2bag/ros2bag/verb/record.py

@@ -18,6 +18,7 @@
 
 from rclpy.qos import InvalidQoSProfileException
 from ros2bag.api import add_writer_storage_plugin_extensions
+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.api import SplitLineFormatter
@@ -65,15 +66,31 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             'topics', nargs='*', default=None, help='List of topics to record.')
         parser.add_argument(
             '-a', '--all', action='store_true',
-            help='Record all topics. Required if no explicit topic list or regex filters.')
+            help='Record all topics and services (Exclude hidden topic).')
+        parser.add_argument(
+            '--all-topics', action='store_true',
+            help='Record all topics (Exclude hidden topic).')
+        parser.add_argument(
+            '--all-services', action='store_true',
+            help='Record all services via service event topics.')
         parser.add_argument(
             '-e', '--regex', default='',
-            help='Record only topics containing provided regular expression. '
-                 'Overrides --all, applies on top of topics list.')
+            help='Record only topics and services containing provided regular expression. '
+                 'Overrides --all, --all-topics and --all-services, applies on top of '
+                 'topics list and service list.')
         parser.add_argument(
-            '-x', '--exclude', default='',
+            '--exclude-topics', default='',
             help='Exclude topics containing provided regular expression. '
-                 'Works on top of --all, --regex, or topics list.')
+                 'Works on top of --all, --all-topics, or --regex.')
+        parser.add_argument(
+            '--exclude-services', default='',
+            help='Exclude services containing provided regular expression. '
+                 'Works on top of --all, --all-services, or --regex.')
+
+        # Enable to record service
+        parser.add_argument(
+            '--services', type=str, metavar='ServiceName', nargs='+',
+            help='List of services to record.')
 
         # Discovery behavior
         parser.add_argument(
@@ -167,20 +184,41 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             help='Choose the compression format/algorithm. '
                  'Has no effect if no compression mode is chosen. Default: %(default)s.')
 
+    def _check_necessary_argument(self, args):
+        # One options out of --all, --all-topics, --all-services, --services, topics or --regex
+        # must be used
+        if not (args.all or args.all_topics or args.all_services or
+           args.services or (args.topics and len(args.topics) > 0) or args.regex):
+            return False
+        return True
+
     def main(self, *, args):  # noqa: D102
-        # both all and topics cannot be true
-        if (args.all and (args.topics or args.regex)) or (args.topics and args.regex):
-            return print_error('Must specify only one option out of topics, --regex or --all')
-        # one out of "all", "topics" and "regex" must be true
-        if not(args.all or (args.topics and len(args.topics) > 0) or (args.regex)):
-            return print_error('Invalid choice: Must specify topic(s), --regex or --all')
 
-        if args.topics and args.exclude:
-            return print_error('--exclude argument cannot be used when specifying a list '
-                               'of topics explicitly')
+        if not self._check_necessary_argument(args):
+            return print_error('Must specify only one option out of --all, --all-topics, '
+                               '--all-services, --services, topics and --regex')
+
+        # Only one option out of --all, --all-services --services or --regex can be used
+        if (args.all and args.all_services) or \
+           ((args.all or args.all_services) and args.regex) or \
+           ((args.all or args.all_services or args.regex) and args.services):
+            return print_error('Must specify only one option out of --all, --all-services, '
+                               '--services or --regex')
+
+        # Only one option out of --all, --all-topics, topics or --regex can be used
+        if (args.all and args.all_topics) or \
+           ((args.all or args.all_topics) and args.regex) or \
+           ((args.all or args.all_topics or args.regex) and args.topics):
+            return print_error('Must specify only one option out of --all, --all-topics, '
+                               'topics or --regex')
 
-        if args.exclude and not(args.regex or args.all):
-            return print_error('--exclude argument requires either --all or --regex')
+        if args.exclude_topics and not (args.regex or args.all or args.all_topics):
+            return print_error('--exclude-topics argument requires either --all, --all-topics '
+                               'or --regex')
+
+        if args.exclude_services and not (args.regex or args.all or args.all_services):
+            return print_error('--exclude-services argument requires either --all, --all-services '
+                               'or --regex')
 
         uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S')
 
@@ -232,14 +270,15 @@ def main(self, *, args):  # noqa: D102
             custom_data=custom_data
         )
         record_options = RecordOptions()
-        record_options.all = args.all
+        record_options.all_topics = args.all_topics or args.all
         record_options.is_discovery_disabled = args.no_discovery
         record_options.topics = args.topics
         record_options.rmw_serialization_format = args.serialization_format
         record_options.topic_polling_interval = datetime.timedelta(
             milliseconds=args.polling_interval)
         record_options.regex = args.regex
-        record_options.exclude = args.exclude
+        record_options.exclude_topics = args.exclude_topics
+        record_options.exclude_services = args.exclude_services
         record_options.node_prefix = NODE_NAME_PREFIX
         record_options.compression_mode = args.compression_mode
         record_options.compression_format = args.compression_format
@@ -251,6 +290,10 @@ def main(self, *, args):  # noqa: D102
         record_options.start_paused = args.start_paused
         record_options.ignore_leaf_topics = args.ignore_leaf_topics
         record_options.use_sim_time = args.use_sim_time
+        record_options.all_services = args.all_services or args.all
+
+        # Convert service name to service event topic name
+        record_options.services = convert_service_to_service_event_topic(args.services)
 
         recorder = Recorder()
 

rosbag2_cpp/CMakeLists.txt

@@ -72,7 +72,8 @@ add_library(${PROJECT_NAME} SHARED
   src/rosbag2_cpp/types/introspection_message.cpp
   src/rosbag2_cpp/writer.cpp
   src/rosbag2_cpp/writers/sequential_writer.cpp
-  src/rosbag2_cpp/reindexer.cpp)
+  src/rosbag2_cpp/reindexer.cpp
+  src/rosbag2_cpp/service_utils.cpp)
 
 target_link_libraries(${PROJECT_NAME}
   PUBLIC
@@ -257,6 +258,12 @@ if(BUILD_TESTING)
   if(TARGET test_time_controller_clock)
     target_link_libraries(test_time_controller_clock ${PROJECT_NAME})
   endif()
+
+  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})
+  endif()
 endif()
 
 ament_package()

rosbag2_cpp/include/rosbag2_cpp/info.hpp

@@ -15,7 +15,9 @@
 #ifndef ROSBAG2_CPP__INFO_HPP_
 #define ROSBAG2_CPP__INFO_HPP_
 
+#include <memory>
 #include <string>
+#include <vector>
 
 #include "rosbag2_cpp/visibility_control.hpp"
 
@@ -24,13 +26,25 @@
 namespace rosbag2_cpp
 {
 
+typedef ROSBAG2_CPP_PUBLIC_TYPE struct rosbag2_service_info_t
+{
+  std::string name;
+  std::string type;
+  std::string serialization_format;
+  size_t request_count;
+  size_t response_count;
+} rosbag2_service_info_t;
+
 class ROSBAG2_CPP_PUBLIC Info
 {
 public:
   virtual ~Info() = default;
 
   virtual rosbag2_storage::BagMetadata read_metadata(
     const std::string & uri, const std::string & storage_id = "");
+
+  virtual std::vector<std::shared_ptr<rosbag2_service_info_t>> read_service_info(
+    const std::string & uri, const std::string & storage_id = "");
 };
 
 }  // namespace rosbag2_cpp