Implement service recording and display info about recorded services (r…

…os2#1480)

* Implement service record and recorded service info display

Signed-off-by: Barry Xu <[email protected]>

* Address MichaelOrlov's review comments

Signed-off-by: Barry Xu <[email protected]>

* Add tests for rosbag2_cpp::Info::read_service_info()

Signed-off-by: Barry Xu <[email protected]>

* Updated code on exclude parameters

Refer to ros2#1483

Signed-off-by: Barry Xu <[email protected]>

* Fix flake8 errors

Signed-off-by: Barry Xu <[email protected]>

* Update code to remove hard-coded values to make the code more readable

Signed-off-by: Barry Xu <[email protected]>

* Fix a bug on exclude parameter

Signed-off-by: Barry Xu <[email protected]>

* Update codes on rosbag2 QoS for service event topic

Signed-off-by: Barry Xu <[email protected]>

* Address the second round of review comments

Signed-off-by: Barry Xu <[email protected]>

* Updated info test on getting service info

Signed-off-by: Barry Xu <[email protected]>

* Rename exclude_services to exclude_service_event

Signed-off-by: Barry Xu <[email protected]>

* Address flakiness in newly added rosbag2_transport tests

- Got rid form ambient sleep for waiting for messages to be recorded
- Exclude "/rosout" topic from recording

Signed-off-by: Michael Orlov <[email protected]>

* Use wait_for_srvice_to_be_ready() instead check_service_ready()

- Also add default timeout parameter for ClientManager::send_request(..)

Signed-off-by: Michael Orlov <[email protected]>

* Add rosbag2_test_common::wait_until_condition(..)

Signed-off-by: Michael Orlov <[email protected]>

* Rewrite get_service_info tests to be more deterministic

- Also parameterize tests to use default supported storage plugins

Signed-off-by: Michael Orlov <[email protected]>

* Renames in the get_service_info tests

Signed-off-by: Michael Orlov <[email protected]>

* Use `std::filesystem` instead of `rcpputils::fs` functions

- Rationale: Deprecation of the rcpputils::fs in future
See ros2/rcpputils#164 for details

Signed-off-by: Michael Orlov <[email protected]>

* Avoid extra metadata readout in `ros2 bag info` verb with `--verbose`

Signed-off-by: Michael Orlov <[email protected]>

* Add info_end_to_end test with `--verbose` parameter

Signed-off-by: Michael Orlov <[email protected]>

* Fix bugs in info_with_verbose_option_end_to_end_test

Signed-off-by: Barry Xu <[email protected]>

* Fix segment fault issue in test_rosbag2_cpp_get_service_info.cpp

Signed-off-by: Barry Xu <[email protected]>

* Update a fix for info_with_verbose_option_end_to_end_test

Signed-off-by: Barry Xu <[email protected]>

* Output a warning while setting both '-t' and 'v' for info command

Signed-off-by: Barry Xu <[email protected]>

* Address comments from Fujita-san

Signed-off-by: Barry Xu <[email protected]>

* Use topic_in_list instead of service_in_list

Signed-off-by: Michael Orlov <[email protected]>

* Handle cases when dependent service msg definitions could be in IDL file

Signed-off-by: Michael Orlov <[email protected]>

* Fix rebase errors

- Delete "rosbag2_transport/qos.cpp" which was moved to the
rosbag2_storage package during prior PR before rebase

Signed-off-by: Michael Orlov <[email protected]>

* Fix the path of a library

Signed-off-by: Barry Xu <[email protected]>

* Resolve the conflicts caused by the rebase

Signed-off-by: Barry Xu <[email protected]>

* Replace sizeof() with strlen()

Signed-off-by: Barry Xu <[email protected]>

* Use consistent variable name

Signed-off-by: Barry Xu <[email protected]>

* Fix a link error on Windows

Signed-off-by: Barry Xu <[email protected]>

* Use visibility control for format library

Signed-off-by: Barry Xu <[email protected]>

* Replace rosbag2_format_output by direct source files linkage

- Also put `format_service_info(..)` and `format_bag_meta_data(..)`
under the rosbag2_py namespace

Signed-off-by: Michael Orlov <[email protected]>

* Remove header files from pybind modules and delete visibility_control.hpp

Signed-off-by: Michael Orlov <[email protected]>

---------

Signed-off-by: Barry Xu <[email protected]>
Signed-off-by: Michael Orlov <[email protected]>
Co-authored-by: Michael Orlov <[email protected]>

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_