Implement service recording and display info about recorded services

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,19 @@ 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,15 @@ 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:
+        services_event_topics.append(service + '/_service_event')
+
+    return services_event_topics

ros2bag/ros2bag/verb/info.py

@@ -26,11 +26,38 @@ 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 args.topic_name and args.verbose:
+            print("Warning! You have set both the '-t' and '-v' parameters. The '-t' parameter "
+                  'will be ignored.')
+        if args.verbose:
+            Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage)
         else:
-            print(m)
+            m = Info().read_metadata(args.bag_path, args.storage)
+            if args.topic_name:
+                for topic_info in m.topics_with_message_count:
+                    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)

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,35 @@ 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(
+            '--exclude-regex', default='',
+            help='Exclude topics and services containing provided regular expression. '
+                 'Works on top of --all, --all-topics, or --regex.')
+        parser.add_argument(
+            '--exclude-topics', type=str, metavar='Topic', nargs='+',
+            help='List of topics not being recorded. '
+                 'Works on top of --all, --all-topics, or --regex.')
+        parser.add_argument(
+            '--exclude-services', type=str, metavar='ServiceName', nargs='+',
+            help='List of services not being recorded. '
+                 'Works on top of --all, --all-services, or --regex.')
+
+        # Enable to record service
         parser.add_argument(
-            '-x', '--exclude', default='',
-            help='Exclude topics containing provided regular expression. '
-                 'Works on top of --all, --regex, or topics list.')
+            '--services', type=str, metavar='ServiceName', nargs='+',
+            help='List of services to record.')
 
         # Discovery behavior
         parser.add_argument(
@@ -167,20 +188,46 @@ 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):
+        # At least 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('Need to specify 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_regex and
+           not (args.regex or args.all or args.all_topics or args.all_services)):
+            return print_error('--exclude-regex argument requires either --all, '
+                               '--all-topics, --all-services 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 +279,17 @@ 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_regex = args.exclude_regex
+        record_options.exclude_topics = args.exclude_topics if args.exclude_topics else []
+        record_options.exclude_service_events = \
+            convert_service_to_service_event_topic(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 +301,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
@@ -259,6 +260,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

rosbag2_cpp/include/rosbag2_cpp/message_definitions/local_message_definition_source.hpp

@@ -61,18 +61,20 @@ class ROSBAG2_CPP_PUBLIC LocalMessageDefinitionSource final
 public:
   /**
    * Concatenate the message definition with its dependencies into a self-contained schema.
-   * The format is different for MSG and IDL definitions, and is described fully in
+   * The format is different for MSG/SRV and IDL definitions, and is described fully in
    * docs/message_definition_encoding.md
+   * For SRV type, root_type must include a string '/srv/'.
    * Throws DefinitionNotFoundError if one or more definition files are missing for the given
    * package resource name.
    */
-  rosbag2_storage::MessageDefinition get_full_text(const std::string & root_topic_type);
+  rosbag2_storage::MessageDefinition get_full_text(const std::string & root_type);
 
   enum struct Format
   {
     UNKNOWN = 0,
     MSG = 1,
     IDL = 2,
+    SRV = 3,
   };
 
   explicit LocalMessageDefinitionSource() = default;

rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp

@@ -0,0 +1,47 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2_CPP__SERVICE_UTILS_HPP_
+#define ROSBAG2_CPP__SERVICE_UTILS_HPP_
+
+#include <string>
+
+#include "rosbag2_cpp/visibility_control.hpp"
+
+namespace rosbag2_cpp
+{
+ROSBAG2_CPP_PUBLIC
+bool
+is_service_event_topic(const std::string & topic, const std::string & topic_type);
+
+// Call this function after is_service_event_topic() return true
+ROSBAG2_CPP_PUBLIC
+std::string
+service_event_topic_name_to_service_name(const std::string & topic_name);
+
+// Call this function after is_service_event_topic() return true
+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);
+}  // namespace rosbag2_cpp
+
+#endif  // ROSBAG2_CPP__SERVICE_UTILS_HPP_